Refactorizations

hexapod_python/ik.py

@@ -2,16 +2,15 @@ import math
 
 from geometry_msgs.msg import Point, Pose, Quaternion
 from trajectory_msgs.msg import JointTrajectoryPoint
-from hexapod_robot.msg import HexapodPose, HexapodLeg
 
 
 leg_offsets = {    
-    'e1': Point(x = -40.0, y =   0.0, z = 25.0),
+    'e1': Point(x = -40.0, y =  80.0, z = 25.0),
     'e2': Point(x = -60.0, y =   0.0, z = 25.0),
     'e3': Point(x = -40.0, y = -80.0, z = 25.0),
-    'e4': Point(x =  40.0, y =  80.0, z = 25.0),
+    'e4': Point(x =  40.0, y = -80.0, z = 25.0),
     'e5': Point(x =  60.0, y =   0.0, z = 25.0),
-    'e6': Point(x =  40.0, y = -80.0, z = 25.0)
+    'e6': Point(x =  40.0, y =  80.0, z = 25.0)
 }
 
 leg_sides = {
@@ -23,106 +22,6 @@ leg_sides = {
     'e6': 'r',    
 }
 
-standing_pose_ = HexapodPose(
-    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 = 1.0)
-    ),
-    legs = [
-        HexapodLeg(label="e1", position=Point(x = leg_offsets['e1'].x + -100.0, y = leg_offsets['e1'].y + 0.0, z =leg_offsets['e1'].z + -130.0)),
-    ]
-
-)
-
-standing_pose = {
-    'body': Pose(
-        position = Point(x = 0.0, y = 0.0, z = 105.0 ),
-        orientation = Quaternion(x = 0.0, y = 0.0, z = 0.0, w = 1.0)
-    ),
-    'legs': {
-        'e1': Point(x = leg_offsets['e1'].x + -100.0, y = leg_offsets['e1'].y, z = 0.0),
-        'e2': Point(x = leg_offsets['e2'].x + -100.0, y = leg_offsets['e2'].y, z = 0.0),
-        'e3': Point(x = leg_offsets['e3'].x + -100.0, y = leg_offsets['e3'].y, z = 0.0),
-        'e4': Point(x = leg_offsets['e4'].x +  100.0, y = leg_offsets['e4'].y, z = 0.0),
-        'e5': Point(x = leg_offsets['e5'].x +  100.0, y = leg_offsets['e5'].y, z = 0.0),
-        'e6': Point(x = leg_offsets['e6'].x +  100.0, y = leg_offsets['e6'].y, z = 0.0)
-    }
-}
-
-shutdown_pose = {
-    'body': Pose(
-        position = Point(x = 0.0, y = 0.0, z = -5.0 ),
-        orientation = Quaternion(x = 0.0, y = 0.0, z = 0.0, w = 1.0)
-    ),
-    'legs': {
-        'e1': Point(x = leg_offsets['e1'].x + -140.0, y = leg_offsets['e1'].y, z = 0.0),
-        'e2': Point(x = leg_offsets['e2'].x + -140.0, y = leg_offsets['e2'].y, z = 0.0),
-        'e3': Point(x = leg_offsets['e3'].x + -140.0, y = leg_offsets['e3'].y, z = 0.0),
-        'e4': Point(x = leg_offsets['e4'].x +  140.0, y = leg_offsets['e4'].y, z = 0.0),
-        'e5': Point(x = leg_offsets['e5'].x +  140.0, y = leg_offsets['e5'].y, z = 0.0),
-        'e6': Point(x = leg_offsets['e6'].x +  140.0, y = leg_offsets['e6'].y, z = 0.0),
-    }
-}
-
-calibration_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 = 1.0)
-    ),
-    'legs': {
-        'e1': Point(x = -200.0, y= 150.0, z=0.0),
-        'e2': Point(x = -200.0, y=   0.0, z=0.0),
-        'e3': Point(x = -200.0, y=-150.0, z=0.0),
-        'e4': Point(x =  200.0, y= 150.0, z=0.0),
-        'e5': Point(x =  200.0, y=   0.0, z=0.0),
-        'e6': Point(x =  200.0, y=-150.0, z=0.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 = 1.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
@@ -139,9 +38,6 @@ def ik(body_pose):
         )
     
     return joint_values
-    
-
-
 
 def leg_ik(side, point):
 
@@ -172,6 +68,7 @@ def leg_ik(side, point):
     tibia_angle = math.acos((tt + ff - dd) / (2 * t * f))                   - math.pi/2
 
     if(side == 'l'):
+        coxa_angle *= -1
         femur_angle *= -1
     else:
         tibia_angle *= -1

hexapod_python/ik_calibration.py

@@ -4,58 +4,22 @@ from builtin_interfaces.msg import Duration
 import rclpy
 from rclpy.node import Node
 
-from hexapod_python.ik import ik, interpolate_pose, calibration_pose, shutdown_pose
+from hexapod_python.ik_publisher_node import IKPublisherNode
+from hexapod_python.ik import ik
+from hexapod_python.ik_poses import calibration_pose
 
-import time
+class IKCalibrationNode(IKPublisherNode):
-
+    def __init__(self):
-
+            super().__init__('ik_calibration')
-class IKPublisherNode(Node):
+            ik_values = [ ik(calibration_pose)]
-    def __init__(self, node_name = "ik_publisher"):
+            self.publish_ik_values(ik_values)
-        super().__init__(node_name)
+            raise SystemExit
-
-        self.leg_groups_ = [ "e1", "e2", "e3", "e4", "e5", "e6"]
-        self.create_leg_publishers()
-        ik_values = [ ik(calibration_pose)]
-
-    def create_leg_publishers(self):
-        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 leg in self.leg_groups_:
-            while self.count_subscribers(f"/{leg}_group_controller/joint_trajectory") == 0:                    
-                time.sleep(0.1)
-
-
-    def publish_ik_values(self, 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
-
-
-            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)
 
-    ik_publisher = IKPublisher()
+    ik_publisher = IKCalibrationNode()
-    rclpy.spin(ik_publisher)
+    rclpy.spin_once(ik_publisher)
     ik_publisher.destroy_node()
     rclpy.shutdown()
     

hexapod_python/ik_poses.py

@@ -0,0 +1,91 @@
+from geometry_msgs.msg import Point, Pose, Quaternion
+from hexapod_python.ik import leg_offsets
+
+standing_pose = {
+    'body': Pose(
+        position = Point(x = 0.0, y = 0.0, z = 105.0 ),
+        orientation = Quaternion(x = 0.0, y = 0.0, z = 0.0, w = 1.0)
+    ),
+    'legs': {
+        'e1': Point(x = leg_offsets['e1'].x + -100.0, y = leg_offsets['e1'].y, z = 0.0),
+        'e2': Point(x = leg_offsets['e2'].x + -100.0, y = leg_offsets['e2'].y, z = 0.0),
+        'e3': Point(x = leg_offsets['e3'].x + -100.0, y = leg_offsets['e3'].y, z = 0.0),
+        'e4': Point(x = leg_offsets['e4'].x +  100.0, y = leg_offsets['e4'].y, z = 0.0),
+        'e5': Point(x = leg_offsets['e5'].x +  100.0, y = leg_offsets['e5'].y, z = 0.0),
+        'e6': Point(x = leg_offsets['e6'].x +  100.0, y = leg_offsets['e6'].y, z = 0.0)
+    }
+}
+
+shutdown_pose = {
+    'body': Pose(
+        position = Point(x = 0.0, y = 0.0, z = -5.0 ),
+        orientation = Quaternion(x = 0.0, y = 0.0, z = 0.0, w = 1.0)
+    ),
+    'legs': {
+        'e1': Point(x = leg_offsets['e1'].x + -140.0, y = leg_offsets['e1'].y, z = 0.0),
+        'e2': Point(x = leg_offsets['e2'].x + -140.0, y = leg_offsets['e2'].y, z = 0.0),
+        'e3': Point(x = leg_offsets['e3'].x + -140.0, y = leg_offsets['e3'].y, z = 0.0),
+        'e4': Point(x = leg_offsets['e4'].x +  140.0, y = leg_offsets['e4'].y, z = 0.0),
+        'e5': Point(x = leg_offsets['e5'].x +  140.0, y = leg_offsets['e5'].y, z = 0.0),
+        'e6': Point(x = leg_offsets['e6'].x +  140.0, y = leg_offsets['e6'].y, z = 0.0),
+    }
+}
+
+calibration_pose = {
+    'body': Pose(
+        position = Point(x = 0.0, y = 0.0, z = -10.0 ),
+        orientation = Quaternion(x = 0.0, y = 0.0, z = 0.0, w = 1.0)
+    ),
+    'legs': {
+        'e1': Point(x = -200.0, y =  150.0, z = 0.0),
+        'e2': Point(x = -200.0, y =    0.0, z = 0.0),
+        'e3': Point(x = -200.0, y = -150.0, z = 0.0),         
+
+        'e4': Point(x =  200.0, y = -150.0, z = 0.0),
+        'e5': Point(x =  200.0, y =    0.0, z = 0.0),
+        'e6': Point(x =  200.0, y =  150.0, z = 0.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 = 1.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

hexapod_python/ik_test.py

@@ -2,7 +2,8 @@ import rclpy
 from rclpy.node import Node
 
 from hexapod_python.ik_publisher_node import IKPublisherNode
-from hexapod_python.ik import ik, interpolate_pose, standing_pose, shutdown_pose
+from hexapod_python.ik import ik
+from hexapod_python.ik_poses import interpolate_pose, standing_pose, shutdown_pose
 
 import time
 
@@ -10,7 +11,11 @@ import time
 class IKTestNode(IKPublisherNode):
     def __init__(self):
             super().__init__('ik_test')
-            ik_values = [ ik(pose) for pose in interpolate_pose(shutdown_pose, standing_pose, 150) + interpolate_pose(standing_pose, shutdown_pose, 150)]
+            ik_values = [ 
+                 ik(pose) for pose in 
+                 interpolate_pose(shutdown_pose, standing_pose, 150) + 
+                 interpolate_pose(standing_pose, shutdown_pose, 150)
+            ]
             self.publish_ik_values(ik_values)
             raise SystemExit