def __init__(self, model, pinocchioData):
nc, nv = model.nimpulse, model.nv
self.pinocchio = pinocchioData
self.J = np.zeros([nc, nv])
self.Jq = np.zeros([nc, nv])
self.f = np.nan # not set at construction type
self.forces = pinocchio.StdVec_Force()
for i in range(model.pinocchio.njoints):
self.forces.append(pinocchio.Force.Zero())
self.Vq = np.zeros([nc, nv])
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 __init__(self, model, pinocchioData):
nc, nv, ndx = model.ncontact, model.nv, model.ndx
self.pinocchio = pinocchioData
self.J = np.zeros([nc, nv])
self.a0 = np.zeros(nc)
self.Ax = np.zeros([nc, ndx])
self.Aq = self.Ax[:, :nv]
self.Av = self.Ax[:, nv:]
self.f = np.nan # not set at construction type
self.forces = pinocchio.StdVec_Force()
for i in range(model.pinocchio.njoints):
self.forces.append(pinocchio.Force.Zero())
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 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_aba(self):
model = self.model
ddq = pin.aba(self.model, self.data, self.q, self.v, self.ddq)
null_fext = pin.StdVec_Force()
for _ in range(model.njoints):
null_fext.append(pin.Force.Zero())
ddq_null_fext = pin.aba(self.model, self.data, self.q, self.v,
self.ddq, null_fext)
self.assertApprox(ddq_null_fext, ddq)
null_fext_list = []
for f in null_fext:
null_fext_list.append(f)
print('size:', len(null_fext_list))
ddq_null_fext_list = pin.aba(self.model, self.data, self.q, self.v,
self.ddq, null_fext_list)
self.assertApprox(ddq_null_fext_list, ddq)
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 __init__(self, model, pinocchioData):
# a0: Desired spatial contact acceleration in frame coordinate
# x = (q, v) state formed by the configuration point q and its tangent vector v and is described by a nx tuple
# nu: dim of control vector
# u (tau, lambda) the control input (input torque command and contact forces)
# df_dx is really jacobian of dlambda/dx (partial
# ddv/dx jacovian of dvel/dx (acceleration)
# v_free is the unconbtrained acceleration
nc = model.ncontact
nv = model.nv
ndx = model.ndx # dim of the state vector
self.pinocchio = pinocchioData
self.J = np.zeros([nc, nv])
self.a0 = np.zeros(nc)
self.Ax = np.zeros([nc, ndx])
self.Aq = self.Ax[:, :nv]
self.Av = self.Ax[:, nv:]
self.f = np.nan # not set at construction type
self.forces = pinocchio.StdVec_Force()
for i in range(model.pinocchio.njoints):
self.forces.append(pinocchio.Force.Zero())
else:
m = pin.buildModelFromUrdf(dirpath+"/../model/urdf/ur3e.urdf")
d = m.createData()
q = pin.utils.zero(m.nq)
v = pin.utils.zero(m.nv)
u = pin.utils.zero(m.nv)
# Random states and control
q = pin.utils.rand(m.nq)
if use_free_jnt:
q[3:7] = q[3:7]/np.linalg.norm(q[3:7]) # normalize quaternion if free joint
v = pin.utils.rand(m.nv)
u = pin.utils.rand(m.nv)
# Set the external forces
fext = pin.StdVec_Force()
for k in range(m.njoints):
fext.append(pin.Force.Zero())
# Evaluate all terms and forward dynamics
pin.computeAllTerms(m, d, q, v)
a = pin.aba(m, d, q, v, u, fext)
# Print state and controls
print("pos =", q)
print("vel =", v)
print("tau =", u)
print("acc =", a)
print("nle =", d.nle)
# Evaluate the ABA derivatives
script_dir = os.path.dirname(os.path.realpath("__file__"))
pinocchio_model_dir = os.path.join(script_dir, "../urdf")
urdf_filename = pinocchio_model_dir + "/talos_full_legs_v2.urdf"
# Load URDF model
model = pinocchio.buildModelFromUrdf(urdf_filename,
pinocchio.JointModelFreeFlyer())
data = model.createData()
print('model name: ' + model.name)
# Se ponen las posiciones, velocidades, aceleraciones y fuerzas (en el
# marco de referencia de cada articulación)
q = pinocchio.utils.zero(model.nq)
v = pinocchio.utils.zero(model.nv)
a = pinocchio.utils.zero(model.nv)
fext = pinocchio.StdVec_Force()
for _ in range(model.njoints):
fext.append(pinocchio.Force.Zero())
# Valores de prueba
v[12] = 1
a[12] = 10
# Calcula las derivadas de la dinámica
# pinocchio.computeABADerivatives(model, data, q, v, a, fext)
# Muestra las aceleraciones
print('ddq: ' + str(data.ddq))
# Muestra las derivadas de la aceleración articular con respecto a la q
print('ddq_dq: ' + str(data.ddq_dq))
# Muestra las derivadas de la aceleración articular con respecto a la velocidad articular
assertNumDiff(dtau_dq, data.dtau_dq,
NUMDIFF_MODIFIER * h) # threshold was 1e-3, is now 2.11e-4 (see assertNumDiff.__doc__)
# Check RNEA versus ABA derivatives.
Mi = pinocchio.computeMinverse(model, data, q)
assert (absmax(Mi - inv(data.M)) < 1e-6)
D = np.dot(Mi, data.dtau_dq)
assertNumDiff(D, -data.ddq_dq, NUMDIFF_MODIFIER * h) # threshold was 1e-3, is now 2.11e-4 (see assertNumDiff.__doc__)
# ---- ABA AND RNEA with forces
# Set forces container
fs = pinocchio.StdVec_Force()
for i in range(model.njoints):
fs.append(pinocchio.Force.Random())
# Check RNEA derivatives versus finite-diff (with forces)
a = pinocchio.aba(model, data, q, v, tau, fs)
dtau_dqn = df_dq(model, lambda q_: pinocchio.rnea(model, data, q_, v, a, fs), q)
pinocchio.computeRNEADerivatives(model, data, q, v, a, fs)
dtau_dq = data.dtau_dq.copy()
assertNumDiff(dtau_dqn, dtau_dq,
NUMDIFF_MODIFIER * h) # threshold was 1e-3, is now 2.11e-4 (see assertNumDiff.__doc__)
# Check ABA derivatives versus finite diff (with forces)
da_dqn = df_dq(model, lambda q_: pinocchio.aba(model, data, q_, v, tau, fs), q)
pinocchio.computeABADerivatives(model, data, q, v, tau, fs)
da_dq = data.ddq_dq.copy()