Commit d17547ab authored by Galo Maldonado's avatar Galo Maldonado
Browse files

update

parent 99f36c37
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])
This diff is collapsed.
......@@ -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);
......
......@@ -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')
......
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);
......@@ -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"
......@@ -74,6 +75,10 @@ class SE3Task(Task):
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,17 +143,17 @@ 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):
......@@ -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]
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 =
......
......@@ -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);
......@@ -86,8 +86,6 @@ class Wrapper():
self.biais(self.q, self.v)
self.q = q.copy()
def parseTrial(self, data):
q=[]
for i in range(0, len(data)):
......@@ -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,14 +280,22 @@ 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)
......@@ -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:
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment