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 ...@@ -2,7 +2,7 @@ import numpy as np
from numpy.linalg import norm from numpy.linalg import norm
from numpy.random import random from numpy.random import random
from polytope_conversion_utils import cone_span_to_face from polytope_conversion_utils import cone_span_to_face
from wrapper import Wrapper from hqp.wrapper import Wrapper
import pinocchio as se3 import pinocchio as se3
from pinocchio.utils import zero as zeros from pinocchio.utils import zero as zeros
from acc_bounds_util_multi_dof import computeAccLimits from acc_bounds_util_multi_dof import computeAccLimits
...@@ -195,9 +195,9 @@ class InvDynFormulation (object): ...@@ -195,9 +195,9 @@ class InvDynFormulation (object):
def __init__(self, name, q, v, dt, mesh_dir, urdfFileName, freeFlyer=True): def __init__(self, name, q, v, dt, mesh_dir, urdfFileName, freeFlyer=True):
if(freeFlyer): 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: else:
self.r = RobotWrapper(urdfFileName, mesh_dir, None); self.r = Wrapper(urdfFileName, mesh_dir, None);
self.freeFlyer = freeFlyer; self.freeFlyer = freeFlyer;
self.nq = self.r.nq; self.nq = self.r.nq;
self.nv = self.r.nv; self.nv = self.r.nv;
...@@ -404,7 +404,8 @@ class InvDynFormulation (object): ...@@ -404,7 +404,8 @@ class InvDynFormulation (object):
self.setPositions(q, updateConstraintReference=False); self.setPositions(q, updateConstraintReference=False);
self.setVelocities(v); self.setVelocities(v);
self.r.computeAllTerms(q, v); #self.r.computeAllTerms(q, v);
self.r.update(q);
self.r.framesKinematics(q); self.r.framesKinematics(q);
self.x_com = self.r.com(q, update_kinematics=False); self.x_com = self.r.com(q, update_kinematics=False);
self.J_com = self.r.Jcom(q, update_kinematics=False); self.J_com = self.r.Jcom(q, update_kinematics=False);
......
...@@ -11,6 +11,105 @@ class Simulator(object): ...@@ -11,6 +11,105 @@ class Simulator(object):
COM_SPHERE_RADIUS = 0.01 COM_SPHERE_RADIUS = 0.01
COM_SPHERE_COLOR = (1, 0, 0, 1) 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): def reset(self, t, q, v, dt, nv):
n = nv n = nv
self.robot.dt = 0.0025 #dt self.robot.dt = 0.0025 #dt
...@@ -31,9 +130,11 @@ class Simulator(object): ...@@ -31,9 +130,11 @@ class Simulator(object):
self.viewer = Viewer(self.robot.name, self.robot) self.viewer = Viewer(self.robot.name, self.robot)
self.updateRobotConfig(self.robot.q0) self.updateRobotConfig(self.robot.q0)
self.viewer.display(self.robot.q0,robot.name) 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.nq = self.robot.nq
#self.nv = self.robot.nv #self.nv = self.robot.nv
#self.na = self.nv-6 #self.na = self.nv
#if(self.DISPLAYCOM): #if(self.DISPLAYCOM):
# self.viewer.addSphere('com', self.COM_SPHERE_RADIUS, mat_zeros(3), mat_zeros(3), self.COM_SPHERE_COLOR, 'OFF') # self.viewer.addSphere('com', self.COM_SPHERE_RADIUS, mat_zeros(3), mat_zeros(3), self.COM_SPHERE_COLOR, 'OFF')
......
import numpy as np import numpy as np
from wrapper import Wrapper from wrapper import Wrapper
import scipy import scipy
from abstract_solver import AbstractSolver
class NProjections(): class NProjections():
def reset(self,q,v,dt): def reset(self,q,v,dt):
...@@ -166,3 +166,34 @@ class NProjections(): ...@@ -166,3 +166,34 @@ class NProjections():
Z += self.null(Jstack[k]/robot.M) * Z[k-1] Z += self.null(Jstack[k]/robot.M) * Z[k-1]
return Z 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): ...@@ -60,6 +60,7 @@ class SE3Task(Task):
self._mask = (np.ones(6)).astype(bool) self._mask = (np.ones(6)).astype(bool)
# for local to global # for local to global
self._gMl = SE3.Identity() self._gMl = SE3.Identity()
self.__gain_matrix = np.matrix(np.eye(robot.nv))
def mask(self, mask): def mask(self, mask):
assert len(mask) == 6, "The mask must have 6 elemets" assert len(mask) == 6, "The mask must have 6 elemets"
...@@ -74,6 +75,10 @@ class SE3Task(Task): ...@@ -74,6 +75,10 @@ class SE3Task(Task):
def refTrajectory(self): def refTrajectory(self):
return self._ref_trajectory 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): def jointPostition(self):
return robot.position(robot.q, joint_id) return robot.position(robot.q, joint_id)
...@@ -138,17 +143,17 @@ class SE3Task(Task): ...@@ -138,17 +143,17 @@ class SE3Task(Task):
#_ Taks functions: #_ Taks functions:
# Compute error acceleration desired # Compute error acceleration desired
p_error= errorInSE3(oMi, M_ref); self.p_error= errorInSE3(oMi, M_ref);
v_error = v_frame - self._gMl.actInv(v_ref) self.v_error = v_frame - self._gMl.actInv(v_ref)
drift = self.robot.frameAcceleration(self._frame_id) drift = self.robot.frameAcceleration(self._frame_id)
drift.linear += np.cross(v_frame.angular.T, v_frame.linear.T).T drift.linear += np.cross(v_frame.angular.T, v_frame.linear.T).T
# porportional derivative task # porportional derivative task
if self.expDecay is True: #if self.expDecay is True:
self.kv = exponentialDecay(self.kp) # self.kv = exponentialDecay(self.kp)
if self.adaptGain is True: #if self.adaptGain is True:
self.kp = adaptativeGain(p_error.vector, self.kmin, self.kmax, self.beta) # 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 +- 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) J = self.robot.frameJacobian(q, self._frame_id, False)
if(local_frame==False): if(local_frame==False):
...@@ -158,10 +163,7 @@ class SE3Task(Task): ...@@ -158,10 +163,7 @@ class SE3Task(Task):
J[:3,:] = self._gMl.rotation * J[:3,:]; J[:3,:] = self._gMl.rotation * J[:3,:];
J[3:,:] = self._gMl.rotation * J[3:,:]; J[3:,:] = self._gMl.rotation * J[3:,:];
self.p_error = p_error.vector return J[self._mask,:]*self.__gain_matrix, drift.vector[self._mask], a_des[self._mask]
self.v_error = p_error.vector
return J[self._mask,:], drift.vector[self._mask], a_des[self._mask]
def jacobian(self, q, update_geometry = False): def jacobian(self, q, update_geometry = False):
...@@ -178,6 +180,7 @@ class CoMTask(Task): ...@@ -178,6 +180,7 @@ class CoMTask(Task):
self._ref_trajectory = ref_trajectory self._ref_trajectory = ref_trajectory
# mask over the desired euclidian axis # mask over the desired euclidian axis
self._mask = (np.ones(3)).astype(bool) self._mask = (np.ones(3)).astype(bool)
self.__gain_matrix = np.matrix(np.eye(robot.nv))
@property @property
def dim(self): def dim(self):
...@@ -190,6 +193,10 @@ class CoMTask(Task): ...@@ -190,6 +193,10 @@ class CoMTask(Task):
assert len(mask) == 3, "The mask must have 3 elements" assert len(mask) == 3, "The mask must have 3 elements"
self._mask = mask.astype(bool) 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): def dyn_value(self, t, q, v, update_geometry = False):
# Get the current CoM position, velocity and acceleration # Get the current CoM position, velocity and acceleration
p_com, v_com, a_com = self.robot.com(q,v,0*v) p_com, v_com, a_com = self.robot.com(q,v,0*v)
...@@ -208,7 +215,7 @@ class CoMTask(Task): ...@@ -208,7 +215,7 @@ class CoMTask(Task):
a_des = -(self.kp * self.p_error + self.kv * self.v_error) + a_ref a_des = -(self.kp * self.p_error + self.kv * self.v_error) + a_ref
# Compute jacobian # 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] return J[self._mask,:], drift[self._mask], a_des[self._mask]
...@@ -380,38 +387,98 @@ class MomentumTask(Task): ...@@ -380,38 +387,98 @@ class MomentumTask(Task):
self.__gain_matrix = np.matrix(np.diag(gain_vector)) self.__gain_matrix = np.matrix(np.diag(gain_vector))
#self.__gain_matrix = np.matrix(np.matlib.repmat(gain,1,42)) #self.__gain_matrix = np.matrix(np.matlib.repmat(gain,1,42))
def dyn_value(self, t, q, v): def _getBiais(self,q,v):
(hg_ref, vhg_ref, ahg_ref) = self._ref_traj(t)
model = self.robot.model model = self.robot.model
data = self.robot.data data = self.robot.data
JMom = se3.ccrba(model, data, q, v) p_com = self.robot.com(q)
hg_act = self.robot.data.hg.np.A.copy()
self.p_error = hg_act[self._mask,:] - hg_ref[self._mask,:] oXi = data.oMi[1]
self.v_error = self.robot.fext[self._mask,:] - vhg_ref[self._mask,:] cXi = se3.SE3.Identity()
#***********************
p_com = data.com[0]
cXi = SE3.Identity()
oXi = self.robot.data.oMi[1]
cXi.rotation = oXi.rotation cXi.rotation = oXi.rotation
cXi.translation = oXi.translation - p_com cXi.translation = oXi.translation - p_com
#cXi.translation = p_com
m_gravity = model.gravity.copy() m_gravity = model.gravity.copy()
model.gravity.setZero() model.gravity.setZero()
b = se3.nle(model,data,q,v) b = se3.nle(model,data,q,v)
model.gravity = m_gravity model.gravity = m_gravity
f_ff = se3.Force(b[:6]) f_ff = se3.Force(b[:6])
f_com = cXi.act(f_ff) f_com = cXi.act(f_ff)
hg_drift = f_com.angular return f_com.np
self.drift=f_com.np[self._mask,:]
#drift = np.matrix(np.zeros((3, 1))) def _getContribution(self,com,s):
#a_tot = Ldot_des - hg_drift '''
#************************ Get segment contribution to centroidal angular
self.a_des = -(self.kp * self.p_error) momenta and its rate of change
#self.a_des = -(self.kp * self.p_error) + (self.kv*v_error) + ahg_ref Inputs:
#self.drift = 0*self.a_des - s : segment index
#print JMom.copy()[self._mask,:].shape - com : position of the CoM in world reference frame
#print self.__gain_matrix.shape '''
# 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 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 ''' ''' Define Fly Momentum Task '''
...@@ -454,6 +521,7 @@ class FlyMomentumTask(Task): ...@@ -454,6 +521,7 @@ class FlyMomentumTask(Task):
#self.v_error = self.robot.fext[self._mask,:] #self.v_error = self.robot.fext[self._mask,:]
#self.v_error = data.hg.vector.copy()[self._mask,:] - vhg_ref[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.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.a_des = self.kv*self.robot.fext[self._mask,:]#vhg_ref #+model.gravity.vector[self._mask,:]
#self.drift = 0 * self.a_des #self.drift = 0 * self.a_des
#self.a_des = #self.a_des =
......
...@@ -159,3 +159,27 @@ class SmoothedSE3Trajectory (object): ...@@ -159,3 +159,27 @@ class SmoothedSE3Trajectory (object):
v.linear = self._v_ref[:,i]; v.linear = self._v_ref[:,i];
a.linear = self._a_ref[:,i]; a.linear = self._a_ref[:,i];
return (M, v, a); 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(): ...@@ -86,8 +86,6 @@ class Wrapper():
self.biais(self.q, self.v) self.biais(self.q, self.v)
self.q = q.copy() self.q = q.copy()
def parseTrial(self, data): def parseTrial(self, data):
q=[] q=[]
for i in range(0, len(data)): for i in range(0, len(data)):
...@@ -151,6 +149,11 @@ class Wrapper(): ...@@ -151,6 +149,11 @@ class Wrapper():
''' the coriolis, centrifugal and gravitational effects ''' ''' the coriolis, centrifugal and gravitational effects '''
return se3.nle(self.model, self.data, q, v) 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): def generalizedAcceleration(self, V, dt):
return np.asmatrix(np.gradient(V, dt)) return np.asmatrix(np.gradient(V, dt))
...@@ -277,14 +280,22 @@ class Wrapper(): ...@@ -277,14 +280,22 @@ class Wrapper():
def com(self, q, v=None, a=None, update_kinematics=True): def com(self, q, v=None, a=None, update_kinematics=True):
if v is not None: if v is not None:
if a is 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] 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 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) 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):