Fiddling with poses and data types

hexapod_python/ik.py

@@ -2,15 +2,16 @@ 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 = 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)
+    'e1': Point(x = -40.0, y =   0.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),
+    'e5': Point(x =  60.0, y =   0.0, z = 25.0),
+    'e6': Point(x =  40.0, y = -80.0, z = 25.0)
 }
 
 leg_sides = {
@@ -22,41 +23,71 @@ 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 = 0.0 ),
-        orientation = Quaternion(x = 0.0, y = 0.0, z = 0.0, w = 0.0)
+        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 + 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),
+        '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 = 0.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 = 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),
+        '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)
+    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 = {}
@@ -69,6 +100,7 @@ def interpolate_pose(start, end, steps):
 
     poses = []
     for i in range(0, steps):
+        print(i)
         pose = {
             'body': Pose(
                 position = Point(
@@ -76,7 +108,7 @@ def interpolate_pose(start, end, steps):
                     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)
+                orientation = Quaternion(x = 0.0, y = 0.0, z = 0.0, w = 1.0)
             ),
             'legs': {}            
         }
@@ -103,9 +135,9 @@ def ik(body_pose):
         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,
+                x = body_pose['legs'][l].x - leg_offsets[l].x - body_pose['body'].position.x,
+                y = body_pose['legs'][l].y - leg_offsets[l].y - body_pose['body'].position.y,
+                z = body_pose['legs'][l].z - leg_offsets[l].z - body_pose['body'].position.z,
             )            
         )
     
@@ -163,4 +195,9 @@ if __name__ == "__main__":
     print(ik(standing_pose))
     print(ik(shutdown_pose))
 
-    print(interpolate_pose(standing_pose, shutdown_pose, 10))
+    
+    poses = interpolate_pose(shutdown_pose, standing_pose , 10)
+    for p in poses:
+        print(p)
+        print(ik(p))
+        print()