add locomotion controllers (by S. Xin, not operational yet)

pyrobolearn/controllers/locomotion/README.md

@@ -0,0 +1,28 @@
+## Locomotion controllers
+
+**Note**: the code given here has not yet been integrated fully with the PRL framework, and several piece of codes are 
+missing or duplicated. It is also very likely that it doesn't work yet. I will soon clean it, document it, make it 
+more modular and flexible, and integrate it into the PRL framework. If you use this code, please cite [1].
+
+
+This folder contains locomotion controllers, including:
+- High-level controllers
+    - Behavior controller which uses template simplified models (LIP, SLIP, etc).
+- Middle-level controllers
+    - Model-predictive controllers (MPCs)
+- Low-level controllers
+    - Marc Raibert's controller
+    - Inverse kinematics controller (which uses QP to optimize several kinematic tasks and constraints)
+    - Inverse dynamics controller (which uses QP to optimize several dynamic tasks and constraints)
+- Hierarchical controllers (which can accept a high-level controller, middle-level controller, and a low-level controller)
+
+
+Note that these controllers which are at different levels run at different frequencies (with the lower-level controllers running typically at higher frequencies than higher-level controllers).
+
+Several of these controllers might be using priority tasks defined in `pyrobolearn/priorities`.
+
+If you use this code, please cite [1].
+
+References:
+1. "Motion Planning and Control of Dynamic Humanoid Locomotion" (PhD thesis), Songyan Xin, 2018
+

pyrobolearn/controllers/locomotion/fsm.py

@@ -0,0 +1,164 @@
+#!/usr/bin/env python
+"""Provide the various states and transitions in a finite state machine (FSM) used in locomotion.
+
+References:
+    [1] "Motion Planning and Control of Dynamic Humanoid Locomotion" (PhD thesis), Songyan Xin, 2018
+"""
+
+
+__author__ = ["Songyan Xin", "Brian Delhaisse"]
+# S.X. wrote the main initial code
+# B.D. integrated it in the PRL framework, cleaned it, added the documentation, and made it more modular and flexible
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Songyan Xin"]
+__license__ = "MIT"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class State(object):
+    """
+    We define a state object which provides some utility functions for the individual states within the state machine.
+    """
+
+    def __init__(self):
+        self.name = str(self)
+
+    def __str__(self):
+        """
+        Returns the name of the State.
+        """
+        return self.__class__.__name__
+
+
+# sagittal hopping states
+class Stance(State):
+    """
+    State: Stance
+    """
+
+    def on_event(self, event, pre_state):
+        if event == 'TO':  # takeoff
+            return Flight()
+        else:
+            return self
+
+
+# lateral hopping states
+class LeftStance(State):
+    """
+    State: LeftStance.
+    """
+    def on_event(self, event):
+        if event == 'TO':  # takeoff
+            return FlightL2R()
+        else:
+            return self
+
+
+class RightStance(State):
+    """
+    State: RightStance.
+    """
+    def on_event(self, event):
+        if event == 'TO':  # takeoff
+            return FlightR2L()
+        else:
+            return self
+
+
+class Flight(State):
+    """
+    State: Flight
+    """
+
+    def on_event(self, event, pre_state):
+
+        if event == 'TD':  # touchdown
+            if pre_state.name is 'LeftStance':
+                return RightStance(), Flight()
+            if pre_state.name is 'RightStance':
+                return LeftStance(), Flight()
+        else:
+            return self
+
+
+class FlightL2R(State):
+    """
+    State: Flight Left to Right (L2R)
+    """
+
+    def on_event(self, event):
+        if event == 'TD':  # touchdown
+            return RightStance()
+        else:
+            return self
+
+
+class FlightR2L(State):
+    """
+    State: Flight Right to Left (R2L)
+    """
+
+    def on_event(self, event):
+        if event == 'TD':  # touchdown
+            return LeftStance()
+        else:
+            return self
+
+
+# state machine
+class HoppingStateMachine(object):
+    """Hopping state machine
+
+    A simple state machine that mimics the functionality of a device from a high level.
+    """
+
+    def __init__(self):
+        """ Initialize the components. """
+
+        # Start with a default state.
+        self.curr_state = FlightR2L()
+        self.prev_state = RightStance()
+        print("Initial State: {}".format(self.curr_state))
+
+        # event flag
+        self.TD_flag = False
+        self.TO_flag = False
+
+    def on_event(self, event, debug=True):
+        """
+        This is the bread and butter of the state machine. Incoming events are
+        delegated to the given states which then handle the event. The result is
+        then assigned as the new state.
+        """
+
+        # The next state will be the result of the on_event function.
+        self.prev_state = self.curr_state
+        self.curr_state = self.curr_state.on_event(event)
+
+        if event is "TD": # touchdown
+            self.TD_flag = True
+            self.TO_flag = False
+        elif event is "TO": # takeoff
+            self.TD_flag = False
+            self.TO_flag = True
+
+        if debug:
+            print("{} -> {} -> {}".format(self.prev_state, event, self.curr_state))
+
+
+def test_hopping_state_machine():
+    hopping_state_machine = HoppingStateMachine()
+    hopping_state_machine.on_event('TD')
+    hopping_state_machine.on_event('TO')
+    hopping_state_machine.on_event('TD')
+    hopping_state_machine.on_event('TO')
+    hopping_state_machine.on_event('TD')
+
+
+# Tests
+if __name__ == '__main__':
+    test_hopping_state_machine()

pyrobolearn/controllers/locomotion/inverse_dynamic_controller.py

@@ -0,0 +1,338 @@
+#!/usr/bin/env python
+"""Provide the inverse dynamic controller for locomotion.
+
+The inverse dynamic controller is a low-level controller that uses quadratic programming to solve several dynamic
+tasks and constraints.
+
+References:
+    [1] "Motion Planning and Control of Dynamic Humanoid Locomotion" (PhD thesis), Songyan Xin, 2018
+"""
+
+import numpy as np
+from scipy import linalg
+from qpsolvers import solve_qp
+
+import rbdl
+
+from utils.task import Task
+from utils.robot_param import RobotParam
+from utils.geometry import quaternionPD, posePD
+
+from pyrobolearn.controllers.controller import Controller
+
+
+__author__ = ["Songyan Xin", "Brian Delhaisse"]
+# S.X. wrote the main initial code
+# B.D. integrated it in the PRL framework, cleaned it, added the documentation, and made it more modular and flexible
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Songyan Xin"]
+__license__ = "MIT"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class InverseDynamicController(Controller):
+    r"""Inverse Dynamic Controller
+
+    The inverse dynamic controller is a low-level controller that uses quadratic programming to solve several dynamic
+    tasks and constraints.
+
+    References:
+        [1] "Motion Planning and Control of Dynamic Humanoid Locomotion" (PhD thesis), Songyan Xin, 2018
+    """
+
+    def __init__(self, urdf_path=''):
+        super(InverseDynamicController, self).__init__()
+
+        # # params
+        # self.robot_name = rospy.get_param("/robot_name")
+        # self.joint_controller_name = rospy.get_param("/joint_controller_name")
+        # self.actuated_joint_names = rospy.get_param("/" + self.robot_name + "/" + self.joint_controller_name + "/joints")
+
+        # model load from rosparam
+        # self.urdf_string = rospy.get_param("/robot_description")
+        # self.rbdl_model = rbdl.URDFReadFromString(self.urdf_string, verboase=False, floating_base=True)
+
+        # load model from file
+        # urdf_path = '/home/xin/codes/agile_robot/envs/robots/urdfs/cogimon_urdf/cogimon.urdf'
+
+        self.rbdl_model = rbdl.loadModel(urdf_path, verboase=False, floating_base=True)
+        self.N = self.rbdl_model.dof_count
+
+        # robot params
+        self.robot_param = RobotParam(urdf_path)
+
+    def __call__(self, robot_state, high_level_cmd):
+        low_level_cmd = None
+        return low_level_cmd
+
+    def CalcJointTorqueNewFormulation(self, robot_state, high_level_cmd):
+        cmd = high_level_cmd
+        # cmd.show()
+
+        if cmd.contact_state == "noSupport":
+            num_of_contacts = 0
+        elif cmd.contact_state == "leftSupport":
+            num_of_contacts = 1
+            J_contact = robot_state.lsole.J
+            Jdqd_contact = robot_state.lsole.Jdqd
+        elif cmd.contact_state == "rightSupport":
+            num_of_contacts = 1
+            J_contact = robot_state.rsole.J
+            Jdqd_contact = robot_state.rsole.Jdqd
+        elif cmd.contact_state == "doubleSupport":
+            num_of_contacts = 2
+            J_contact = np.vstack((robot_state.lsole.J, robot_state.rsole.J))
+            Jdqd_contact = np.hstack((robot_state.lsole.Jdqd, robot_state.rsole.Jdqd))
+
+        # least square task: minimize qdd and GRFs
+        ls_A = np.identity(self.N + num_of_contacts * 6)
+        ls_b = np.zeros(self.N + num_of_contacts * 6)
+        ls_task = Task(ls_A, ls_b)
+
+        # minimize joint torque task: tau
+        if cmd.contact_state == "noSupport":
+            min_torque_A = robot_state.inertia_matrix
+            min_torque_b = - robot_state.nonlinear_effects
+        else:
+            min_torque_A = np.hstack((robot_state.inertia_matrix, -J_contact.T))
+            min_torque_b = - robot_state.nonlinear_effects
+        min_torque_task = Task(min_torque_A, min_torque_b)
+
+        # minimize qdd:
+        min_qdd_A = np.hstack((np.identity(self.N), np.zeros((self.N, num_of_contacts * 6))))
+        min_qdd_b = np.zeros(self.N)
+        min_qdd_task = Task(min_qdd_A, min_qdd_b)
+
+        # minmize GRF
+        if cmd.contact_state == "noSupport":
+            min_GRF_task = None
+        else:
+            min_GRF_A = np.hstack((np.zeros((num_of_contacts * 6,self.N)), np.identity(num_of_contacts * 6)))
+            min_GRF_b = np.zeros(num_of_contacts * 6)
+            min_GRF_task = Task(min_GRF_A, min_GRF_b)
+
+
+        # foot tracking task
+        if cmd.contact_state == "noSupport":
+            lsole_acc = posePD(pose_des=cmd.lsole_pose, pose_cur=robot_state.lsole.pose,
+                               spatial_velocity_des=cmd.lsole_spatial_velocity,
+                               spatial_velocity_cur=robot_state.lsole.spatial_velocity,
+                               kp_linear=1000, kd_linear=2.0 * np.sqrt(100),
+                               kp_angular=1000, kd_angular=2.0 * np.sqrt(10))
+
+            rsole_acc = posePD(pose_des=cmd.rsole_pose, pose_cur=robot_state.rsole.pose,
+                               spatial_velocity_des=cmd.rsole_spatial_velocity,
+                               spatial_velocity_cur=robot_state.rsole.spatial_velocity,
+                               kp_linear=1000, kd_linear=2.0 * np.sqrt(100),
+                               kp_angular=1000, kd_angular=2.0 * np.sqrt(10))
+
+            feet_track_A = np.vstack((robot_state.lsole.J, robot_state.rsole.J))
+            feet_track_b = np.hstack((lsole_acc, rsole_acc)) - np.hstack((robot_state.lsole.Jdqd, robot_state.rsole.Jdqd))
+            feet_track_task = Task(feet_track_A, feet_track_b)
+
+
+        elif cmd.contact_state == "leftSupport":
+            rsole_acc = posePD(pose_des=cmd.rsole_pose, pose_cur=robot_state.rsole.pose,
+                               spatial_velocity_des=cmd.rsole_spatial_velocity,
+                               spatial_velocity_cur=robot_state.rsole.spatial_velocity,
+                               kp_linear=500, kd_linear=2.0 * np.sqrt(500),
+                               kp_angular=1000, kd_angular=2.0 * np.sqrt(500))
+            rfoot_track_A = np.hstack((robot_state.rsole.J, np.zeros((6, num_of_contacts * 6))))
+            rfoot_track_b = rsole_acc - robot_state.rsole.Jdqd
+            rfoot_track_task = Task(rfoot_track_A, rfoot_track_b)
+
+
+        elif cmd.contact_state == "rightSupport":
+            lsole_acc = posePD(pose_des=cmd.lsole_pose, pose_cur=robot_state.lsole.pose,
+                               spatial_velocity_des=cmd.lsole_spatial_velocity,
+                               spatial_velocity_cur=robot_state.lsole.spatial_velocity,
+                               kp_linear=500, kd_linear=2.0 * np.sqrt(500),
+                               kp_angular=1000, kd_angular=2.0 * np.sqrt(500))
+            lfoot_track_A = np.hstack((robot_state.lsole.J, np.zeros((6, num_of_contacts * 6))))
+            lfoot_track_b = lsole_acc - robot_state.lsole.Jdqd
+            lfoot_track_task = Task(lfoot_track_A, lfoot_track_b)
+
+
+
+        elif cmd.contact_state == "doubleSupport":
+            lsole_acc = np.zeros(6)
+            rsole_acc = np.zeros(6)
+            lsole_acc = - 2.0 * np.sqrt(10) * robot_state.lsole.spatial_velocity
+            rsole_acc = - 2.0 * np.sqrt(10) * robot_state.rsole.spatial_velocity
+            # lsole_acc = posePD(pose_des=cmd.lsole_pose, pose_cur=robot_state.lsole.pose,
+            #                    spatial_velocity_des=cmd.lsole_spatial_velocity,
+            #                    spatial_velocity_cur=robot_state.lsole.spatial_velocity,
+            #                    kp_linear=0, kd_linear=2.0 * np.sqrt(10),
+            #                    kp_angular=0, kd_angular=2.0 * np.sqrt(10))
+            # rsole_acc = posePD(pose_des=cmd.rsole_pose, pose_cur=robot_state.rsole.pose,
+            #                    spatial_velocity_des=cmd.rsole_spatial_velocity,
+            #                    spatial_velocity_cur=robot_state.rsole.spatial_velocity,
+            #                    kp_linear=0, kd_linear=2.0 * np.sqrt(10),
+            #                    kp_angular=0, kd_angular=2.0 * np.sqrt(10))
+
+            feet_damp_A = np.hstack((np.vstack((robot_state.lsole.J, robot_state.rsole.J)), np.zeros((12, num_of_contacts * 6))))
+            feet_damp_b = np.hstack((lsole_acc, rsole_acc)) - np.hstack((robot_state.lsole.Jdqd, robot_state.rsole.Jdqd))
+            feet_damp_task = Task(feet_damp_A, feet_damp_b)
+
+
+        # foot_A = np.hstack((np.vstack((robot_state.lsole.J, robot_state.rsole.J)), np.zeros((12, num_of_contacts * 6))))
+        # foot_b = np.hstack((lsole_acc, rsole_acc)) - np.hstack((robot_state.lsole.Jdqd, robot_state.rsole.Jdqd))
+        # foot_task = Task(foot_A, foot_b)
+
+        # centroidal dynamic task
+        kp_linear = 300
+        kp_angular = 10
+        des_linear_momentum = robot_state.mass * (
+                kp_linear * (cmd.com_pose[:3] - robot_state.com) + 2 * np.sqrt(kp_linear) * (
+                cmd.com_spatial_velocity[-3:] - robot_state.com_velocity) + cmd.com_spatial_acceleration[-3:])
+        des_angular_momentum = kp_angular * cmd.angular_momentum - 2 * np.sqrt(
+            kp_angular) * robot_state.angular_momentum
+
+        # angular momentum task
+        angular_momentum_A = np.hstack((robot_state.CMM[:3, :], np.zeros((3, num_of_contacts * 6))))
+        angular_momentum_b = des_angular_momentum - robot_state.CMM_bias_force[:3]
+        angular_momentum_task = Task(angular_momentum_A, angular_momentum_b)
+
+        # linear momentum task
+        linear_momentum_A = np.hstack((robot_state.CMM[-3:, :], np.zeros((3, num_of_contacts * 6))))
+        linear_momentum_b = des_linear_momentum - robot_state.CMM_bias_force[-3:]
+        linear_momentum_task = Task(linear_momentum_A, linear_momentum_b)
+
+        # pelvis orientation task
+        pelvis_orientation_A = np.hstack((robot_state.waist.J[:3, :], np.zeros((3, num_of_contacts * 6))))
+        pelvis_orientation_b = quaternionPD(quat_des=cmd.com_pose[-4:], quat_cur=robot_state.waist.quaternion,
+                                            omega_des=cmd.com_spatial_velocity[:3],
+                                            omega_cur=robot_state.waist.spatial_velocity[:3],
+                                            kp=1000, kd=2.0 * np.sqrt(500))
+        pelvis_orientation_task = Task(pelvis_orientation_A, pelvis_orientation_b)
+
+        # set task weight
+        # ls_task.set_weight(50)
+        # pelvis_orientation_task.set_weight(100)
+        # linear_momentum_task.set_weight(300)
+        # angular_momentum_task.set_weight(100)
+
+
+        # choose task combination
+        if cmd.contact_state == "noSupport":
+            ls_task.set_weight(10)
+            min_qdd_task.set_weight(10)
+            min_torque_task.set_weight(10)
+            feet_track_task.set_weight(100)
+            pelvis_orientation_task.set_weight(10)
+            tasks = [min_qdd_task, min_torque_task, feet_track_task, pelvis_orientation_task]
+
+        elif cmd.contact_state == "doubleSupport":
+            ls_task.set_weight(50) # = min_qdd + min_GRF
+            min_torque_task.set_weight(50)
+            min_qdd_task.set_weight(50)
+            min_GRF_task.set_weight(50)
+            pelvis_orientation_task.set_weight(100)
+            linear_momentum_task.set_weight(300)
+            feet_damp_task.set_weight(100)
+            angular_momentum_task.set_weight(100)
+            tasks = [ls_task, feet_damp_task, pelvis_orientation_task, linear_momentum_task, angular_momentum_task]
+
+        elif cmd.contact_state == "leftSupport":
+            ls_task.set_weight(1)
+            rfoot_track_task.set_weight(300)
+            pelvis_orientation_task.set_weight(10)
+            linear_momentum_task.set_weight(300)
+            angular_momentum_task.set_weight(10)
+            tasks = [ls_task, rfoot_track_task, pelvis_orientation_task, linear_momentum_task, angular_momentum_task]
+
+        elif cmd.contact_state == "rightSupport":
+            ls_task.set_weight(1)
+            lfoot_track_task.set_weight(300)
+            pelvis_orientation_task.set_weight(10)
+            linear_momentum_task.set_weight(300)
+            angular_momentum_task.set_weight(10)
+            tasks = [ls_task, lfoot_track_task, pelvis_orientation_task, linear_momentum_task, angular_momentum_task]
+
+        # combine all task A and b matrices
+        A = np.empty((0, self.N + num_of_contacts * 6))
+        b = np.empty(0)
+        for task in tasks:
+            A = np.vstack((A, task.w * task.A))
+            b = np.hstack((b, task.w * task.b))
+
+        ############################
+        #  Inequality constraints  #
+        ############################
+
+        if cmd.contact_state == "noSupport":
+            inequalityConsMatrix = np.zeros((9, self.N + num_of_contacts * 6))
+            inequalityConsVector = np.zeros(9)
+
+        elif cmd.contact_state == "leftSupport":
+            lGRFCons = self.robot_param.SpatialForceCons.dot(
+                linalg.block_diag(robot_state.lsole.rot.T, robot_state.lsole.rot.T))
+            inequalityConsMatrix = np.hstack((np.zeros((9, self.N)), lGRFCons))
+            inequalityConsVector = np.zeros(9)
+
+        elif cmd.contact_state == "rightSupport":
+            rGRFCons = self.robot_param.SpatialForceCons.dot(
+                linalg.block_diag(robot_state.rsole.rot.T, robot_state.rsole.rot.T))
+            inequalityConsMatrix = np.hstack((np.zeros((9, self.N)), rGRFCons))
+            inequalityConsVector = np.zeros(9)
+
+        elif cmd.contact_state == "doubleSupport":
+            lGRFCons = self.robot_param.SpatialForceCons.dot(
+                linalg.block_diag(robot_state.lsole.rot.T, robot_state.lsole.rot.T))
+            rGRFCons = self.robot_param.SpatialForceCons.dot(
+                linalg.block_diag(robot_state.rsole.rot.T, robot_state.rsole.rot.T))
+            GRFCons = np.vstack((np.hstack((lGRFCons, np.zeros((9, 6)))),
+                                 np.hstack((np.zeros((9, 6)), rGRFCons))))
+            inequalityConsMatrix = np.hstack((np.zeros((18, self.N)), GRFCons))
+            inequalityConsVector = np.zeros(18)
+
+        ########################
+        # Equality constraints #
+        ########################
+        # dynamic constraints
+
+        if cmd.contact_state == "noSupport":
+            dynamicConsMatrix = robot_state.inertia_matrix[:6, :]  # 6*N
+            dynamicConsVector = - robot_state.nonlinear_effects[:6]  # 6*1
+        else:
+            dynamicConsMatrix = np.hstack((robot_state.inertia_matrix[:6, :], -J_contact.T[:6, :]))  # 6*(N+12)
+            dynamicConsVector = - robot_state.nonlinear_effects[:6]  # 6*1
+
+        # # foot no movement constraints
+        # feetFixConsMatrix = np.hstack((J_contact, np.zeros((12, num_of_contacts * 6))))
+        # feetFixConsVector = - Jdqd_contact
+        #
+        #
+        # equalityConsMatrix = np.vstack((dynamicConsMatrix, feetFixConsMatrix))
+        # equalityConsVector = np.hstack((dynamicConsVector, feetFixConsVector))
+
+        equalityConsMatrix = dynamicConsMatrix
+        equalityConsVector = dynamicConsVector
+
+        ##########################
+        #  Solve the QP problem  #
+        ##########################
+        X = solve_qp(P=A.T.dot(A), q=-A.T.dot(b),
+                     G=inequalityConsMatrix, h=inequalityConsVector,
+                     A=equalityConsMatrix, b=equalityConsVector,
+                     solver='quadprog')
+
+        # print("X.shape:", X.shape)
+        qdd = X[:self.N]
+        GRF = X[self.N:]
+
+        ###################
+        # Inverse Dynamic #
+        ###################
+        if cmd.contact_state == "noSupport":
+            tau_N = robot_state.inertia_matrix.dot(qdd) + robot_state.nonlinear_effects  # N
+            tau_n = tau_N[6:]
+        else:
+            tau_N = robot_state.inertia_matrix.dot(qdd) + robot_state.nonlinear_effects - J_contact.T.dot(GRF)  # N
+            tau_n = tau_N[6:]
+
+        return tau_n
+

pyrobolearn/controllers/locomotion/inverse_kinematic_controller.py

@@ -0,0 +1,151 @@
+#!/usr/bin/env python
+"""Provide the inverse kinematic controller for locomotion.
+
+The inverse kinematic controller is a low-level controller that uses quadratic programming to solve several kinematic
+tasks and constraints.
+
+References:
+    [1] "Motion Planning and Control of Dynamic Humanoid Locomotion" (PhD thesis), Songyan Xin, 2018
+"""
+
+import os
+import tf
+import rbdl
+import cvxopt
+import numpy as np
+from cvxopt import matrix, solvers
+
+from utils.utils import *
+from utils.geometry import PositionPD, QuaternionPD, PosePD
+
+from pyrobolearn.controllers.controller import Controller
+
+
+__author__ = ["Songyan Xin", "Brian Delhaisse"]
+# S.X. wrote the main initial code
+# B.D. integrated it in the PRL framework, cleaned it, added the documentation, and made it more modular and flexible
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Songyan Xin"]
+__license__ = "MIT"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class InverseKinematicController(Controller):
+    r"""Inverse Kinematics Controller
+
+    The inverse kinematic controller is a low-level controller that uses quadratic programming to solve several
+    kinematic tasks and constraints.
+
+    References:
+        [1] "Motion Planning and Control of Dynamic Humanoid Locomotion" (PhD thesis), Songyan Xin, 2018
+    """
+
+    def __init__(self):
+        super(InverseKinematicController, self).__init__()
+
+    def jacobian_stack(self, robot, cmd):
+
+        Xd_lsole = PosePD(pose_des=cmd.lsole_pose, pose_cur=robot.state.lsole.pose, kp=100.0, kd=0.0)
+        Xd_rsole = PosePD(pose_des=cmd.rsole_pose, pose_cur=robot.state.rsole.pose, kp=100.0, kd=0.0)
+        Xd_com = robot.state.mass * PositionPD(pos_des=cmd.com_pose.position, pos_cur=robot.state.com_pos, vel_cur=robot.state.com_vel, kp=300.0, kd=1.0)
+        Xd_waist = QuaternionPD(quat_des=cmd.com_pose.quaternion, quat_cur=robot.state.waist.quaternion, kp=100.0, kd=10.0)
+
+        if cmd.contact_state is ContactState.doubleSupport:
+            # stack all task jacobians to get J
+            J = np.vstack((robot.state.lsole.J, robot.state.rsole.J, robot.state.CMM[-3:, :], robot.state.waist.J[:3, :]))
+            Xd = np.hstack((Xd_lsole, Xd_rsole, Xd_com, Xd_waist))
+            qd_cmd = np.linalg.pinv(J).dot(Xd)
+
+        elif cmd.contact_state is ContactState.leftSupport:
+            # task of first priority
+            J_1 = np.vstack((robot.state.lsole.J, robot.state.CMM[-3:, :], robot.state.waist.J[:3, :]))
+            Xd_1 = np.hstack((Xd_lsole, Xd_com, Xd_waist))
+            # task of second priority
+            J_2 = robot.state.rsole.J
+            Xd_2 = Xd_rsole
+            qd_cmd = np.linalg.pinv(J_1).dot(Xd_1) + nullspace(J_1).dot(np.linalg.pinv(J_2)).dot(Xd_2)
+
+        elif cmd.contact_state is ContactState.rightSupport:
+            # task of first priority
+            J_1 = np.vstack((robot.state.rsole.J, robot.state.CMM[-3:, :], robot.state.waist.J[:3, :]))
+            Xd_1 = np.hstack((Xd_rsole, Xd_com, Xd_waist))
+            # task of second priority
+            J_2 = robot.state.lsole.J
+            Xd_2 = Xd_lsole
+            qd_cmd = np.linalg.pinv(J_1).dot(Xd_1) + nullspace(J_1).dot(np.linalg.pinv(J_2)).dot(Xd_2)
+
+        elif cmd.contact_state is ContactState.noSupport:
+            J = np.vstack((robot.state.CMM[-3:, :], robot.state.waist.J[:3, :]))
+            Xd = np.hstack((Xd_com, Xd_waist))
+            qd_cmd = np.linalg.pinv(J).dot(Xd)
+
+        return qd_cmd[6:]
+
+    def nullspace_projection(self, robot, cmd):
+
+        Xd_com = robot.state.mass * PositionPD(pos_des=cmd.com_pose.position, pos_cur=robot.state.com_pos, vel_des=cmd.com_vel, vel_cur=robot.state.com_vel, kp=300.0, kd=1.0)
+        Xd_waist = QuaternionPD(quat_des=cmd.com_pose.quaternion, quat_cur=robot.state.waist.quaternion, kp=100.0, kd=1.0)
+
+        # task of first priority
+        J_1 = np.vstack((robot.state.lsole.J, robot.state.rsole.J, robot.state.CMM[-3:, :]))
+        Xd_1 = np.hstack((np.zeros(6), np.zeros(6), Xd_com))
+
+        # task of second priority
+        J_2 = robot.state.waist.J[:3, :]
+        Xd_2 = Xd_waist
+
+        qd_cmd = np.linalg.pinv(J_1).dot(Xd_1) + nullspace(J_1).dot(np.linalg.pinv(J_2)).dot(Xd_2)
+
+        return qd_cmd[6:]
+
+    def qp(self, robot, cmd):
+
+        # define all tasks
+
+        # 1. least square task
+        A_ls = np.identity(robot.N)
+        b_ls = np.zeros(robot.N)
+
+        # 2. com task
+        kp = 100
+        A_com = robot.state.CMM[-3:, :]
+        b_com = robot.state.mass * (kp*(cmd.com_pos-robot.state.com_pos) - 2*np.sqrt(kp)*robot.state.com_vel)
+
+        # 3. base orientation task
+        A_base = robot.state.waist.J[:3, :]
+        b_base = RotationPD(cmd.waist_rot, robot.state.waist.rot, np.zeros(3), robot.state.waist.angular_vel, np.zeros(3), kp=500)
+
+        # 4. lsole task
+        A_lsole = robot.state.lsole.J
+        b_lsole = SE3PD(cmd.lsole_pos, cmd.lsole_rot,
+                              robot.state.lsole.pos, robot.state.lsole.rot, np.hstack((np.zeros(3), cmd.lsole_vel)),
+                              robot.state.lsole.vel,
+                              np.zeros(6),
+                              200, 200)
+
+        # 5. rsole task
+        A_rsole = robot.state.rsole.J
+        b_rsole = SE3PD(cmd.rsole_pos, cmd.rsole_rot,
+                              robot.state.rsole.pos, robot.state.rsole.rot, np.hstack((np.zeros(3), cmd.rsole_vel)),
+                              robot.state.rsole.vel,
+                              np.zeros(6),
+                              200, 200)
+
+        w_ls = 1.0
+        w_com = 10.0
+        w_base = 10.0
+        w_lsole = 0.0
+        w_rsole = 0.0
+
+        # combine all tasks
+        A = np.vstack((w_ls*A_ls, w_com*A_com, w_base*A_base, w_lsole*A_lsole, w_rsole*A_rsole))
+        b = np.hstack((w_ls*b_ls, w_com*b_com, w_base*b_base, w_lsole*b_lsole, w_rsole*b_rsole))
+
+        qd_N = np.linalg.pinv(A).dot(b)
+        qd_n = qd_N[6:]
+
+        return qd_n
+

pyrobolearn/controllers/locomotion/marc_raibert_controller.py

@@ -0,0 +1,75 @@
+#!/usr/bin/env python
+"""Provide the Marc Raibert's controller for locomotion.
+"""
+
+import rospy
+import numpy as np
+from custom_srv.srv import *
+from util import sigmoid
+
+
+__author__ = ["Songyan Xin", "Brian Delhaisse"]
+# S.X. wrote the main initial code
+# B.D. integrated it in the PRL framework, cleaned it, added the documentation, and made it more modular and flexible
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Songyan Xin"]
+__license__ = "MIT"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+def MarcRaibertFootPlacement(com_vel_des, com_vel, K, T):
+    shift = com_vel * T / 2.0 + K * (com_vel - com_vel_des)
+    return shift
+
+
+class MarcRaibertController(object):
+    r"""Marc Raibert Controller
+
+    """
+
+    def __init__(self, des_com_vel, Kp=[0.3, 0.3, 0.0], Kd=[0.1, 0.1, 0.0]):
+        self.des_com_vel = np.array(des_com_vel)
+        self.Kp = np.array(Kp)
+        self.Kd = np.array(Kd)
+        self.update_des_com_vel_server = rospy.Service('update_des_com_vel', PassVector, self.handle_update_des_com_vel)
+        self.update_K_server = rospy.Service('update_K', PassVector, self.handle_update_K)
+
+    def __call__(self, cur_com_vel, T, leg_length, step_count):
+        # if T == 0.0:
+        #     feedback = self.Kp * (cur_com_vel - self.des_com_vel) - self.Kd * cur_com_vel
+        #     shift = feedback
+        #     print "shift: ", shift
+        # else:
+        #     neutral_point = cur_com_vel * T / 2.0
+        #     neutral_point[2] = 0.0
+        #     feedback = self.Kp * (cur_com_vel - self.des_com_vel) - self.Kd*cur_com_vel
+        #     shift = (neutral_point + feedback)*sigmoid(self.count, shift_x= 0.0, scale_x = 0.5,shift_y=-1.0, scale_y=2.0)
+        #     print "shift: ", shift, " = ", neutral_point, "+ ", feedback
+
+        neutral_point = cur_com_vel * T / 2.0
+        neutral_point[2] = 0.0
+        feedback = self.Kp * (cur_com_vel - self.des_com_vel) - self.Kd * cur_com_vel
+        shift = (neutral_point + feedback) * sigmoid(step_count, shift_x=0.0, scale_x=0.5, shift_y=-1.0, scale_y=2.0)
+        shift[2] = - np.sqrt(leg_length ** 2 - shift[0] ** 2 - shift[1] ** 2)
+        # print "step_count: ", step_count, "shift: ", shift, " = ", neutral_point, "+ ", feedback
+
+        return shift
+
+    def update_des_com_vel(self, des_com_vel):
+        self.des_com_vel = des_com_vel
+
+    def update_K(self, K):
+        self.K = K
+
+    def handle_update_des_com_vel(self, request):
+        print "request: ", request
+        self.des_com_vel = np.array([request.vector.x, request.vector.y, request.vector.z])
+        return PassVectorResponse(1, "MarcRaibertController: des_com_vel updated!")
+
+    def handle_update_K(self, request):
+        print "request: ", request
+        self.K = np.array([request.vector.x, request.vector.y, request.vector.z])
+        return PassVectorResponse(1, "MarcRaibertController: K updated!")

pyrobolearn/controllers/locomotion/utils/README.md

@@ -0,0 +1,4 @@
+## utils functions for locomotion
+
+Note that this folder will likely disappear. The various code (written and provided by Songyan Xin) will be integrated into the PRL framework.
+

pyrobolearn/controllers/locomotion/utils/command.py

@@ -0,0 +1,70 @@
+import numpy as np
+
+
+class HighLevelCommand(object):
+    def __init__(self, com_pos=[0, 0, 0], com_quat=[0, 0, 0, 1],
+                 lsole_pos=[0, 0, 0], lsole_quat=[0, 0, 0, 1],
+                 rsole_pos=[0, 0, 0], rsole_quat=[0, 0, 0, 1],
+                 com_vel_linear=np.zeros(3),
+                 com_vel_angular=np.zeros(3),
+                 com_acc_linear=np.zeros(3),
+                 com_acc_angular=np.zeros(3),
+                 lsole_vel_linear=np.zeros(3),
+                 lsole_vel_angular=np.zeros(3),
+                 lsole_acc_linear=np.zeros(3),
+                 lsole_acc_angular=np.zeros(3),
+                 rsole_vel_linear=np.zeros(3),
+                 rsole_vel_angular=np.zeros(3),
+                 rsole_acc_linear=np.zeros(3),
+                 rsole_acc_angular=np.zeros(3),
+                 angular_momentum=np.zeros(3),
+                 contact_state=""):
+
+        self.com_pos = com_pos
+        self.com_quat = com_quat
+        self.com_pose = np.concatenate((com_pos, com_quat))
+
+        self.com_vel_linear = com_vel_linear
+        self.com_vel_angular = com_vel_angular
+        self.com_spatial_velocity = np.concatenate((com_vel_angular, com_vel_linear))
+
+        self.com_acc_linear = com_acc_linear
+        self.com_acc_angular = com_acc_angular
+        self.com_spatial_acceleration = np.concatenate((com_acc_angular, com_acc_linear))
+
+        self.lsole_pos = lsole_pos
+        self.lsole_quat = lsole_quat
+        self.lsole_pose = np.concatenate((lsole_pos, lsole_quat))
+
+        self.lsole_vel_linear = lsole_vel_linear
+        self.lsole_vel_angular = lsole_vel_angular
+        self.lsole_spatial_velocity = np.concatenate((lsole_vel_angular, lsole_vel_linear))
+
+        self.lsole_acc_linear = lsole_acc_linear
+        self.lsole_acc_angular = lsole_acc_angular
+        self.lsole_spatial_acceleration = np.concatenate((lsole_acc_angular,lsole_acc_linear))
+
+        self.rsole_pos = rsole_pos
+        self.rsole_quat = rsole_quat
+        self.rsole_pose = np.concatenate((rsole_pos, rsole_quat))
+
+        self.rsole_vel_linear = rsole_vel_linear
+        self.rsole_vel_angular = rsole_vel_angular
+        self.rsole_spatial_velocity = np.concatenate((rsole_vel_angular, rsole_vel_linear))
+
+        self.rsole_acc_linear = rsole_acc_linear
+        self.rsole_acc_angular = rsole_acc_angular
+        self.rsole_spatial_acceleration = np.concatenate((rsole_acc_angular, rsole_acc_linear))
+
+        self.angular_momentum = angular_momentum
+
+        self.contact_state = contact_state
+
+    def show(self):
+        print("-" * 30)
+        print("[HighLevelCommand]")
+        print("contact_state: ", self.contact_state)
+        print("com_pos:", self.com_pos)
+        print("lsole_pos:", self.lsole_pos)
+        print("rsole_pos:", self.rsole_pos)
+        print("-" * 30)

pyrobolearn/controllers/locomotion/utils/geometry.py

@@ -0,0 +1,78 @@
+import numpy as np
+from tf import transformations
+'''
+geometry types:
+
+position: [x,y,z]
+quaternion: [qx,qy,qz,qw]
+pose: [x,y,z,qx,qy,qz,qw]
+
+twist: [vx,vy,vz,wx,wy,wz]
+wrench: [fx,fy,fz,tx,ty,tz]
+
+spatial_velocity = [wx,wy,wz,vx,vy,vz]
+spatial_force = [tx,ty,tz,fx,fy,fz] 
+
+'''
+# homogeneous_vector = lambda P: np.append(P,1)
+def homogeneous_vector(P):
+    return np.hstack((P, 1))
+
+
+# homogeneous_matrix = lambda rot=np.identity(3), pos=np.zeros(3): np.vstack((np.append(rot[0, :], pos[0]), np.append(rot[1, :], pos[1]), np.append(rot[2, :], pos[2]), np.array([0, 0, 0, 1])))
+def homogeneous_matrix(rot=np.identity(3), pos=np.zeros(3)):
+    transform_matrix = np.identity(4)
+    transform_matrix[:3, :3] = rot[:3, :3]
+    transform_matrix[:3, -1] = pos[:3]
+    return transform_matrix
+
+def transform_matrix(rot=np.identity(3), pos=np.zeros(3)):
+    transform_matrix = np.identity(4)
+    transform_matrix[:3, :3] = rot[:3, :3]
+    transform_matrix[:3, -1] = pos[:3]
+    return transform_matrix
+
+
+def pose2transform(pose):
+    position, quaternion = pose[:3], pose[-4:]
+    rotation_matrix = transformations.quaternion_matrix(quaternion)
+    rotation_matrix[:3, -1] = position
+    return rotation_matrix
+
+def transform2pose(transform):
+    position = transform[:3,-1]
+    quaternion = transformations.quaternion_from_matrix(transform)
+    pose = np.concatenate((position, quaternion))
+    return pose
+
+
+def positionPD(pos_des, pos_cur, vel_des=np.zeros(3), vel_cur=np.zeros(3), acc_des=np.zeros(3), kp=100, kd=0.0):
+    return kp * (pos_des - pos_cur) + kd * (vel_des - vel_cur) + acc_des
+
+def rotationPD(rot_des, rot_cur, omega_des=np.zeros(3), omega_cur=np.zeros(3), omega_dot_des=np.zeros(3), kp=200,
+               kd=0.0):
+    vex = lambda M: 0.5 * np.array([M[2, 1] - M[1, 2], M[0, 2] - M[2, 0], M[1, 0] - M[0, 1]])
+    return kp * vex(rot_des.dot(rot_cur.T) - np.identity(3)) + kd * (omega_des - omega_cur) + omega_dot_des
+
+def quaternion_error(quat_des, quat_cur):
+    skew = lambda V: np.array([[0, -V[2], V[1]], [V[2], 0, -V[0]], [-V[1], V[0], 0]])
+    diff_quat = quat_cur[-1] * quat_des[:3] - quat_des[-1] * quat_cur[:3] - skew(quat_des[:3]).dot(quat_cur[:3])
+    return diff_quat
+
+def quaternionPD(quat_des, quat_cur, omega_des=np.zeros(3), omega_cur=np.zeros(3), omega_dot_des=np.zeros(3), kp=100,
+                 kd=0.0):
+    return kp * quaternion_error(quat_des=quat_des, quat_cur=quat_cur) + kd * (omega_des - omega_cur) + omega_dot_des
+
+def posePD(pose_des, pose_cur, spatial_velocity_des=np.zeros(6), spatial_velocity_cur=np.zeros(6), spatial_acceleration_des=np.zeros(6), kp_linear=100, kd_linear=10, kp_angular=100, kd_angular=10):
+    error_linear = positionPD(pos_cur=pose_cur[:3], pos_des=pose_des[:3],
+                              vel_cur=spatial_velocity_cur[-3:],
+                              vel_des=spatial_velocity_des[-3:],
+                              acc_des=spatial_acceleration_des[-3:],
+                              kp=kp_linear, kd=kd_linear)
+    error_angular = quaternionPD(quat_cur=pose_cur[-4:], quat_des=pose_des[-4:],
+                                 omega_cur=spatial_velocity_cur[:3],
+                                 omega_des=spatial_velocity_des[:3],
+                                 omega_dot_des=spatial_acceleration_des[:3],
+                                 kp=kp_angular, kd=kd_angular)
+    error = np.concatenate((error_angular, error_linear))
+    return error

pyrobolearn/controllers/locomotion/utils/math.py

@@ -0,0 +1,82 @@
+import numpy as np
+
+
+# skew = lambda V: np.array([[0, -V[2], V[1]], [V[2], 0, -V[0]], [-V[1], V[0], 0]])
+def skew(V):
+    return np.array([[0, -V[2], V[1]], [V[2], 0, -V[0]], [-V[1], V[0], 0]])
+
+
+# vex = lambda M: 0.5 * np.array([M[2, 1] - M[1, 2], M[0, 2] - M[2, 0], M[1, 0] - M[0, 1]])
+def vex(M):
+    return 0.5 * np.array([M[2, 1] - M[1, 2], M[0, 2] - M[2, 0], M[1, 0] - M[0, 1]])
+
+
+def nullspace(M):
+    return np.identity(M.shape[1]) - np.linalg.pinv(M).dot(M)
+
+
+def sigmoid(x, shift_x=0.0, shift_y=0.0, scale_x=1.0, scale_y=1.0):
+    y = 1 / (1 + np.exp(-(scale_x * x + shift_x))) * scale_y + shift_y
+    return y
+
+
+# homogeneous_vector = lambda P: np.append(P,1)
+def homogeneous_vector(P):
+    return np.hstack((P, 1))
+
+
+# homogeneous_matrix = lambda rot=np.identity(3), pos=np.zeros(3): np.vstack((np.append(rot[0, :], pos[0]),
+# np.append(rot[1, :], pos[1]), np.append(rot[2, :], pos[2]), np.array([0, 0, 0, 1])))
+def homogeneous_matrix(rot=np.identity(3), pos=np.zeros(3)):
+    transform_matrix = np.identity(4)
+    transform_matrix[:3, :3] = rot[:3, :3]
+    transform_matrix[:3, -1] = pos[:3]
+    return transform_matrix
+
+
+def sinwave(t, y_ini=0.0, y_max=1.0, y_min=-1.0, T=1):
+    median = (y_max + y_min) / 2.0
+    A = np.abs(y_max - y_min) / 2.0
+    f = 1.0 / T
+    phase = np.math.asin(np.clip((y_ini - median) / A, -1, 1))
+    pos_t = A * np.sin(2 * np.pi * f * t + phase) + median
+    vel_t = A * np.cos(2 * np.pi * f * t + phase) * 2 * np.pi * f
+    acc_t = - A * np.sin(2 * np.pi * f * t + phase) * (2 * np.pi * f) ** 2
+    return pos_t, vel_t, acc_t
+
+
+class SineWave(object):
+    def __init__(self, y_ini=0.0, y_max=1.0, y_min=-1.0, T=1):
+        """
+        sine wave is defined in the form:
+            y(t) = A*sin(2*pi*f*t + phi) = A*sin(w*t + phi)
+        A = the amplitude, the peak deviation of the function from zero.
+        f = the ordinary frequency, the number of oscillations (cycles) that occur each second of time.
+        w = 2*pi*f, the angular frequency, the rate of change of the function argument in units of radians per second
+        phi = the phase, specifies (in radians) where in its cycle the oscillation is at t = 0.
+        :param y_ini: the initial value of the sine wave
+        :param y_max: the maximum value of the sine wave
+        :param y_min: the minimum value of the sine wave
+        :param T: cycle time
+        """
+        if y_ini < y_min or y_ini > y_max:
+            print 'Error: the initial value should be between y_min and y_max values!'
+        elif y_max < y_min:
+            print 'Error: please change the order of inputs y_max and y_min!'
+
+        self.median = (y_max + y_min) / 2.0
+        self.A = np.abs(y_max - y_min) / 2.0
+        self.f = 1.0 / T
+        self.phase = np.math.asin(np.clip((y_ini - self.median) / self.A, -1, 1))
+
+    def __call__(self, t):
+        return self.A * np.sin(2 * np.pi * self.f * t + self.phase) + self.median
+
+    def pos(self, t):
+        return self.A * np.sin(2 * np.pi * self.f * t + self.phase) + self.median
+
+    def vel(self, t):
+        return self.A * np.cos(2 * np.pi * self.f * t + self.phase) * 2 * np.pi * self.f
+
+    def acc(self, t):
+        return - self.A * np.sin(2 * np.pi * self.f * t + self.phase) * (2 * np.pi * self.f) ** 2

pyrobolearn/controllers/locomotion/utils/robot_param.py

@@ -0,0 +1,47 @@
+import numpy as np
+from urdf_parser_py.urdf import URDF
+
+
+class RobotParam:
+    """
+    unique params associated with a specific robot.
+    """
+    def __init__(self, urdf_path, mu=0.7):
+
+        # parse urdf
+        urdf_model = URDF.from_xml_file(urdf_path)
+
+        lfoot_link_name = "LFoot"
+        rfoot_link_name = "RFoot"
+        lfoot_link = [link for link in urdf_model.links if link.name == lfoot_link_name][0]
+        rfoot_link = [link for link in urdf_model.links if link.name == rfoot_link_name][0]
+
+        self.foot_size = lfoot_link.collision.geometry.size
+        self.foot_sole_position = np.array([lfoot_link.collision.origin.xyz[0],
+                                            lfoot_link.collision.origin.xyz[1],
+                                            lfoot_link.collision.origin.xyz[2]-self.foot_size[2]/2.0])
+        print("foot_size: ", self.foot_size)
+        print("foot_sole_position: ", self.foot_sole_position)
+
+        # zmp constraints
+        dx_min, dx_max = -self.foot_size[0] / 2.0, self.foot_size[0] / 2.0
+        dy_min, dy_max = -self.foot_size[1] / 2.0, self.foot_size[1] / 2.0
+        self.zmpConsSpatialForce = np.array([[-1, 0, 0, 0, 0, dy_min],
+                                             [1, 0, 0, 0, 0, -dy_max],
+                                             [0, 1, 0, 0, 0, dx_min],
+                                             [0, -1, 0, 0, 0, -dx_max]])
+
+        # friction constraints
+        self.mu = mu
+        self.frictionConsSpatialForce = np.array([[0, 0, 0, 1, 0, -self.mu],
+                                                  [0, 0, 0, -1, 0, -self.mu],
+                                                  [0, 0, 0, 0, 1, -self.mu],
+                                                  [0, 0, 0, 0, -1, -self.mu]])
+
+        # unilateral constraints
+        self.unilateralConsSpatialForce = np.array([[0, 0, 0, 0, 0, -1]])
+
+        # GRF constraints: zmp constraints + friction constraints + unilateral constraints
+        self.SpatialForceCons = np.vstack((self.zmpConsSpatialForce,
+                                           self.frictionConsSpatialForce,
+                                           self.unilateralConsSpatialForce))

pyrobolearn/controllers/locomotion/utils/task.py

@@ -0,0 +1,14 @@
+
+class Task:
+    def __init__(self, A, b, w=1):
+        self.A = A  # matrix
+        self.b = b  # vector
+        self.w = w  # float
+
+    def set_weight(self, w):
+        self.w = w
+
+    def show(self):
+        print "A", self.A.shape, ":", "\n", self.A
+        print "b", self.b.shape, ":", "\n", self.b
+        print "w:", self.w