update

hqp/#algebra.py#

+def quaternion_from_matrix(matrix, isprecise=False):
+    """Return quaternion from rotation matrix.                                                                   
+                                                                                                                 
+    If isprecise is True, the input matrix is assumed to be a precise rotation                                   
+    matrix and a faster algorithm is used.                                                                       
+                                                                                                                 
+    >>> q = quaternion_from_matrix(np.identity(4), True)                                                         
+    >>> np.allclose(q, [1, 0, 0, 0])                                                                             
+    True                                                                                                         
+    >>> q = quaternion_from_matrix(np.diag([1, -1, -1, 1]))                                                      
+    >>> np.allclose(q, [0, 1, 0, 0]) or np.allclose(q, [0, -1, 0, 0])                                            
+    True                                                                                                         
+    >>> R = rotation_matrix(0.123, (1, 2, 3))                                                                    
+    >>> q = quaternion_from_matrix(R, True)                                                                      
+    >>> np.allclose(q, [0.9981095, 0.0164262, 0.0328524, 0.0492786])                                             
+    True                                                                                                         
+    >>> R = [[-0.545, 0.797, 0.260, 0], [0.733, 0.603, -0.313, 0],                                               
+    ...      [-0.407, 0.021, -0.913, 0], [0, 0, 0, 1]]                                                           
+    >>> q = quaternion_from_matrix(R)                                                                            
+    >>> np.allclose(q, [0.19069, 0.43736, 0.87485, -0.083611])                                                   
+    True                                                                                                         
+    >>> R = [[0.395, 0.362, 0.843, 0], [-0.626, 0.796, -0.056, 0],                                               
+    ...      [-0.677, -0.498, 0.529, 0], [0, 0, 0, 1]]                                                           
+    >>> q = quaternion_from_matrix(R)                                                                            
+    >>> np.allclose(q, [0.82336615, -0.13610694, 0.46344705, -0.29792603])                                       

hqp/inv_dyn_formulation_util.py

@@ -2,7 +2,7 @@ import numpy as np
 from numpy.linalg import norm
 from numpy.random import random
 from polytope_conversion_utils import cone_span_to_face
-from wrapper import Wrapper
+from hqp.wrapper import Wrapper
 import pinocchio as se3
 from pinocchio.utils import zero as zeros
 from acc_bounds_util_multi_dof import computeAccLimits
@@ -195,9 +195,9 @@ class InvDynFormulation (object):
     
     def __init__(self, name, q, v, dt, mesh_dir, urdfFileName, freeFlyer=True):
         if(freeFlyer):
-            self.r = RobotWrapper(urdfFileName, mesh_dir, root_joint=se3.JointModelFreeFlyer());
+            self.r = Wrapper(urdfFileName, mesh_dir,'robot',True)# root_joint=se3.JointModelFreeFlyer());
         else:
-            self.r = RobotWrapper(urdfFileName, mesh_dir, None);
+            self.r = Wrapper(urdfFileName, mesh_dir, None);
         self.freeFlyer = freeFlyer;
         self.nq = self.r.nq;
         self.nv = self.r.nv;
@@ -404,7 +404,8 @@ class InvDynFormulation (object):
         self.setPositions(q, updateConstraintReference=False);
         self.setVelocities(v);
         
-        self.r.computeAllTerms(q, v);
+        #self.r.computeAllTerms(q, v);
+        self.r.update(q);
         self.r.framesKinematics(q);
         self.x_com    = self.r.com(q, update_kinematics=False);
         self.J_com    = self.r.Jcom(q, update_kinematics=False);

hqp/simulator.py

@@ -11,6 +11,105 @@ class Simulator(object):
     COM_SPHERE_RADIUS = 0.01
     COM_SPHERE_COLOR = (1, 0, 0, 1)
 
+    '''
+    name = ''
+    q = None;   # current positions
+    v = None;   # current velocities
+
+    LCP = None;   # approximate LCP solver using staggered projections
+    
+    USE_LCP_SOLVER          = True;
+    DETECT_CONTACT_POINTS   = True;   
+    #'' True: detect collisions between feet and ground, False: collision is specified by the user ''
+    GROUND_HEIGHT           = 0.0;
+    LOW_PASS_FILTER_INPUT_TORQUES = False;
+    
+    ENABLE_FORCE_LIMITS = True;
+    ENABLE_TORQUE_LIMITS = True;
+    ENABLE_JOINT_LIMITS = True;
+    
+    ACCOUNT_FOR_ROTOR_INERTIAS = False;
+
+    VIEWER_DT = 0.05;
+    DISPLAY_COM = True;
+    DISPLAY_CAPTURE_POINT = True;
+    COM_SPHERE_RADIUS           = 0.01;
+    CAPTURE_POINT_SPHERE_RADIUS = 0.01;
+    CONTACT_FORCE_ARROW_RADIUS  = 0.01;
+    COM_SPHERE_COLOR            = (1, 0, 0, 1); # red, green, blue, alpha
+    CAPTURE_POINT_SPHERE_COLOR  = (0, 1, 0, 1);    
+    CONTACT_FORCE_ARROW_COLOR   = (1, 0, 0, 1);
+    CONTACT_FORCE_ARROW_SCALE   = 1e-3;
+    contact_force_arrow_names = [];  # list of names of contact force arrows
+    
+    SLIP_VEL_THR = 0.1;
+    SLIP_ANGVEL_THR = 0.2;
+    NORMAL_FORCE_THR = 5.0;
+    JOINT_LIMITS_DQ_THR = 1e-1; #1.0;
+    TORQUE_VIOLATION_THR = 1.0;
+    DQ_MAX = 9.14286;
+                       
+    ENABLE_WALL_DRILL_CONTACT = False;
+    wall_x = 0.5;
+    wall_damping = np.array([30, 30, 30, 0.3, 0.3, 0.3]);
+    
+    k=0;    # number of contact constraints (i.e. size of contact force vector)
+    na=0;   # number of actuated DoFs
+    nq=0;   # number of position DoFs
+    nv=0;   # number of velocity DoFs
+    r=[];   # robot
+    
+    mu=[];          # friction coefficient (force, moment)
+    fMin = 0;       # minimum normal force
+
+    dt = 0;     # time step used to compute joint acceleration bounds
+    qMin = [];  # joint lower bounds
+    qMax = [];  # joint upper bounds
+    
+    #'' Mapping between y and tau: y = C*tau+c ''
+    C = [];
+    c = [];
+    
+    M = [];         # mass matrix
+    Md = [];        # rotor inertia
+    h = [];         # dynamic drift
+    q = [];
+    dq = [];
+
+    x_com = [];     # com 3d position
+    dx_com = [];    # com 3d velocity
+    ddx_com = [];   # com 3d acceleration
+    cp = None;      # capture point
+#    H_lankle = [];  # left ankle homogeneous matrix
+#    H_rankle = [];  # right ankle homogeneous matrix
+    J_com = [];     # com Jacobian
+    Jc = [];        # contact Jacobian
+    
+    Minv = [];      # inverse of the mass matrix
+    Jc_Minv = [];   # Jc*Minv
+    Lambda_c = [];  # task-space mass matrix (Jc*Minv*Jc^T)^-1
+    Jc_T_pinv = []; # Lambda_c*Jc_Minv
+    Nc_T = [];      # I - Jc^T*Jc_T_pinv
+    S_T = [];       # selection matrix
+    dJc_v = [];     # product of contact Jacobian time derivative and velocity vector: dJc*v
+    
+    candidateContactConstraints = [];
+    rigidContactConstraints = [];
+#    constr_rfoot = None;
+#    constr_lfoot = None;
+#    constr_rhand = None;
+    
+#    f_rh = [];      # force right hand
+#    w_rf = [];      # wrench right foot
+#    w_lf = [];      # wrench left foot
+    
+    #'' debug variables ''
+    x_c = [];       # contact points
+    dx_c = [];      # contact points velocities
+    x_c_init = [];  # initial position of constrained bodies    
+        
+    viewer = None;
+    '''
     def reset(self, t, q, v, dt, nv):
         n = nv
         self.robot.dt = 0.0025 #dt
@@ -31,9 +130,11 @@ class Simulator(object):
         self.viewer = Viewer(self.robot.name, self.robot)
         self.updateRobotConfig(self.robot.q0)
         self.viewer.display(self.robot.q0,robot.name)
+        #self.fMin = 0.001 
+        #self.mu = np.array([ 0.3,  0.1])
         #self.nq = self.robot.nq
         #self.nv = self.robot.nv
-        #self.na = self.nv-6        
+        #self.na = self.nv        
 
         #if(self.DISPLAYCOM):
         #    self.viewer.addSphere('com', self.COM_SPHERE_RADIUS, mat_zeros(3), mat_zeros(3), self.COM_SPHERE_COLOR, 'OFF')

hqp/solvers.py

 import numpy as np
 from wrapper import Wrapper
 import scipy
-
+from abstract_solver import AbstractSolver
 
 class NProjections():
     def reset(self,q,v,dt):
@@ -166,3 +166,34 @@ class NProjections():
         Z += self.null(Jstack[k]/robot.M) * Z[k-1]
         return Z
 
+
+
+
+class StandardQpSolver (AbstractSolver):
+    """
+    Nonrobust inverse dynamics solver for the problem:
+    min ||D*x - d||^2
+    s.t.  lbA <= A*x <= ubA 
+    """
+
+    def __init__(self, n, m_in, solver='slsqp', accuracy=1e-6, maxIter=100, verb=0):
+        AbstractSolver.__init__(self, n, m_in, solver, accuracy, maxIter, verb);
+        self.name = "Classic TSID";
+        
+    def f_cost(self,x):
+        e = np.dot(self.D, x) - self.d;
+        return 0.5*np.dot(e.T,e);
+    
+    def f_cost_grad(self,x):
+        return np.dot(self.H,x) - self.dD;
+        
+    def f_cost_hess(self,x):
+        return self.H;
+
+    def get_linear_inequality_matrix(self):
+        return self.A;
+          
+    def get_linear_inequality_vectors(self):
+        return (self.lbA, self.ubA);
+
+

hqp/tasks.py

@@ -60,6 +60,7 @@ class SE3Task(Task):
         self._mask = (np.ones(6)).astype(bool)
         # for local to global
         self._gMl = SE3.Identity()
+        self.__gain_matrix = np.matrix(np.eye(robot.nv))
 
     def mask(self, mask):
         assert len(mask) == 6, "The mask must have 6 elemets"
@@ -73,6 +74,10 @@ class SE3Task(Task):
     @property
     def refTrajectory(self):
         return self._ref_trajectory
+        
+    def setGain(self, gain_vector):
+        assert gain_vector.shape == (self.robot.nv,)         
+        self.__gain_matrix = np.matrix(np.diag(gain_vector))
 
     def jointPostition(self):
         return robot.position(robot.q, joint_id)
@@ -138,19 +143,19 @@ class SE3Task(Task):
         
         #_ Taks functions:
         # Compute error acceleration desired
-        p_error= errorInSE3(oMi, M_ref);
-        v_error = v_frame - self._gMl.actInv(v_ref)
+        self.p_error= errorInSE3(oMi, M_ref);
+        self.v_error = v_frame - self._gMl.actInv(v_ref)
         drift = self.robot.frameAcceleration(self._frame_id)
         drift.linear += np.cross(v_frame.angular.T, v_frame.linear.T).T    
         
         # porportional derivative task
-        if self.expDecay is True:
-            self.kv = exponentialDecay(self.kp)
-        if self.adaptGain is True:
-            self.kp = adaptativeGain(p_error.vector, self.kmin, self.kmax, self.beta)
-        a_des = - self.kp * p_error.vector - self.kv * v_error.vector + self._gMl.actInv(a_ref).vector #sign +-
+        #if self.expDecay is True:
+        #    self.kv = exponentialDecay(self.kp)
+        #if self.adaptGain is True:
+        #    self.kp = adaptativeGain(p_error.vector, self.kmin, self.kmax, self.beta)
+        a_des = self._gMl.actInv(a_ref).vector - (self.kp*self.p_error.vector + self.kv*self.v_error.vector) 
         J = self.robot.frameJacobian(q, self._frame_id, False)
-    
+        
         if(local_frame==False):
             drift = self._gMl.act(drift);
             a_des[:3] = self._gMl.rotation * a_des[:3];
@@ -158,10 +163,7 @@ class SE3Task(Task):
             J[:3,:] = self._gMl.rotation * J[:3,:];
             J[3:,:] = self._gMl.rotation * J[3:,:];
 
-        self.p_error = p_error.vector
-        self.v_error = p_error.vector
-
-        return J[self._mask,:], drift.vector[self._mask], a_des[self._mask]
+        return J[self._mask,:]*self.__gain_matrix, drift.vector[self._mask], a_des[self._mask]
 
 
     def jacobian(self, q, update_geometry = False):
@@ -178,6 +180,7 @@ class CoMTask(Task):
         self._ref_trajectory = ref_trajectory
         # mask over the desired euclidian axis
         self._mask = (np.ones(3)).astype(bool)
+        self.__gain_matrix = np.matrix(np.eye(robot.nv))
     
     @property
     def dim(self):
@@ -190,6 +193,10 @@ class CoMTask(Task):
         assert len(mask) == 3, "The mask must have 3 elements"
         self._mask = mask.astype(bool)
 
+    def setGain(self, gain_vector):
+        assert gain_vector.shape == (self.robot.nv,)         
+        self.__gain_matrix = np.matrix(np.diag(gain_vector))
+
     def dyn_value(self, t, q, v, update_geometry = False):
         # Get the current CoM position, velocity and acceleration
         p_com, v_com, a_com = self.robot.com(q,v,0*v)
@@ -208,7 +215,7 @@ class CoMTask(Task):
         a_des = -(self.kp * self.p_error + self.kv * self.v_error) + a_ref
 
         # Compute jacobian
-        J = self.robot.Jcom(q)
+        J = self.robot.Jcom(q)*self.__gain_matrix
         
         return J[self._mask,:], drift[self._mask], a_des[self._mask]
 
@@ -380,38 +387,98 @@ class MomentumTask(Task):
         self.__gain_matrix = np.matrix(np.diag(gain_vector))
         #self.__gain_matrix = np.matrix(np.matlib.repmat(gain,1,42))
 
-    def dyn_value(self, t, q, v):
-        (hg_ref, vhg_ref, ahg_ref) = self._ref_traj(t)
+    def _getBiais(self,q,v):
         model = self.robot.model
-        data = self.robot.data 
-        JMom = se3.ccrba(model, data, q, v)
-        hg_act =  self.robot.data.hg.np.A.copy()
-        self.p_error = hg_act[self._mask,:] - hg_ref[self._mask,:]
-        self.v_error = self.robot.fext[self._mask,:] - vhg_ref[self._mask,:]
-        #***********************
-        p_com = data.com[0]
-        cXi = SE3.Identity()
-        oXi = self.robot.data.oMi[1]
+        data = self.robot.data
+        p_com = self.robot.com(q)
+
+        oXi = data.oMi[1]
+        cXi = se3.SE3.Identity()
         cXi.rotation = oXi.rotation
         cXi.translation = oXi.translation - p_com
+        #cXi.translation = p_com
+
         m_gravity = model.gravity.copy()
         model.gravity.setZero()
         b = se3.nle(model,data,q,v)
         model.gravity = m_gravity
         f_ff = se3.Force(b[:6])
         f_com = cXi.act(f_ff)
-        hg_drift = f_com.angular 
-        self.drift=f_com.np[self._mask,:]
-        #drift = np.matrix(np.zeros((3, 1)))
-        #a_tot = Ldot_des - hg_drift 
-        #************************
-        self.a_des = -(self.kp * self.p_error) 
-        #self.a_des = -(self.kp * self.p_error) + (self.kv*v_error) + ahg_ref
-        #self.drift = 0*self.a_des
-        #print JMom.copy()[self._mask,:].shape
-        #print self.__gain_matrix.shape
+        return f_com.np
+
+    def _getContribution(self,com,s):
+        ''' 
+        Get segment contribution to centroidal angular
+        momenta and its rate of change 
+        Inputs:                                                                           
+        - s : segment index  
+        - com : position of the CoM in world reference frame 
+        '''
+        # get spatial inertia and velocity
+        Y = self.robot.model.inertias[s]
+        V = self.robot.data.v[s]
+        # centroidal momenta in body frame 
+        # ihi = I Vi
+        iHi = se3.Force(Y.matrix()*V.vector)
+        # rate of change of centroidal momenta in body frame 
+        # ih_doti = Iai + Vi x* hi  
+        # TO VERIFY  
+        iHDi = (self.robot.model.inertias[s]*self.robot.data.a[s]) + se3.Inertia.vxiv(Y,V)
+        #iHDi.linear  
+        # express at the center of mass 
+        oMc = se3.SE3.Identity()
+        oMc.translation = self.robot.data.oMi[1].translation - com
+        # uncomment in case need to change the rotation of the reference frame 
+        oMc.rotation = self.robot.data.oMi[1].rotation 
+        cMi = oMc.actInv( self.robot.data.oMi[s] )
+        cHi = cMi.act(iHi).np.A1
+        cHDi = cMi.act(iHDi).np.A1
+        return cHi, cHDi
+
+    def _calculateHdot(self, q):
+        # Individual contributions to centroidal momenta (and its rate of change)                            
+        segH = []; segF=[]
+        p_com = self.robot.com(q)
+        for s in range(1,26):
+            hc, hcd = self._getContribution(p_com,s)
+            segH.append(hc)
+            segF.append(hcd)
+        return np.matrix(np.sum(np.array(segF).squeeze(),0)).T
+
+
+    def dyn_value_h(self, t, q, v):
+        (self.hg_ref, self.dhg_ref, aahg_ref) = self._ref_traj(t)
+        JMom = se3.ccrba(self.robot.model, self.robot.data, q, v)
+        b = self._getBiais(q,v)
+        self.hg_act =  self.robot.data.hg.np.A.copy()
+        self.dhg_act = self._calculateHdot(q)
+        
+        self.drift=b[self._mask,:]
+        
+        self.p_error = self.hg_act[self._mask,:] - self.hg_ref[self._mask,:]        
+        self.v_error = self.dhg_act[self._mask,:] - self.dhg_ref[self._mask,:]
+        #self.dh_des =  -( (self.kp * self.p_error) + (self.kv*self.v_error) )
+        self.dh_des = - (self.kp * self.p_error)
         self._jacobian = JMom.copy()[self._mask,:] * self.__gain_matrix
-        return self._jacobian, self.drift, self.a_des
+        #self.drift = 0*self.dh_des
+        return self._jacobian, self.drift, self.dh_des
+
+    def dyn_value(self, t, q, v):
+        (self.hg_ref, self.dhg_ref, aahg_ref) = self._ref_traj(t)
+        JMom = se3.ccrba(self.robot.model, self.robot.data, q, v)
+        b = self._getBiais(q,v)
+        self.hg_act =  self.robot.data.hg.np.A.copy()
+        self.dhg_act = self._calculateHdot(q)
+        
+        self.drift=b[self._mask,:]
+        
+        self.p_error = self.hg_act[self._mask,:] - self.hg_ref[self._mask,:]        
+        self.v_error = self.dhg_act[self._mask,:] - self.dhg_ref[self._mask,:]
+        self.dh_des =  - ((self.kp * self.p_error) + (self.kv*self.v_error) )
+        self.h_des = - (self.kp * self.p_error)
+        self._jacobian = JMom.copy()[self._mask,:] * self.__gain_matrix
+        #self.drift = 0*self.dh_des
+        return self._jacobian, self.drift, self.h_des
 
 
 ''' Define Fly Momentum Task '''
@@ -454,6 +521,7 @@ class FlyMomentumTask(Task):
         #self.v_error = self.robot.fext[self._mask,:] 
         #self.v_error = data.hg.vector.copy()[self._mask,:] - vhg_ref[self._mask,:]
         #self.a_des   = -self.kv*self.v_error #+model.gravity.vector[self._mask,:]
+
         self.a_des   = self.kv*self.robot.fext[self._mask,:]#vhg_ref #+model.gravity.vector[self._mask,:]
         #self.drift = 0 * self.a_des
         #self.a_des   = 

hqp/trajectories.py

@@ -44,7 +44,7 @@ class VaryingNdTrajectory (object):
     assert t>=0.0, "Time must be non-negative"
     i = int(t/self._dt);
     if(i>=self._x_ref.shape[1]):
-       raise ValueError("Specified time exceeds the duration of the trajectory: "+str(t))
+      raise ValueError("Specified time exceeds the duration of the trajectory: "+str(t))
     return (self._x_ref[:,i], self._v_ref[:,i], self._a_ref[:,i]);
 
 
@@ -159,3 +159,27 @@ class SmoothedSE3Trajectory (object):
     v.linear = self._v_ref[:,i];
     a.linear = self._a_ref[:,i];
     return (M, v, a);
+
+
+''' An Nd trajectory with constant state and velocity and zero acceleration. '''
+class ConstantNdTrajectoryXV (object):
+
+  def __init__ (self, name, x_ref, v_ref):
+    self._name = name
+    self._dim = x_ref.shape[0]
+    self._x_ref = np.matrix.copy(x_ref);
+    self._v_ref = np.matrix.copy(v_ref);
+    self._a_ref = np.zeros(x_ref.shape);
+
+  @property
+  def dim(self):
+    return self._dim
+    
+  def setReference(self, x_ref):
+    assert x_ref.shape[0]==self._x_ref.shape[0]
+    self._x_ref = x_ref;
+    self._v_ref = v_ref;
+
+  def __call__ (self, t):
+    return (self._x_ref, self._v_ref, self._a_ref);
+

hqp/wrapper.py

@@ -84,9 +84,7 @@ class Wrapper():
         se3.computeJacobians(self.model, self.data, q)
         se3.rnea(self.model, self.data, q, self.v, self.a)
         self.biais(self.q, self.v)
-        self.q = q.copy()
-        
-        
+        self.q = q.copy()        
 
     def parseTrial(self, data):
         q=[]
@@ -151,6 +149,11 @@ class Wrapper():
         ''' the coriolis, centrifugal and gravitational effects '''
         return se3.nle(self.model, self.data, q, v)
 
+    def bias(self, q, v, update_kinematics=True):
+        if(update_kinematics):
+            return se3.nle(self.model, self.data, q, v)
+        return self.data.nle
+
     def generalizedAcceleration(self, V, dt):
         return np.asmatrix(np.gradient(V, dt))
 
@@ -277,15 +280,23 @@ class Wrapper():
     def com(self, q, v=None, a=None, update_kinematics=True):
         if v is not None:
             if a is None:
-                se3.centerOfMass(self.model, self.data, q, v, update_kinematics)
+                se3.centerOfMass(self.model, self.data, q, v)#, update_kinematics)
                 return self.data.com[0], self.data.vcom[0]
-            se3.centerOfMass(self.model, self.data, q, v, a, update_kinematics)
+            se3.centerOfMass(self.model, self.data, q, v, a)#, update_kinematics)
             return self.data.com[0], self.data.vcom[0], self.data.acom[0]
-        return se3.centerOfMass(self.model, self.data, q, update_kinematics)
+        return se3.centerOfMass(self.model, self.data, q)#, update_kinematics)
     
-    def Jcom(self, q):
+    def Jcom(self, q): #, update_kinematics=True):
         return se3.jacobianCenterOfMass(self.model, self.data, q)
+        #if(update_kinematics):
+        #    return se3.jacobianCenterOfMass(self.model, self.data, q)
+        #return self.data.Jcom
     
+    def mass(self, q, update_kinematics=True):
+        if(update_kinematics):
+            return se3.crba(self.model, self.data, q)
+        return self.data.M
+
     def getDoF(self, jointName):
         idx = self.model.getJointId(jointName)
         if idx < len (self.model.joints):
@@ -304,6 +315,12 @@ class Wrapper():
     def computeJacobians(self, q):
         return se3.computeJacobians(self.model, self.data, q)
 
+    ''' Compute the placements of all the operational frames and put the results in data.
+        To be called after forwardKinematics.
+    '''
+    def framesKinematics(self, q):
+        se3.framesKinematics(self.model, self.data, q)
+
     def framePosition(self, index, q=None):
         f = self.model.frames[index]
         if q is not None: