def computeWrench(cs, Robot, dt):
rp = RosPack()
urdf = rp.get_path(
Robot.packageName
) + '/urdf/' + Robot.urdfName + Robot.urdfSuffix + '.urdf'
#srdf = "package://" + package + '/srdf/' + cfg.Robot.urdfName+cfg.Robot.srdfSuffix + '.srdf'
robot = RobotWrapper.BuildFromURDF(urdf, pin.StdVec_StdString(),
pin.JointModelFreeFlyer(), False)
model = robot.model
data = robot.data
q_t = cs.concatenateQtrajectories()
dq_t = cs.concatenateQtrajectories()
ddq_t = cs.concatenateQtrajectories()
t = q_t.min()
for phase in cs.contactPhases:
phase.wrench_t = None
t = phase.timeInitial
while t <= phase.timeFinal:
pin.rnea(model, data, phase.q_t(t), phase.dq_t(t), phase.ddq_t(t))
pcom, vcom, acom = robot.com(phase.q_t(t), phase.dq_t(t),
phase.ddq_t(t))
# FIXME : why do I need to call com to have the correct values in data ??
phi0 = data.oMi[1].act(pin.Force(data.tau[:6]))
if phase.wrench_t is None:
phase.wrench_t = piecewise(
polynomial(phi0.vector.reshape(-1, 1), t, t))
else:
phase.wrench_t.append(phi0.vector, t)
t += dt
if phase.timeFinal - dt / 2. <= t <= phase.timeFinal + dt / 2.:
t = phase.timeFinal
def test_coriolis_matrix(self):
model = self.model
C = pin.computeCoriolisMatrix(model, self.data, self.q, self.v)
data2 = model.createData()
tau_coriolis_ref = pin.rnea(
model, data2, self.q, self.v, self.a * 0) - pin.rnea(
model, data2, self.q, self.v * 0, self.a * 0)
self.assertApprox(tau_coriolis_ref, C.dot(self.v))
def inverseDynamics(self, q, v, a, f_ext=None):
'''ID(q, v, a, f_ext)
f_ext: Vector of external forces expressed in the local frame of each joint
do it recursively
'''
if f_ext is None:
for i in range(0, len(q)):
Tau.append(se3.rnea(self.model, self.data, q, v, a))
else:
for i in range(0, len(q)):
Tau.append(se3.rnea(self.model, self.data, q, v, a, f_ext))
return Tau
def compute_efforts(trajectory_data, index=(0, 0)):
"""
@brief Compute the efforts in the trajectory using RNEA method.
@param trajectory_data Sequence of States for which to compute the efforts.
@param index Index under which the efforts will be saved in the trajectory_data
(usually the couple of (roll, pitch) angles of the support foot).
"""
jiminy_model = trajectory_data['jiminy_model']
use_theoretical_model = trajectory_data['use_theoretical_model']
if use_theoretical_model:
pnc_model = jiminy_model.pinocchio_model_th
pnc_data = jiminy_model.pinocchio_data_th
else:
pnc_model = jiminy_model.pinocchio_model
pnc_data = jiminy_model.pinocchio_data
## Compute the efforts at each time step
root_joint_idx = pnc_model.getJointId('root_joint')
for s in trajectory_data['evolution_robot']:
# Apply a first run of rnea without explicit external forces
pnc.computeJointJacobians(pnc_model, pnc_data, s.q)
pnc.rnea(pnc_model, pnc_data, s.q, s.v, s.a)
# Initialize vector of exterior forces to 0
fs = pnc.StdVec_Force()
fs.extend(len(pnc_model.names) * (pnc.Force.Zero(), ))
# Compute the force at the henkle level
support_foot_idx = pnc_model.frames[pnc_model.getBodyId(
s.support_foot)].parent
fs[support_foot_idx] = pnc_data.oMi[support_foot_idx]\
.actInv(pnc_data.oMi[root_joint_idx]).act(pnc_data.f[root_joint_idx])
# Recompute the efforts with RNEA and the correct external forces
s.tau[index] = pnc.rnea(pnc_model, pnc_data, s.q, s.v, s.a, fs)
s.f_ext[index] = fs[support_foot_idx].copy()
# Add the force to the structure
s.f[index] = dict(
list(zip(pnc_model.names, [f_ind.copy() for f_ind in pnc_data.f])))
# Add the external force applied at the soles as if the floor was a parent of the soles
for joint in ['LeftSole', 'RightSole']:
ha_M_s = pnc_model.frames[pnc_model.getBodyId(joint)].placement
if s.support_foot == joint:
s.f[index][joint] = ha_M_s.actInv(s.f_ext[index])
else:
s.f[index][joint] = pnc.Force.Zero()
def test_rnea(self):
model = self.model
data = model.createData()
q = zero(model.nq)
qdot = zero(model.nv)
qddot = zero(model.nv)
for i in range(model.nbody):
data.a[i] = se3.Motion.Zero()
se3.rnea(model, data, q, qdot, qddot)
for i in range(model.nbody):
self.assertApprox(data.v[i].np, zero(6))
self.assertApprox(data.a_gf[0].np, -model.gravity.np)
self.assertApprox(data.f[-1], model.inertias[-1] * data.a_gf[-1])
def test_rnea(self):
model = self.model
data = model.createData()
q = zero(model.nq)
qdot = zero(model.nv)
qddot = zero(model.nv)
for i in range(model.nbody):
data.a[i] = se3.Motion.Zero()
se3.rnea(model, data, q, qdot, qddot)
for i in range(model.nbody):
self.assertApprox(data.v[i].np, zero(6))
self.assertApprox(data.a_gf[0].np, -model.gravity.np)
self.assertApprox(data.f[-1], model.inertias[-1] * data.a_gf[-1])
def forces_from_torques_full(model, data, q, v, dv, tauj, Jlinvel):
tau_tot = pin.rnea(model, data, q, v, dv)
tau_forces = tau_tot
tau_forces[6:] -= tauj
forces, _, rank, s = np.linalg.lstsq(Jlinvel.T, tau_forces, rcond=None)
return forces
def forces_from_torques(model, data, q, v, dv, tauj, Jlinvel_red):
tau_tot = pin.rnea(model, data, q, v, dv)
tau_forces = tau_tot[6:] - tauj
# forces = np.linalg.solve(Jlinvel_red.T, tau_forces) # equivalent
forces, _, rank, s = np.linalg.lstsq(Jlinvel_red.T, tau_forces, rcond=None)
return forces
def update(self, q):
se3.computeAllTerms(self.model, self.data, self.q, self.v)
#se3.forwardKinematics(self.model, self.data, q, self.v, self.a)
#- se3::forwardKinematics
#- se3::crba
#- se3::nonLinearEffects
#- se3::computeJacobians
#- se3::centerOfMass
#- se3::jacobianCenterOfMass
#- se3::kineticEnergy
#- se3::potentialEnergy
se3.framesKinematics(self.model, self.data, q)
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()
def estimate_forces(robot, q, vq, dvq, tau_j, cf_ids, dim, world_frame=True):
"""
dim: 3 -> point contact == 3D force, 6 -> planar contact == 6D wrench
"""
robot.forwardKinematics(q)
# simplify the assumption:
# No linear velocity --> almost no influence
# vq[0:3] = 0
# No angular velocity --> almost no influence
# vq[3:6] = 0
# No joint velocity --> max 1N difference during flight phase and high variation (lag of the est)
# vq[6:] = 0
# No linear spatial acceleration --> import difference on all axes during the whole traj(~ 10 N diff)
# dvq[:3] = 0
# No angular spatial acceleration --> difference spikes (~ 5N when entering flight phase)
# dvq[3:6] = 0
# No joint acceleration --> ~ 10-20 N diference uniquelly during flight phase
# dvq[6:] = 0
tau_cf = pin.rnea(rmodel, rdata, q, vq, dvq)
tau_cf[6:] -= tau_j
# compute/stack contact jacobians
Jlinvel = compute_joint_jac(robot, q, cf_ids, dim, world_frame=world_frame)
forces = np.linalg.pinv(Jlinvel.T) @ tau_cf
return forces.reshape((len(cf_ids), dim)).T
def iden_model(model, data, N, nq, nv, q, v, a): """This function calculates joint torques and generates the joint torque regressor. Note: a flag IsFrictioncld to include in dynamic model Input: model, data: model and data structure of robot from Pinocchio q, v, a: joint's position, velocity, acceleration N : number of samples nq: length of q Output: tau: vector of joint torque W : joint torque regressor""" tau = np.empty(nq*N) W = np.empty([N*nq, 10*nq]) for i in range(N): tau_temp = pin.rnea(model, data, q[i,:], v[i,:], a[i,:]) W_temp = pin.computeJointTorqueRegressor(model, data, q[i,:], v[i,:], a[i,:]) for j in range(nq): tau[j*N + i] = tau_temp[j] W[j*N + i, :] = W_temp[j,:] if isFrictionincld: W = np.c_[W,np.zeros([N*nq,2*nq])] for i in range(N): for j in range(nq): tau[j*N + i] = tau[j*N + i] + v[i,j]*fv + np.sign(v[i,j])*fc W[j*N + i, 10*nq+2*j] = v[i,j] W[j*N + i, 10*nq+2*j + 1] = np.sign(v[i,j]) return tau, W
def estimate_forces(q, vq, dvq, tau_j, cf_ids, world_frame=True):
robot.forwardKinematics(q)
# simplify the assumption:
# No linear velocity --> almost no influence
# vq[0:3] = 0
# No angular velocity --> almost no influence
# vq[3:6] = 0
# No joint velocity --> max 1N difference during flight phase and high variation (lag of the est)
# vq[6:] = 0
# No linear spatial acceleration --> import difference on all axes during the whole traj(~ 10 N diff)
# dvq[:3] = 0
# No angular spatial acceleration --> difference spikes (~ 5N when entering flight phase)
# dvq[3:6] = 0
# No joint acceleration --> ~ 10-20 N diference uniquelly during flight phase
# dvq[6:] = 0
tau_cf = pin.rnea(rmodel, rdata, q, vq, dvq)
tau_cf[6:] -= tau_j
# compute/stack contact jacobians
Jlinvel = np.zeros((12, robot.nv))
for i, frame_id in enumerate(contact_frame_ids):
if world_frame:
oTl = robot.framePlacement(q, frame_id, update_kinematics=False)
Jlinvel[3 * i:3 *
(i + 1), :] = oTl.rotation @ robot.computeFrameJacobian(
q, frame_id)[:3, :] # jac in world coord
else:
Jlinvel[3 * i:3 * (i + 1), :] = robot.computeFrameJacobian(
q, frame_id)[:3, :] # jac in local coord
forces = np.linalg.pinv(Jlinvel.T) @ tau_cf
return forces.reshape((4, 3)).T
def main():
'''
Main procedure
1 ) Build the model from an URDF file
2 ) compute forwardKinematics
3 ) compute forwardKinematics of the frames
4 ) explore data
'''
model_file = Path(__file__).absolute().parents[1].joinpath(
'urdf', 'twolinks.urdf')
model = buildModelFromUrdf(str(model_file))
# Create data required by the algorithms
data = model.createData()
# Sample a random configuration
numpy_vector_joint_pos = randomConfiguration(model)
numpy_vector_joint_vel = np.random.rand(model.njoints - 1)
numpy_vector_joint_acc = np.random.rand(model.njoints - 1)
# Calls Reverese Newton-Euler algorithm
numpy_vector_joint_torques = rnea(model, data, numpy_vector_joint_pos,
numpy_vector_joint_vel,
numpy_vector_joint_acc)
computeRNEADerivatives(model, data, numpy_vector_joint_pos,
numpy_vector_joint_vel, numpy_vector_joint_acc)
dtorques_dq = data.dtau_dq
dtorques_dqd = data.dtau_dv
dtorques_dqdd = data.M
def constraint(tau):
tau = a2m(tau)
q = rmodel.neutralConfiguration
v = zero(rmodel.nv)
a = zero(rmodel.nv)
res = m2a(pinocchio.rnea(rmodel, rdata, q, v, a) - tau)
return res.tolist()
def cost(tau):
tau = a2m(tau)
q = rmodel.neutralConfiguration
v = zero(rmodel.nv)
a = zero(rmodel.nv)
res = m2a(pinocchio.rnea(rmodel, rdata, q, v, a) - tau)
return sum(res**2)
def test_jointBodyRegressor(self):
model = pin.buildSampleModelManipulator()
data = model.createData()
JOINT_ID = model.njoints - 1
q = pin.randomConfiguration(model)
v = pin.utils.rand(model.nv)
a = pin.utils.rand(model.nv)
pin.rnea(model,data,q,v,a)
f = data.f[JOINT_ID]
f_regressor = pin.jointBodyRegressor(model,data,JOINT_ID).dot(model.inertias[JOINT_ID].toDynamicParameters())
self.assertApprox(f_regressor, f.vector)
def cid2(q_, v_, tau_):
pinocchio.computeJointJacobians(model, data, q_)
K = Kid(q_)
b = pinocchio.rnea(model, data, q_, v_, zero(model.nv)).copy()
pinocchio.forwardKinematics(model, data, q_, v_, zero(model.nv))
gamma = data.a[-1].vector
r = np.concatenate([tau_ - b, -gamma])
return inv(K) * r
def test_generalized_gravity(self):
model = self.model
tau = pin.computeGeneralizedGravity(model, self.data, self.q)
data2 = model.createData()
tau_ref = pin.rnea(model, data2, self.q, self.v * 0, self.a * 0)
self.assertApprox(tau, tau_ref)
def test_nle(self):
model = self.model
tau = pin.nonLinearEffects(model, self.data, self.q, self.v)
data2 = model.createData()
tau_ref = pin.rnea(model, data2, self.q, self.v, self.a * 0)
self.assertApprox(tau, tau_ref)
def forces_from_torques_full(model, data, q, v, dv, tauj, Jlinvel):
"""
Compromise between the euler equations and contact leg dynamics
"""
tau_tot = pin.rnea(model, data, q, v, dv)
tau_forces = tau_tot
tau_forces[6:] -= tauj
forces, _, rank, s = np.linalg.lstsq(Jlinvel.T, tau_forces, rcond=None)
return forces
def forces_from_torques(model, data, q, v, dv, tauj, Jlinvel_red):
"""
Works only well if the contact kinematics are perfect
"""
tau_tot = pin.rnea(model, data, q, v, dv)
tau_forces = tau_tot[6:] - tauj
# forces = np.linalg.solve(Jlinvel_red.T, tau_forces) # equivalent
forces, _, rank, s = np.linalg.lstsq(Jlinvel_red.T, tau_forces, rcond=None)
return forces
def compute_gravity_compensation(self, robot_state, torque):
pin.forwardKinematics(self.model, self.data,
self.trafo_q(robot_state[0]))
b = pin.rnea(self.model, self.data, self.trafo_q(robot_state[0]),
self.trafo_q(robot_state[1]), np.zeros(12))
add = self.inv_trafo_q(b)
torque += add
torque = np.clip(torque, -0.36, 0.36)
return torque
def computeWrench(res):
rp = RosPack()
urdf = rp.get_path(
cfg.Robot.packageName
) + '/urdf/' + cfg.Robot.urdfName + cfg.Robot.urdfSuffix + '.urdf'
#srdf = "package://" + package + '/srdf/' + cfg.Robot.urdfName+cfg.Robot.srdfSuffix + '.srdf'
robot = RobotWrapper.BuildFromURDF(urdf, pin.StdVec_StdString(),
pin.JointModelFreeFlyer(), False)
model = robot.model
data = robot.data
for k in range(res.N):
pin.rnea(model, data, res.q_t[:, k], res.dq_t[:, k], res.ddq_t[:, k])
pcom, vcom, acom = robot.com(
res.q_t[:, k], res.dq_t[:, k], res.ddq_t[:, k]
) # FIXME : why do I need to call com to have the correct values in data ??
phi0 = data.oMi[1].act(pin.Force(data.tau[:6]))
res.wrench_t[:, k] = phi0.vector
return res
def talker():
rospy.init_node("tocabi_pinocchio", anonymous = False)
rospy.Subscriber("/tocabi/pinocchio/jointstates", JointState, callback)
global model
pub = rospy.Publisher("/tocabi/pinocchio", model, queue_size=1)
modelmsg.CMM = [0 for i in range(33*6)]
modelmsg.COR = [0 for i in range(33*33)]
modelmsg.M = [0 for i in range(33*33)]
modelmsg.g = [0 for i in range(33)]
rate = rospy.Rate(1000) #10hz
model = pinocchio.buildModelFromUrdf("/home/jhk/catkin_ws/src/dyros_tocabi/tocabi_description/robots/dyros_tocabi.urdf",pinocchio.JointModelFreeFlyer())
data = model.createData()
global start
q = pinocchio.randomConfiguration(model)
qdot = pinocchio.utils.zero(model.nv)
qdot_t = pinocchio.utils.zero(model.nv)
qddot_t = pinocchio.utils.zero(model.nv)
while not rospy.is_shutdown():
if start:
global qmsg
for i in range(0, 39):
q[i] = qmsg.position[i]
for j in range(0, 38):
qdot[j] = qmsg.velocity[j]
qdot_t[j] = 0.0
qddot_t[j] = 0.0
CMM = pinocchio.computeCentroidalMap(model, data, q)
pinocchio.crba(model, data, q)
pinocchio.computeCoriolisMatrix(model, data, q, qdot)
pinocchio.rnea(model, data, q, qdot_t, qddot_t)
CMM_slice = CMM[:,6:39]
COR_slice = data.C[:,6:39]
M_slice = data.M[:,6:39]
g_slice = data.tau[6:39]
modelmsg.CMM = CMM_slice.flatten()
modelmsg.COR = COR_slice.flatten()
modelmsg.M = M_slice.flatten()
modelmsg.g = g_slice.flatten()
def compute_efforts_from_fixed_body(robot,
position,
velocity,
acceleration,
fixed_body_name,
use_theoretical_model=True):
"""
@brief Compute the efforts using RNEA method.
@note This function modifies internal data.
@param robot The jiminy robot
@param position Joint position vector
@param velocity See position
@param acceleration See position
@param fixed_body_name The name of the body frame on which to apply the external forces
"""
if use_theoretical_model:
pnc_model = robot.pinocchio_model_th
pnc_data = robot.pinocchio_data_th
else:
pnc_model = robot.pinocchio_model
pnc_data = robot.pinocchio_data
# Apply a first run of rnea without explicit external forces
pin.computeJointJacobians(pnc_model, pnc_data, position)
pin.rnea(pnc_model, pnc_data, position, velocity, acceleration)
# Initialize vector of exterior forces to zero
f_ext = pin.StdVec_Force()
f_ext.extend(len(pnc_model.names) * (pin.Force.Zero(), ))
# Compute the force at the contact frame
support_foot_idx = pnc_model.frames[pnc_model.getBodyId(
fixed_body_name)].parent
f_ext[support_foot_idx] = pnc_data.oMi[support_foot_idx] \
.actInv(pnc_data.oMi[1]).act(pnc_data.f[1])
# Recompute the efforts with RNEA and the correct external forces
tau = pin.rnea(pnc_model, pnc_data, position, velocity, acceleration,
f_ext)
f_ext = f_ext[support_foot_idx]
return tau, f_ext
def test_static_torque(self):
model = self.model
tau = pin.computeStaticTorque(model, self.data, self.q, self.fext)
data2 = model.createData()
tau_ref = pin.rnea(model, data2, self.q, self.v * 0, self.a * 0,
self.fext)
self.assertApprox(tau, tau_ref)
def estimate_torques(robot, q, vq, dvq, forces, cf_ids, dim, world_frame=True):
robot.forwardKinematics(q)
tau_tot = pin.rnea(rmodel, rdata, q, vq, dvq)
# compute/stack contact jacobians
Jlinvel = compute_joint_jac(robot, q, cf_ids, dim, world_frame=world_frame)
tau_forces = Jlinvel.T @ forces
return (tau_tot - tau_forces)[6:]
def compute_id_torques(self, q, v, a):
"""
This function computes the torques for the give state using rnea
Input:
q : joint positions
v : joint velocity
a : joint acceleration
"""
return np.reshape(pin.rnea(self.pinModel, self.pinData, q, v, a),
(self.nv, ))
def cid(q_, v_, tau_):
pinocchio.computeJointJacobians(model, data, q_)
J6 = pinocchio.getJointJacobian(model, data, model.joints[-1].id, pinocchio.ReferenceFrame.LOCAL).copy()
J = J6[:3, :]
b = pinocchio.rnea(model, data, q_, v_, zero(model.nv)).copy()
M = pinocchio.crba(model, data, q_).copy()
pinocchio.forwardKinematics(model, data, q_, v_, zero(model.nv))
gamma = data.a[-1].linear + cross(data.v[-1].angular, data.v[-1].linear)
K = np.bmat([[M, J.T], [J, zero([3, 3])]])
r = np.concatenate([tau_ - b, -gamma])
return inv(K) * r
def test_rnea(self):
model = self.model
tau = pin.rnea(self.model, self.data, self.q, self.v, self.a)
null_fext = pin.StdVec_Force()
for k in range(model.njoints):
null_fext.append(pin.Force.Zero())
tau_null_fext = pin.rnea(self.model, self.data, self.q, self.v, self.a,
null_fext)
self.assertApprox(tau_null_fext, tau)
null_fext_list = []
for f in null_fext:
null_fext_list.append(f)
print('size:', len(null_fext_list))
tau_null_fext_list = pin.rnea(self.model, self.data, self.q, self.v,
self.a, null_fext_list)
self.assertApprox(tau_null_fext_list, tau)
def computeWaistData(robot, res):
N = len(res.t_t)
model = robot.model
data = robot.data
res.waist_t = np.matrix(np.zeros([3, N]))
res.waist_orientation_t = np.matrix(np.empty([3, N]))
for k in range(N):
q = res.q_t[:, k]
v = res.dq_t[:, k]
a = res.ddq_t[:, k]
pin.rnea(model, data, q, v, a)
pcom, vcom, acom = robot.com(q, v, a)
res.waist_t[:, k] = robot.data.oMi[1].translation
res.waist_orientation_t[:, k] = matrixToRpy(robot.data.oMi[1].rotation)
return res
-err_rh-dJdq_rh[:3],
err_CP_u
])).reshape(ConstraintsSize,)
lb = -1000*np.ones(robot.nv)
ub = 1000*np.ones(robot.nv)
if i==0:
qp.init(H, g, A, lb, ub, lbA, ubA, np.array([100]))
else:
qp.hotstart(H, g, A, lb, ub, lbA, ubA, np.array([100]))
sol = np.zeros(robot.nv)
qp.getPrimalSolution(sol)
a = np.matrix(sol).T
se3.rnea(robot.model,robot.data,q,v,a)
tau_joints = robot.data.tau[6:]
## configuration integration
v += np.matrix(a*dt)
robot.increment(q,v*dt)
## Display new configuration
robot.display(q)
display_com_projection(robot,q)
time.sleep(0.005)
model = se3.Model.BuildEmptyModel()
assert(model.nbody==1 and model.nq==0 and model.nv==0)
model = se3.Model.BuildHumanoidSimple()
nb=28 # We should have 28 bodies, thus 27 joints, one of them a free-flyer.
assert(model.nbody==nb and model.nq==nb-1+6 and model.nv==nb-1+5)
model.inertias[1] = model.inertias[2]
assert( isapprox(model.inertias[1].np,model.inertias[2].np) )
model.jointPlacements[1] = model.jointPlacements[2]
assert( isapprox(model.jointPlacements[1].np,model.jointPlacements[2].np) )
assert(model.parents[0]==0 and model.parents[1] == 0)
model.parents[2] = model.parents[1]
assert( model.parents[2] == model.parents[1] )
assert(model.names[0] == "universe" )
assert( isapprox(model.gravity.np,np.matrix('0; 0; -9.81; 0; 0; 0')) )
data = model.createData()
q = zero(model.nq)
qdot = zero(model.nv)
qddot = zero(model.nv)
for i in range(model.nbody): data.a[i] = se3.Motion.Zero()
se3.rnea(model,data,q,qdot,qddot)
for i in range(model.nbody):
assert( isapprox(data.v[i].np,zero(6)) )
assert( isapprox(data.a[0].np,-model.gravity.np) )
assert( isapprox(data.f[-1],model.inertias[-1]*data.a[-1]) )
print("Check dM = C+C.T /2 \t\t", norm( Mp - (C+C.transpose(1,0,2)) ))
print("Check dM = Q+Q.T+Q.T+Q.T /2 \t", norm( Mp -
(Q+Q.transpose(0,2,1)+Q.transpose(1,0,2)+Q.transpose(2,0,1))/2 ))
# --- CORIOLIS
# --- CORIOLIS
# --- CORIOLIS
coriolis = Coriolis(robot)
C = coriolis(q,vq)
print("Check coriolis \t\t\t",norm(C*vq-rnea0(q,vq)))
# --- DRNEA
# --- DRNEA
# --- DRNEA
drnea = DRNEA(robot)
aq = rand(robot.model.nv)
R = drnea(q,vq,aq)
NV = robot.model.nv
Rd = zero([NV,NV])
eps = 1e-8
r0 = pin.rnea(robot.model,robot.data,q,vq,aq).copy()
for i in range(NV):
dq = zero(NV); dq[i]=eps
qdq = pin.integrate(robot.model,q,dq)
Rd[:,i] = (pin.rnea(robot.model,robot.data,qdq,vq,aq)-r0)/eps
print("Check drnea \t\t\t",norm(Rd-R))
def __call__(self,q,vq,aq):
robot = self.robot
pin.rnea(robot.model,robot.data,q,vq,aq)
NJ = robot.model.njoints
NV = robot.model.nv
J = robot.data.J
oMi = robot.data.oMi
v = [ (oMi[i]*robot.data.v [i]).vector for i in range(NJ) ]
a = [ (oMi[i]*robot.data.a_gf[i]).vector for i in range(NJ) ]
f = [ (oMi[i]*robot.data.f [i]).vector for i in range(NJ) ]
Y = [ (oMi[i]*robot.model.inertias[i]).matrix() for i in range(NJ) ]
Ycrb = [ (oMi[i]*robot.data.Ycrb[i]) .matrix() for i in range(NJ) ]
Tkf = [ zero([6,NV]) for i in range(NJ) ]
Yvx = [ zero([6,6]) for i in range(NJ) ]
adjf = lambda f: -np.bmat([ [zero([3,3]),skew(f[:3])] , [skew(f[:3]),skew(f[3:])] ])
R = self.R = zero([NV,NV])
for i in reversed(range(1,NJ)): # i is the torque index : dtau_i = R[i,:] dq
i0,i1 = iv(i)[0],iv(i)[-1]+1
Yi = Y[i]
Yci = Ycrb[i]
Si = J[:,i0:i1]
vi = v[i]
ai = a[i]
fi = f[i]
aqi = aq[i0:i1]
vqi = vq[i0:i1]
dvi = Si*vqi
li = parent(i)
Yvx[ i] += Y[i]*adj(v[i])
Yvx[ i] -= adjf(Y[i]*v[i])
Yvx[ i] -= adjdual(v[i])*Yi
Yvx[li] += Yvx[i]
for k in ancestors(i): # k is the derivative index: dtau = R[:,k] dq_k
k0,k1 = iv(k)[0],iv(k)[-1]+1
Sk = J[:,k0:k1]
lk = parent(k)
# Si' Yi Tk ai = Si' Yi ( Sk x (ai-alk) + (vi-vlk) x (Sk x vlk) )
# Tk Si' Yi ai = Si' Yi ( - Sk x a[lk] + (vi-vlk) x (Sk x vlk) )
Tkf = Yci*(- MCross(Sk,a[lk]) + MCross(MCross(Sk,v[lk]),v[lk]))
# Tk Si' fs = Tk Si' Ycrb[i] ai + Si' Ys (vs-vi) x (Sk x vlk)
# = "" + Si' Ys vs x (Sk x vlk) - Si' Ys vi x (Sk x vlk)
Tkf += Yvx[i]*MCross(Sk,v[lk])
R[i0:i1,k0:k1] = Si.T*Tkf
if i==k:
for kk in ancestors(k)[:-1]:
kk0,kk1 = iv(kk)[0],iv(kk)[-1]+1
Skk = J[:,kk0:kk1]
R[kk0:kk1,k0:k1] = Skk.T * FCross(Sk,f[i])
R[kk0:kk1,k0:k1] += Skk.T * Tkf
self.a = a
self.v = v
self.f = f
self.Y = Y
self.Ycrb = Ycrb
self.Yvx = Yvx
return R