swapping Z, position legs now working

src/stompy_msgs/CMakeLists.txt

@@ -50,6 +50,7 @@ find_package(catkin REQUIRED COMPONENTS
 add_message_files(
   FILES
   LegPlan.msg
+  LegState.msg
 )
 
 ## Generate services in the 'srv' folder

src/stompy_msgs/msg/LegPlan.msg

@@ -1,5 +1,5 @@
-std_msgs/UInt8 mode
-std_msgs/UInt8 frame
-std_msgs/Time start_time
-std_msgs/Float64 speed
+uint8 mode
+uint8 frame
+time start_time
+float64 speed
 geometry_msgs/Twist target

src/stompy_msgs/msg/LegState.msg

@@ -0,0 +1,3 @@
+time time
+float64 load
+geometry_msgs/Point position

src/stompy_ros/python/stompy_ros/head/legs.py

@@ -0,0 +1,17 @@
+#!/usr/bin/env python
+
+
+global legs
+legs = {}
+
+
+def new_leg_message(name, msg):
+    global legs
+    legs[name] = {
+        # TODO msg type with time
+        'time': msg.time.to_sec(),
+        'load': float(msg.load),
+        'x': float(msg.position.x),
+        'y': float(msg.position.y),
+        'z': float(msg.position.z),
+    }

src/stompy_ros/python/stompy_ros/head/modes.py

@@ -37,6 +37,7 @@
 from .. import info
 from .. import kinematics
 from .. import leg
+from . import legs
 
 
 # foot/leg modes
@@ -116,7 +117,7 @@ def get_mode_from_buttons(buttons):
 class Mode(object):
     mode = None
 
-    def __init__(self, msg):
+    def __init__(self, msg=None):
         pass
 
     def exit(self):
@@ -147,20 +148,25 @@ class Idle(Mode):
 class MoveLeg(Mode):
     mode = MOVE_LEG
 
-    def __init__(self, msg):
+    def __init__(self, msg=None):
         # TODO get move parameters from server
         self.scale_angles = 1 / 100.
         self.scale_legs = 1 / 200.
         self.scale_body = 1 / 200.
-        inds = get_pressed_button_indices(msg.buttons)
-        self.leg = None
-        for i in inds:
-            if i in leg_buttons:
-                self.leg = leg_buttons[i]
-        if self.leg is None:
-            raise ValueError(
-                "Invalid mode transition, unknown leg: %s" % (msg.buttons,))
-        self.frame = 'body'
+        if msg is None:
+            self.leg = 'fr'
+            self.frame = 'body'
+        else:
+            inds = get_pressed_button_indices(msg.buttons)
+            self.leg = None
+            for i in inds:
+                if i in leg_buttons:
+                    self.leg = leg_buttons[i]
+            if self.leg is None:
+                raise ValueError(
+                    "Invalid mode transition, unknown leg: %s" %
+                    (msg.buttons,))
+            self.frame = 'body'
 
     def new_input(self, msg):
         # check if changing leg or angles
@@ -215,21 +221,25 @@ def new_input(self, msg):
 class MoveBody(Mode):
     mode = MOVE_BODY
 
-    def __init__(self, msg):
+    def __init__(self, msg=None):
         # TODO get move parameters from server
         self.scale_translate = 1 / 200.
         self.scale_rotate = 1 / 100.
-        inds = get_pressed_button_indices(msg.buttons)
-        self.translate = None
-        for i in inds:
-            if i in body_buttons:
-                if body_buttons[i] == 'translate':
-                    self.translate = True
-                else:
-                    self.translate = False
-        if self.translate is None:
-            raise ValueError(
-                "Invalid mode transition, unknown body: %s" % (msg.buttons,))
+        if msg is None:
+            self.translate = True
+        else:
+            inds = get_pressed_button_indices(msg.buttons)
+            self.translate = None
+            for i in inds:
+                if i in body_buttons:
+                    if body_buttons[i] == 'translate':
+                        self.translate = True
+                    else:
+                        self.translate = False
+            if self.translate is None:
+                raise ValueError(
+                    "Invalid mode transition, unknown body: %s" %
+                    (msg.buttons,))
 
     def new_input(self, msg):
         inds = get_pressed_button_indices(msg.buttons)
@@ -262,14 +272,125 @@ def new_input(self, msg):
 
 class PositionLegs(Mode):
     mode = POSITION_LEGS
-    # TODO lift legs, move to centers, place down
+
+    def __init__(self, msg=None):
+        # self.z_lift = -0.6
+        # self.z_lower = -1.5
+        # get all initial foot positions and loads
+        # TODO determine support triangle
+        # queue up legs to move
+        self.leg = None
+        self.state = None
+        self.target = None
+        self.last = None
+        self.speed = 0.002
+        self.moved_legs = []
+
+    def exit(self):
+        pass
+
+    def check_mode(self, msg):
+        new_mode = None
+        return new_mode
+
+    def update(self):
+        # TODO lift legs, move to centers, place down
+        if len(self.moved_legs) == len(legs.legs):
+            return IDLE, {}
+        new_mode = None
+        plans = {}
+        if self.leg is None:
+            # find new leg to move
+            loads = {}
+            for leg_name in legs.legs:
+                if leg_name in self.moved_legs:
+                    continue
+                loads[leg_name] = legs.legs[leg_name]['load']
+            # find least loaded
+            self.leg = min(loads, key=lambda l: loads[l])
+            leg_state = legs.legs[self.leg]
+            if leg_state['load'] > 20:
+                # lift
+                self.state = 'lift'
+                self.target = None
+                plans[self.leg] = leg.plans.make_message(
+                    leg.plans.VELOCITY_MODE, kinematics.frames.BODY_FRAME,
+                    (0., 0., self.speed))
+            else:
+                # skip lift
+                self.state = 'center'
+                self.target = info.foot_centers[self.leg]
+                dx = self.target[0] - leg_state['x']
+                dy = self.target[1] - leg_state['y']
+                d = (dx * dx + dy * dy) ** 0.5
+                self.last = (leg_state['time'], d)
+                dx = dx / d * self.speed
+                dy = dy / d * self.speed
+                plans[self.leg] = leg.plans.make_message(
+                    leg.plans.VELOCITY_MODE, kinematics.frames.BODY_FRAME,
+                    (dx, dy, 0.0))
+        else:
+            # continue moving leg
+            leg_state = legs.legs[self.leg]
+            if self.state == 'lower':
+                print('lower', self.leg, leg_state['load'])
+                if leg_state['load'] > 40:
+                    plans[self.leg] = leg.plans.make_stop_message(
+                        frame=kinematics.frames.BODY_FRAME)
+                    self.moved_legs.append(self.leg)
+                    self.leg = None
+            elif self.state == 'center':
+                if self.last is None or leg_state['time'] != self.last[0]:
+                    dx = self.target[0] - leg_state['x']
+                    dy = self.target[1] - leg_state['y']
+                    d = (dx * dx + dy * dy) ** 0.5
+                    if self.last is not None:
+                        dd = self.last[1] - d
+                    else:
+                        dd = 0.
+                    print(
+                        'center', self.leg, self.target, leg_state,
+                        self.last, d, dd)
+                    self.last = (leg_state['time'], d)
+                    if d < 0.05 or dd < -0.001:
+                        self.state = 'lower'
+                        self.last = leg_state.copy()
+                        plans[self.leg] = leg.plans.make_message(
+                            leg.plans.VELOCITY_MODE,
+                            kinematics.frames.BODY_FRAME,
+                            (0., 0., -self.speed))
+            elif self.state == 'lift':
+                print('lower', self.leg, leg_state['load'])
+                if self.target is None:
+                    if leg_state['load'] < 10:
+                        # continue lifting for n seconds
+                        self.target = leg_state['time'] + 1
+                else:
+                    if leg_state['time'] > self.target:
+                        self.state = 'center'
+                        self.target = info.foot_centers[self.leg]
+                        dx = self.target[0] - leg_state['x']
+                        dy = self.target[1] - leg_state['y']
+                        d = (dx * dx + dy * dy) ** 0.5
+                        dx = dx / d * self.speed
+                        dy = dy / d * self.speed
+                        self.last = (leg_state['time'], d)
+                        plans[self.leg] = leg.plans.make_message(
+                            leg.plans.VELOCITY_MODE,
+                            kinematics.frames.BODY_FRAME,
+                            (dx, dy, 0.0))
+        return new_mode, plans
+
+    def new_input(self, msg):
+        pass
 
 
 class Restriction(Mode):
     mode = RESTRICTION
 
 
 mode_classes = {
+    IDLE: Idle,
     MOVE_LEG: MoveLeg,
     MOVE_BODY: MoveBody,
     POSITION_LEGS: PositionLegs,

src/stompy_ros/python/stompy_ros/head/node.py

@@ -14,16 +14,22 @@
 
 import rospy
 import sensor_msgs.msg
+import std_msgs.msg
 
 import stompy_msgs.msg
 from .. import info
+from . import legs
 from . import modes
 
 DEADMAN_BUTTON = 1
 
 
 class HeadNode(object):
+    _leg_msg = stompy_msgs.msg.LegState
+    _load_msg = std_msgs.msg.Float64
+
     def __init__(self):
+        # TODO publish mode
         self.mode = modes.Idle(None)
         self.connect()
 
@@ -36,8 +42,11 @@ def connect(self, queue_size=10):
             self.leg_plans[leg_name] = rospy.Publisher(
                 '/stompy/%s/plan' % leg_name,
                 stompy_msgs.msg.LegPlan, queue_size=queue_size)
-            # TODO sub to foot (or do this in the mode?)
-            pass
+            # sub to foot
+            rospy.Subscriber(
+                '/stompy/%s/leg' % leg_name,
+                self._leg_msg,
+                lambda msg, name=leg_name: legs.new_leg_message(name, msg))
 
     def new_joystick(self, msg):
         # TODO throttle
@@ -59,8 +68,8 @@ def send_plans(self, plans):
     def update(self):
         new_mode, plans = self.mode.update()
         if new_mode is not None:
-            # TODO check for new mode?
-            pass
+            self.mode.exit()
+            self.mode = modes.mode_classes[new_mode]()
         if plans is not None:
             self.send_plans(plans)
 

src/stompy_ros/python/stompy_ros/kinematics/leg.py

@@ -22,8 +22,8 @@
         looking down on leg pointed right + is up, - is down
         positive and negative
     z
-        looking at side with leg pointed right, + is down
-        0 is in line with hip joint, below this is +
+        looking at side with leg pointed right, - is down
+        0 is in line with hip joint, below this is -
 """
 
 import numpy
@@ -84,7 +84,7 @@ def inverse(x, y, z):
     hip_angle = numpy.arctan2(y, x)
 
     L = numpy.sqrt(z ** 2. + (l - hip_link) ** 2.)
-    a1 = numpy.arccos(z / L)
+    a1 = numpy.arccos(-z / L)
     a2 = numpy.arccos(
         (knee_link ** 2. - thigh_link ** 2. - L ** 2.) /
         (-2 * thigh_link * L))
@@ -128,7 +128,7 @@ def forward(hip_angle, thigh_angle, knee_angle):
     # rotate about hip angle
     y = x * numpy.sin(hip_angle)
     x *= numpy.cos(hip_angle)
-    return x, y, -z
+    return x, y, z
 
 
 leg_to_joints = inverse