changed poses to not draw marks on the table =)

hexapod_python/ik_calibration.py

@@ -14,7 +14,16 @@ class IKCalibrationNode(IKPublisherNode):
     def __init__(self):
             super().__init__('ik_calibration')
             self.modified_calibration_pose = copy.deepcopy(calibration_pose)
+            for leg in self.modified_calibration_pose['legs']:
+                 self.modified_calibration_pose['legs'][leg].z += 20
+            
+            self.calibration_leg='e6'
+            
+            [ self.ik_values_reference ] = [ ik(calibration_pose) ]            
             ik_values = [ ik(self.modified_calibration_pose)]
+
+
+
             self.publish_ik_values(ik_values)
 
             self.cmd_vel_subscription = self.create_subscription(
@@ -25,12 +34,22 @@ class IKCalibrationNode(IKPublisherNode):
             )
     
     def twist_callback(self, msg):
-        print(msg)
-        self.modified_calibration_pose['legs']['e1'].x -= msg.linear.x
-        self.modified_calibration_pose['legs']['e1'].y -= msg.linear.y
-        self.modified_calibration_pose['legs']['e1'].z -= msg.linear.z
-        ik_values = [ ik(self.modified_calibration_pose)]
-        self.publish_ik_values(ik_values)
+        self.modified_calibration_pose['legs'][self.calibration_leg].x -= msg.linear.x
+        self.modified_calibration_pose['legs'][self.calibration_leg].y -= msg.linear.y
+        self.modified_calibration_pose['legs'][self.calibration_leg].z -= msg.linear.z
+        ik_values_list = [ ik(self.modified_calibration_pose)]
+        [ ik_values ] = ik_values_list
+        
+        im = ik_values[self.calibration_leg]
+        ir = self.ik_values_reference[self.calibration_leg]
+
+        print(im)
+        print(ir)
+        print([(m - r) for (m,r) in zip(im, ir)])
+        print([(m - r) * 129 for (m,r) in zip(im, ir)])
+        print()
+
+        self.publish_ik_values(ik_values_list)
 
 
 def main(args = None):

hexapod_python/ik_poses.py

@@ -16,6 +16,23 @@ standing_pose = {
     }
 }
 
+stand_up_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 + -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)
+    }
+}
+
+
+
 shutdown_pose = {
     'body': Pose(
         position = Point(x = 0.0, y = 0.0, z = -5.0 ),
@@ -33,7 +50,7 @@ shutdown_pose = {
 
 calibration_pose = {
     'body': Pose(
-        position = Point(x = 0.0, y = 0.0, z = -10.0 ),
+        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': {

hexapod_python/ik_publisher_node.py

@@ -20,6 +20,8 @@ class IKPublisherNode(Node):
             controller_topic = f"/{leg}_group_controller/joint_trajectory"
             self.publisher_[leg] = self.create_publisher(JointTrajectory, controller_topic , 10)
 
+        time.sleep(1)
+
         # wait for the controller to subscribe
         print("Waiting for subscribers")
         for leg in self.leg_groups_:

hexapod_python/ik_test.py

@@ -3,7 +3,7 @@ from rclpy.node import Node
 
 from hexapod_python.ik_publisher_node import IKPublisherNode
 from hexapod_python.ik import ik
-from hexapod_python.ik_poses import interpolate_pose, standing_pose, shutdown_pose
+from hexapod_python.ik_poses import interpolate_pose, standing_pose, stand_up_pose, shutdown_pose
 
 import time
 
@@ -11,11 +11,10 @@ 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, stand_up_pose, 50)]
+            self.publish_ik_values(ik_values)
+            time.sleep(5)
+            ik_values = [ik(pose) for pose in interpolate_pose(stand_up_pose, shutdown_pose, 50)]
             self.publish_ik_values(ik_values)
             raise SystemExit