def computeJacobian(self, q):
pin.forwardKinematics(self.rmodel, self.rdata, q)
pin.updateFramePlacements(self.rmodel, self.rdata)
pin.computeJointJacobians(self.rmodel, self.rdata, q)
J = pin.getFrameJacobian(self.rmodel, self.rdata, self.ee_frame_id,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
return J
def Jtf(q, f):
pinocchio.computeJointJacobians(rmodel, rdata, q)
pinocchio.forwardKinematics(rmodel, rdata, q)
pinocchio.updateFramePlacements(rmodel, rdata)
J = pinocchio.getFrameJacobian(rmodel, rdata, CONTACTFRAME,
pinocchio.ReferenceFrame.LOCAL)
return J.T * f
def Kid(q_, J_=None):
pinocchio.computeJointJacobians(model, data, q_)
J = pinocchio.getJointJacobian(model, data, model.joints[-1].id, pinocchio.ReferenceFrame.LOCAL).copy()
if J_ is not None:
J_[:, :] = J
M = pinocchio.crba(model, data, q_).copy()
return np.bmat([[M, J.T], [J, zero([6, 6])]])
def compute_forces(self, compute_data=True):
'''Compute the contact forces from q, v and elastic model'''
if compute_data:
se3.forwardKinematics(self.model, self.data, self.q, self.v,
zero(self.model.nv))
se3.computeJointJacobians(self.model, self.data)
se3.updateFramePlacements(self.model, self.data)
# check collisions only if unilateral contacts are enables
# or if the contact is not active yet
for c in self.contacts:
if (self.unilateral_contacts or not c.active):
c.check_collision()
contact_changed = False
if (self.nc != np.sum([c.active for c in self.contacts])):
contact_changed = True
print("%.3f Number of active contacts changed from %d to %d." %
(self.t, self.nc, np.sum([c.active for c in self.contacts])))
self.resize_contacts()
i = 0
for c in self.contacts:
if (c.active):
self.f[3 * i:3 * i + 3] = c.compute_force(
self.unilateral_contacts)
self.p[3 * i:3 * i + 3] = c.p
self.dp[3 * i:3 * i + 3] = c.v
self.p0[3 * i:3 * i + 3] = c.p0
self.dp0[3 * i:3 * i + 3] = c.dp0
i += 1
# if(contact_changed):
# print(i, c.frame_name, 'p', c.p.T, 'p0', c.p0.T, 'f', c.f.T)
return self.f
def calcDiff(self, q):
pin.framesForwardKinematics(self.rmodel, self.rdata, q)
pin.computeJointJacobians(self.rmodel, self.rdata, q)
M = self.rdata.oMf[self.frameIndex]
J = pin.getFrameJacobian(self.rmodel, self.rdata, self.frameIndex,
pin.LOCAL_WORLD_ALIGNED)[:3, :]
return 2 * J.T @ (M.translation - self.ptarget)
def calcDiff(self, data, x, u=None, recalc=True):
if u is None:
u = self.unone
if recalc:
xout, cost = self.calc(data, x, u)
nq, nv = self.nq, self.nv
q = a2m(x[:nq])
v = a2m(x[-nv:])
tauq = a2m(data.actuation.a)
pinocchio.computeABADerivatives(self.pinocchio, data.pinocchio, q, v, tauq)
da_dq = data.pinocchio.ddq_dq
da_dv = data.pinocchio.ddq_dv
da_dact = data.pinocchio.Minv
dact_dx = data.actuation.Ax
dact_du = data.actuation.Au
data.Fx[:, :nv] = da_dq
data.Fx[:, nv:] = da_dv
data.Fx += np.dot(da_dact, dact_dx)
data.Fu[:, :] = np.dot(da_dact, dact_du)
pinocchio.computeJointJacobians(self.pinocchio, data.pinocchio, q)
pinocchio.updateFramePlacements(self.pinocchio, data.pinocchio)
self.costs.calcDiff(data.costs, x, u, recalc=False)
return data.xout, data.cost
def _inverse_kinematics_step(
self, frame_id: int, xdes: np.ndarray,
q0: np.ndarray) -> typing.Tuple[np.ndarray, np.ndarray]:
"""Compute one IK iteration for a single finger."""
dt = 1.0e-1
pinocchio.computeJointJacobians(
self.robot_model,
self.data,
q0,
)
pinocchio.framesForwardKinematics(
self.robot_model,
self.data,
q0,
)
Ji = pinocchio.getFrameJacobian(
self.robot_model,
self.data,
frame_id,
pinocchio.ReferenceFrame.LOCAL_WORLD_ALIGNED,
)[:3, :]
xcurrent = self.data.oMf[frame_id].translation
try:
Jinv = np.linalg.inv(Ji)
except Exception:
Jinv = np.linalg.pinv(Ji)
err = xdes - xcurrent
dq = Jinv.dot(xdes - xcurrent)
qnext = pinocchio.integrate(self.robot_model, q0, dt * dq)
return qnext, err
def calcDiff(self, data, x, u=None, recalc=True):
self.costsData.shareMemory(data)
nq, nv = self.state.nv, self.state.nq
q, v = x[:nq], x[-nv:]
self.actuation.calcDiff(self.actuationData, x, u)
if u is None:
u = self.unone
if recalc:
self.calc(data, x, u)
pinocchio.computeJointJacobians(self.state.pinocchio, self.pinocchioData, q)
# Computing the dynamics derivatives
if self.enable_force:
pinocchio.computeABADerivatives(self.state.pinocchio, self.pinocchioData, q, v, u)
ddq_dq = self.pinocchioData.ddq_dq
ddq_dv = self.pinocchioData.ddq_dv
data.Fx = np.hstack([ddq_dq, ddq_dv]) + self.pinocchioData.Minv * self.actuationData.dtau_dx
data.Fu = self.pinocchioData.Minv * self.actuationData.dtau_du
else:
pinocchio.computeRNEADerivatives(self.state.pinocchio, self.pinocchioData, q, v, data.xout)
ddq_dq = self.Minv * (self.actuationData.dtau_dx[:, :nv] - self.pinocchioData.dtau_dq)
ddq_dv = self.Minv * (self.actuationData.dtau_dx[:, nv:] - self.pinocchioData.dtau_dv)
data.Fx = np.hstack([ddq_dq, ddq_dv])
data.Fu = self.Minv * self.actuationData.dtau_du
# Computing the cost derivatives
self.costs.calcDiff(self.costsData, x, u, False)
def calcDiff(self, data, x, u=None, recalc=True):
if u is None:
u = self.unone
if recalc:
xout, cost = self.calc(data, x, u)
nq, nv = self.nq, self.nv
q = a2m(x[:nq])
v = a2m(x[-nv:])
tauq = a2m(u)
a = a2m(data.xout)
# --- Dynamics
if self.forceAba:
pinocchio.computeABADerivatives(self.pinocchio, data.pinocchio, q,
v, tauq)
data.Fx[:, :nv] = data.pinocchio.ddq_dq
data.Fx[:, nv:] = data.pinocchio.ddq_dv
data.Fu[:, :] = data.pinocchio.Minv
else:
pinocchio.computeRNEADerivatives(self.pinocchio, data.pinocchio, q,
v, a)
data.Fx[:, :nv] = -np.dot(data.Minv, data.pinocchio.dtau_dq)
data.Fx[:, nv:] = -np.dot(data.Minv, data.pinocchio.dtau_dv)
data.Fu[:, :] = data.Minv
# --- Cost
pinocchio.computeJointJacobians(self.pinocchio, data.pinocchio, q)
pinocchio.updateFramePlacements(self.pinocchio, data.pinocchio)
self.costs.calcDiff(data.costs, x, u, recalc=False)
return data.xout, data.cost
def test_frame_algo(self):
model = self.model
data = model.createData()
q = pin.neutral(model)
v = np.random.rand((model.nv))
frame_id = self.frame_idx
J1 = pin.computeFrameJacobian(model, data, q, frame_id)
J2 = pin.computeFrameJacobian(model, data, q, frame_id, pin.LOCAL)
self.assertApprox(J1, J2)
data2 = model.createData()
pin.computeJointJacobians(model, data2, q)
J3 = pin.getFrameJacobian(model, data2, frame_id, pin.LOCAL)
self.assertApprox(J1, J3)
dJ1 = pin.frameJacobianTimeVariation(model, data, q, v, frame_id,
pin.LOCAL)
data3 = model.createData()
pin.computeJointJacobiansTimeVariation(model, data3, q, v)
dJ2 = pin.getFrameJacobianTimeVariation(model, data3, frame_id,
pin.LOCAL)
self.assertApprox(dJ1, dJ2)
def calcDiff3d(self, q):
pin.forwardKinematics(robot.model, robot.data, q)
M = pin.updateFramePlacement(robot.model, robot.data, self.frameIndex)
pin.computeJointJacobians(robot.model, robot.data, q)
J = pin.getFrameJacobian(robot.model, robot.data, self.frameIndex,
pin.LOCAL_WORLD_ALIGNED)[:3, :]
return 2 * J.T @ (M.translation - self.Mtarget.translation)
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)
# THIS FUNCTION CALLS THE FORWARD KINEMATICS
computeJointJacobians(model, data, numpy_vector_joint_pos)
joint_number = model.njoints
for i in range(joint_number):
joint = model.joints[i]
joint_placement = data.oMi[i]
joint_jacobian = getJointJacobian(model, data, i, ReferenceFrame.WORLD)
# CAUTION updateFramePlacements must be called to update frame's positions
# Remark: in pinocchio frames, joints and bodies are different things
updateFramePlacements(model, data)
# Print out the placement of each joint of the kinematic tree
frame_number = model.nframes
for i in range(frame_number):
frame = model.frames[i]
frame_placement = data.oMf[i]
frame_jacobian = getFrameJacobian(model, data, i, ReferenceFrame.WORLD)
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 dresidualWorld(q):
pinocchio.forwardKinematics(rmodel, rdata, q)
pinocchio.computeJointJacobians(rmodel, rdata, q)
rMi = Mref.inverse() * rdata.oMi[jid]
return np.dot(
pinocchio.Jlog6(rMi),
pinocchio.getJointJacobian(rmodel, rdata, jid,
pinocchio.ReferenceFrame.WORLD))
def MvJtf(q, vnext, v, f):
M = pinocchio.crba(rmodel, rdata, q)
pinocchio.computeJointJacobians(rmodel, rdata, q)
pinocchio.forwardKinematics(rmodel, rdata, q)
pinocchio.updateFramePlacements(rmodel, rdata)
J = pinocchio.getFrameJacobian(rmodel, rdata, CONTACTFRAME,
pinocchio.ReferenceFrame.LOCAL)
return M * (vnext - v) - J.T * f
def computeJacobian(self, q):
pin.forwardKinematics(self.rmodel, self.rdata, q)
pin.updateFramePlacements(self.rmodel, self.rdata)
pin.computeJointJacobians(self.rmodel, self.rdata, q)
J = pin.getFrameJacobian(self.rmodel, self.rdata, self.ee_frame_id,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
#modify J to remove the term corresponding to the base frame orientation
J = np.hstack([J[:, :3], np.zeros((6, 4)), J[:, 6:]])
return J
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)
# Perform the forward kinematics over the kinematic tree
forwardKinematics(model, data, numpy_vector_joint_pos,
numpy_vector_joint_vel)
computeJointJacobians(model, data, numpy_vector_joint_pos)
joint_number = model.njoints
for i in range(joint_number):
joint = model.joints[i]
joint_placement = data.oMi[i]
joint_velocity = getVelocity(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
joint_jacobian = getJointJacobian(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
joint_linear_vel = joint_velocity.linear
joint_angular_vel = joint_velocity.angular
joint_6v_vel = joint_velocity.vector
err = joint_6v_vel - joint_jacobian.dot(numpy_vector_joint_vel)
assert np.linalg.norm(err, ord=np.inf) < 1.0e-10, err
# CAUTION updateFramePlacements must be called to update frame's positions
# Remark: in pinocchio frames, joints and bodies are different things
updateFramePlacements(model, data)
frame_number = model.nframes
for i in range(frame_number):
frame = model.frames[i]
frame_placement = data.oMf[i]
frame_velocity = getFrameVelocity(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
frame_jacobian = getFrameJacobian(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
frame_linear_vel = frame_velocity.linear
frame_angular_vel = frame_velocity.angular
frame_6v_vel = frame_velocity.vector
err = frame_6v_vel - frame_jacobian.dot(numpy_vector_joint_vel)
assert np.linalg.norm(err, ord=np.inf) < 1.0e-10, err
def computeCollisions(self, q, vq=None):
res = pio.computeCollisions(self.rmodel, self.rdata, self.gmodel,
self.gdata, q, False)
pio.computeDistances(self.rmodel, self.rdata, self.gmodel, self.gdata,
q)
pio.computeJointJacobians(self.rmodel, self.rdata, q)
if not (vq is None):
pio.forwardKinematics(self.rmodel, self.rdata, q, vq, 0 * vq)
return res
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 dConstraint_dx(self, x):
q = a2m(x)
pinocchio.forwardKinematics(self.rmodel, self.rdata, q)
pinocchio.computeJointJacobians(self.rmodel, self.rdata, q)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
refMeff = self.refEff.inverse() * self.rdata.oMf[self.idEff]
log_M = pinocchio.Jlog6(refMeff)
M_q = pinocchio.getFrameJacobian(self.rmodel, self.rdata, self.idEff,
LOCAL)
self.Jeq = log_M * M_q
return self.Jeq
def compute_efforts_from_fixed_body(
robot: jiminy.Model,
position: np.ndarray,
velocity: np.ndarray,
acceleration: np.ndarray,
fixed_body_name: str,
use_theoretical_model: bool = True) -> Tuple[np.ndarray, pin.Force]:
"""Compute the efforts using RNEA method.
.. warning::
This function modifies the internal robot data.
:param robot: Jiminy robot.
:param position: Robot configuration vector.
:param velocity: Robot velocity vector.
:param acceleration: Robot acceleration vector.
:param fixed_body_name: Name of the body frame.
:param use_theoretical_model: Whether the state corresponds to the
theoretical model when updating and fetching
the robot's state.
Optional: True by default.
:returns: articular efforts and external forces.
"""
# Pick the right pinocchio model and data
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
# Note that `computeJointJacobians` also compute the forward kinematics
# of the model (position only obviously).
pin.computeJointJacobians(pnc_model, pnc_data, position)
jiminy.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 = jiminy.rnea(
pnc_model, pnc_data, position, velocity, acceleration, f_ext)
f_ext = f_ext[support_foot_idx]
return tau, f_ext
def get_link_jacobian(self, link_id):
frame_id = self._model.getFrameId(link_id)
pin.computeJointJacobians(self._model, self._data, self._q)
jac = pin.getFrameJacobian(self._model, self._data, frame_id,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
# Pinocchio has linear on top of angular
ret = np.zeros_like(jac)
ret[0:3] = jac[3:6]
ret[3:6] = jac[0:3]
return np.copy(ret)
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)
# IN WHAT FOLLOWS WE CONFIRM THAT rnea COMPUTES THE FORWARD KINEMATCS
computeJointJacobians(model, data, numpy_vector_joint_pos)
joint_number = model.njoints
for i in range(joint_number):
joint = model.joints[i]
joint_placement = data.oMi[i]
joint_velocity = getVelocity(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
joint_jacobian = getJointJacobian(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
err = joint_velocity.vector - \
joint_jacobian.dot(numpy_vector_joint_vel)
assert np.linalg.norm(err, ord=np.inf) < 1.0e-10, err
# CAUTION updateFramePlacements must be called to update frame's positions
# Remark: in pinocchio frames, joints and bodies are different things
updateFramePlacements(model, data)
frame_number = model.nframes
for i in range(frame_number):
frame = model.frames[i]
frame_placement = data.oMf[i]
frame_velocity = getFrameVelocity(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
frame_jacobian = getFrameJacobian(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
err = frame_velocity.vector - \
frame_jacobian.dot(numpy_vector_joint_vel)
assert np.linalg.norm(err, ord=np.inf) < 1.0e-10, err
def getJacobian(self, q=None):
if q is None:
q = self.current_j_pos
if q.shape[0] == 7:
q_tmp = q.copy()
q = np.zeros(9)
q[:7] = q_tmp
pinocchio.computeJointJacobians(self.model, self.data, q)
pinocchio.framesForwardKinematics(self.model, self.data, q)
return pinocchio.getFrameJacobian(self.model, self.data,
self.end_effector_frame_id,
pinocchio.LOCAL_WORLD_ALIGNED)[:, :7]
def setUp(self):
self.x = self.ROBOT_STATE.rand()
self.robot_data = self.ROBOT_MODEL.createData()
self.data = self.IMPULSE.createData(self.robot_data)
self.data_der = self.IMPULSE_DER.createData(self.robot_data)
nq, nv = self.ROBOT_MODEL.nq, self.ROBOT_MODEL.nv
pinocchio.forwardKinematics(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:],
pinocchio.utils.zero(nv))
pinocchio.computeJointJacobians(self.ROBOT_MODEL, self.robot_data)
pinocchio.updateFramePlacements(self.ROBOT_MODEL, self.robot_data)
pinocchio.computeForwardKinematicsDerivatives(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:],
pinocchio.utils.zero(nv))
def dConstraint_dx_rightfoot(self, x, nc=0):
q = self.vq2q(a2m(x))
pinocchio.forwardKinematics(self.rmodel, self.rdata, q)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
pinocchio.computeJointJacobians(self.rmodel, self.rdata, q)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
refMr = self.refR.inverse() * self.rdata.oMf[self.idR]
log_M = pinocchio.Jlog6(refMr)
M_q = pinocchio.getFrameJacobian(self.rmodel, self.rdata, self.idR,
LOCAL)
Q_v = self.dq_dv(a2m(x))
self.Jeq[nc:nc + 6, :] = log_M * M_q * Q_v
return self.Jeq[nc:nc + 6, :]
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 Jacobian(self, q):
"""
Description:
1. Computes the Jacobian Tensor of the robot.
Args:
q -> np.array (3,) : Joint Positions of the robot.
Returns:
np.array (3,3): Jacobian Tensor of the robot.
"""
frame = pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
pin.computeJointJacobians(self.model, self.model_data, q)
return pin.getFrameJacobian(self.model, self.model_data, self.EOAT_ID,
frame)[:3]
def setUp(self):
self.robot_data = self.ROBOT_MODEL.createData()
self.x = self.ROBOT_STATE.rand()
self.u = pinocchio.utils.rand(self.ROBOT_MODEL.nv)
self.multibody_data = crocoddyl.DataCollectorMultibody(self.robot_data)
self.data = self.COST.createData(self.multibody_data)
self.data_der = self.COST_DER.createData(self.multibody_data)
nq, nv = self.ROBOT_MODEL.nq, self.ROBOT_MODEL.nv
pinocchio.forwardKinematics(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:])
pinocchio.computeForwardKinematicsDerivatives(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:],
pinocchio.utils.zero(nv))
pinocchio.computeJointJacobians(self.ROBOT_MODEL, self.robot_data, self.x[:nq])
pinocchio.updateFramePlacements(self.ROBOT_MODEL, self.robot_data)
pinocchio.jacobianCenterOfMass(self.ROBOT_MODEL, self.robot_data, self.x[:nq], False)
def comp_combined_force(self, q, des_force, des_torque, in_touch):
if (np.sum(in_touch) == 0):
return np.zeros((9)), np.zeros((9))
q = self.trafo_q(q)
# TODO: I think it is sufficient if this is calculated only once:
full_JAC = pin.computeJointJacobians(self.model, self.data, q)
J_trans_inv_mat = np.zeros((3, 3 * 12))
for i in range(3):
if (in_touch[i] == 1):
idx = i * 4 + 4
idx_low = idx - 4
J_NEW = pin.getJointJacobian(
self.model, self.data, idx,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)[:3, :]
J_trans = J_NEW.T
J_trans_inv = np.matmul(
np.linalg.pinv(np.matmul(J_trans.T, J_trans)), J_trans.T)
J_trans_inv_mat[:, idx_low * 3:idx * 3] = J_trans_inv
add_torque_force = np.matmul(
np.matmul(
np.linalg.pinv(np.matmul(J_trans_inv_mat.T, J_trans_inv_mat)),
J_trans_inv_mat.T), des_force)
add_torque_force1 = self.inv_trafo_q(add_torque_force[:12])
add_torque_force2 = self.inv_trafo_q(add_torque_force[12:24])
add_torque_force3 = self.inv_trafo_q(add_torque_force[24:36])
return (add_torque_force1 + add_torque_force2 +
add_torque_force3), np.zeros((9))
def hessian(robot,q,crossterms=False):
'''
Compute the Hessian tensor of all the robot joints.
If crossterms, then also compute the Si x Si terms,
that are not part of the true Hessian but enters in some computations like VRNEA.
'''
lambdas.setRobotArgs(robot)
H=np.zeros([6,robot.model.nv,robot.model.nv])
pin.computeJointJacobians(robot.model,robot.data,q)
J = robot.data.J
skiplast = -1 if not crossterms else None
for joint_i in range(1,robot.model.njoints):
for joint_j in ancestors(joint_i)[:skiplast]: # j is a child of i
for i in iv(joint_i):
for j in iv(joint_j):
Si = J[:,i]
Sj = J[:,j]
H[:,i,j] = np.asarray(Mcross(Sj, Si))[:,0]
return H
