demo

hexapod_python/ik.py

@@ -0,0 +1,166 @@
+import math
+
+from geometry_msgs.msg import Point, Pose, Quaternion
+from trajectory_msgs.msg import JointTrajectoryPoint
+
+
+leg_offsets = {    
+    'e1': Point(x = -40.0, y =   0.0, z = 0.0),
+    'e2': Point(x = -60.0, y =   0.0, z = 0.0),
+    'e3': Point(x = -40.0, y = -80.0, z = 0.0),
+    'e4': Point(x =  40.0, y =  80.0, z = 0.0),
+    'e5': Point(x =  60.0, y =   0.0, z = 0.0),
+    'e6': Point(x =  40.0, y = -80.0, z = 0.0)
+}
+
+leg_sides = {
+    'e1': 'l',
+    'e2': 'l',
+    'e3': 'l',
+    'e4': 'r',
+    'e5': 'r',
+    'e6': 'r',    
+}
+
+standing_pose = {
+    'body': Pose(
+        position = Point(x = 0.0, y = 0.0, z = 0.0 ),
+        orientation = Quaternion(x = 0.0, y = 0.0, z = 0.0, w = 0.0)
+    ),
+    'legs': {
+        'e1': Point(x = leg_offsets['e1'].x + -100.0, y = leg_offsets['e1'].y + 0.0, z =leg_offsets['e1'].z + -130.0),
+        'e2': Point(x = leg_offsets['e2'].x + -100.0, y = leg_offsets['e2'].y + 0.0, z =leg_offsets['e2'].z + -130.0),
+        'e3': Point(x = leg_offsets['e3'].x + -100.0, y = leg_offsets['e3'].y + 0.0, z =leg_offsets['e3'].z + -130.0),
+        'e4': Point(x = leg_offsets['e4'].x +  100.0, y = leg_offsets['e4'].y + 0.0, z =leg_offsets['e4'].z + -130.0),
+        'e5': Point(x = leg_offsets['e5'].x +  100.0, y = leg_offsets['e5'].y + 0.0, z =leg_offsets['e5'].z + -130.0),
+        'e6': Point(x = leg_offsets['e6'].x +  100.0, y = leg_offsets['e6'].y + 0.0, z =leg_offsets['e6'].z + -130.0),
+    }
+}
+
+shutdown_pose = {
+    'body': Pose(
+        position = Point(x = 0.0, y = 0.0, z = 0.0 ),
+        orientation = Quaternion(x = 0.0, y = 0.0, z = 0.0, w = 0.0)
+    ),
+    'legs': {
+        'e1': Point(x = leg_offsets['e1'].x + -140.0, y = leg_offsets['e1'].y, z = 8.0),
+        'e2': Point(x = leg_offsets['e2'].x + -140.0, y = leg_offsets['e2'].y, z = 8.0),
+        'e3': Point(x = leg_offsets['e3'].x + -140.0, y = leg_offsets['e3'].y, z = 8.0),
+        'e4': Point(x = leg_offsets['e4'].x +  140.0, y = leg_offsets['e4'].y, z = 8.0),
+        'e5': Point(x = leg_offsets['e5'].x +  140.0, y = leg_offsets['e5'].y, z = 8.0),
+        'e6': Point(x = leg_offsets['e6'].x +  140.0, y = leg_offsets['e6'].y, z = 8.0),
+    }
+}
+
+def interpolate_pose(start, end, steps):
+
+    body_step_x = end['body'].position.x - start['body'].position.x / (steps-1)
+    body_step_y = end['body'].position.y - start['body'].position.y / (steps-1)
+    body_step_z = end['body'].position.z - start['body'].position.z / (steps-1)
+
+    leg_step_x = {}
+    leg_step_y = {}
+    leg_step_z = {}
+
+    for leg in start['legs']:
+        leg_step_x[leg] = (end['legs'][leg].x - start['legs'][leg].x) / (steps-1)
+        leg_step_y[leg] = (end['legs'][leg].y - start['legs'][leg].y) / (steps-1)
+        leg_step_z[leg] = (end['legs'][leg].z - start['legs'][leg].z) / (steps-1)
+
+    poses = []
+    for i in range(0, steps):
+        pose = {
+            'body': Pose(
+                position = Point(
+                    x = start['body'].position.x + body_step_x * i, 
+                    y = start['body'].position.y + body_step_y * i, 
+                    z = start['body'].position.z + body_step_z * i
+                ),
+                orientation = Quaternion(x = 0.0, y = 0.0, z = 0.0, w = 0.0)
+            ),
+            'legs': {}            
+        }
+
+        for leg in start['legs']:
+            pose['legs'][leg] = Point(
+                x = start['legs'][leg].x + leg_step_x[leg] * i,
+                y = start['legs'][leg].y + leg_step_y[leg] * i,
+                z = start['legs'][leg].z + leg_step_z[leg] * i
+            )
+
+        poses.append(pose)
+
+    return poses
+
+
+
+
+def ik(body_pose):
+    # todo apply rotation and translation of body
+
+    joint_values = {}
+    for l in body_pose['legs']:
+        joint_values[l] = leg_ik(
+            leg_sides[l], 
+            Point(
+                x = body_pose['legs'][l].x - leg_offsets[l].x,
+                y = body_pose['legs'][l].y - leg_offsets[l].y,
+                z = body_pose['legs'][l].z - leg_offsets[l].z,
+            )            
+        )
+    
+    return joint_values
+    
+
+
+
+def leg_ik(side, point):
+
+    x = point.x
+    y = point.y
+    z = point.z
+
+    c = 20
+    f = 80
+    t = 130
+    
+    ff = f*f
+    tt = t*t
+
+    if side == 'l':
+        x = -x
+
+    coxa_angle = math.atan2(y,x)
+    
+    r = math.sqrt(x*x + y * y) - c
+
+    rr = r*r
+    zz = z*z
+    dd = rr + zz
+    d  = math.sqrt(dd)
+    
+    femur_angle = math.acos((ff + dd - tt) / (2 * f * d)) + math.acos(-z/d) - math.pi/2
+    tibia_angle = math.acos((tt + ff - dd) / (2 * t * f))                   - math.pi/2
+
+    if(side == 'l'):
+        femur_angle *= -1
+    else:
+        tibia_angle *= -1
+
+
+    return([coxa_angle, femur_angle, tibia_angle])
+
+if __name__ == "__main__":
+    print(leg_ik('l', Point(x = -100.0, y = 0.0, z = -130.0)))
+    print(leg_ik('r', Point(x =  100.0, y = 0.0, z = -130.0)))
+    print(leg_ik('l', Point(x = -120.0, y = 0.0, z = -130.0)))
+    print(leg_ik('r', Point(x =  120.0, y = 0.0, z = -130.0)))
+
+    for o in leg_offsets:
+        print(leg_offsets[o])
+
+
+    print(ik(standing_pose))
+    print(ik(shutdown_pose))
+
+    print(interpolate_pose(standing_pose, shutdown_pose, 10))

hexapod_python/ik_test.py

@@ -0,0 +1,60 @@
+from geometry_msgs.msg import Point, Pose
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+from builtin_interfaces.msg import Duration
+import rclpy
+from rclpy.node import Node
+
+from hexapod_python.ik import ik, interpolate_pose, standing_pose, shutdown_pose
+
+import time
+
+
+class JTCPublisher(Node):
+    def __init__(self):
+            super().__init__('jtc_publisher')
+
+            self.leg_groups_ = [ "e1", "e2", "e3", "e4", "e5", "e6"]
+            self.publisher_ = {}
+
+            for leg in self.leg_groups_:            
+                controller_topic = f"/{leg}_group_controller/joint_trajectory"
+                self.publisher_[leg] = self.create_publisher(JointTrajectory, controller_topic , 10)
+
+
+            # wait for the controller to subscribe
+            print("Waiting for subscribers")
+            for tag in self.leg_groups_:
+                while self.count_subscribers(f"/{leg}_group_controller/joint_trajectory") == 0:                    
+                    time.sleep(0.1)
+
+            ik_values = [ ik(pose) for pose in interpolate_pose(shutdown_pose, standing_pose, 150) + interpolate_pose(standing_pose, shutdown_pose, 150)]
+            print(ik_values)
+            for leg in self.leg_groups_:
+                t = 0
+                joint_trajectory_points = []
+                for i in ik_values:
+                    joint_trajectory_points.append(
+                        JointTrajectoryPoint(
+                            positions = i[leg],
+                            time_from_start = Duration(nanosec = t%50 * 20000000 , sec = int(t/50))
+                        )
+                    )
+                    t += 1
+
+
+                print(joint_trajectory_points)
+                group_num = leg[1:]
+                trajectory = JointTrajectory(
+                    joint_names = [ f"c{group_num}_joint", f"f{group_num}_joint", f"t{group_num}_joint" ],
+                    points = joint_trajectory_points
+                )
+                self.publisher_[leg].publish(trajectory)
+
+def main(args = None):
+    rclpy.init(args=args)
+
+    jtc_publisher = JTCPublisher()
+    rclpy.spin(jtc_publisher)
+    jtc_publisher.destroy_node()
+    rclpy.shutdown()
+    

package.xml

@@ -10,6 +10,7 @@
   <exec_depend>rclpy</exec_depend>
   <exec_depend>std_msgs</exec_depend>
   <exec_depend>trajectory_msgs</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

setup.py

@@ -20,7 +20,8 @@ setup(
     tests_require=['pytest'],
     entry_points={
         'console_scripts': [
-            'jtc_test = hexapod_python.jtc_test:main'
+            'jtc_test = hexapod_python.jtc_test:main',
+            'ik_test = hexapod_python.ik_test:main'
         ],
     },
 )