def constraint_leftfoot(self, x, nc=0):
q, tauq, fr, fl = self.x2qf(x)
pinocchio.forwardKinematics(self.rmodel, self.rdata, q)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
refMl = self.refL.inverse() * self.rdata.oMf[self.idL]
self.eq[nc:nc + 6] = m2a(pinocchio.log(refMl).vector)
return self.eq[nc:nc + 6].tolist()
def init(self, q0, v0=None, reset_contact_positions=False):
self.q = q0.copy()
if (v0 is None): self.v = zero(self.NV)
else: self.v = v0.copy()
self.dv = zero(self.NV)
# reset contact position
if (reset_contact_positions):
se3.forwardKinematics(self.robot.model, self.robot.data, self.q)
se3.updateFramePlacements(self.robot.model, self.robot.data)
self.Mrf0 = self.robot.data.oMf[self.RF].copy(
) # Initial (i.e. 0-load) position of the R spring.
self.Mlf0 = self.robot.data.oMf[self.LF].copy(
) # Initial (i.e. 0-load) position of the L spring.
self.compute_f_df(self.q, self.v, compute_data=True)
self.df = zero(4)
self.com_p, self.com_v, self.com_a, self.com_j = self.robot.get_com_and_derivatives(
self.q, self.v, self.f, self.df)
self.am, self.dam, self.ddam = self.robot.get_angular_momentum_and_derivatives(
self.q, self.v, self.f, self.df, self.Krf[0, 0], self.Krf[1, 1])
self.com_s = zero(2)
self.acc_lf = zero(3)
self.acc_rf = zero(3)
self.ddf = zero(4)
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 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 record(self, motion, variable, idx=None):
Jtask = []
task = []
if variable is 'joint':
for i in range(0, len(motion)):
se3.forwardKinematics(self.model, self.data, motion[i])
#task['rotation'].append(se3.utils.matrixToRpy(self.data.oMi[idx].rotation))
#task['translation'].append(self.data.oMi[idx].translation)
#task.append([self.data.oMi[idx].translation,
# se3.utils.matrixToRpy(self.data.oMi[idx].rotation)])
#task.append(np.row_stack((self.data.oMi[idx].translation,
# se3.utils.matrixToRpy(self.data.oMi[idx].rotation))))
task.append(
np.row_stack(
(self.data.oMi[idx].translation,
np.matrix(
euler_from_matrix(self.data.oMi[idx].rotation,
'syxz')).T)))
#M = (self.data.oMi[idx-1].rotation).T*self.data.oMi[idx].rotation
#task.append(np.row_stack((self.data.oMi[idx].translation,
# np.matrix(euler_from_matrix(M,'sxyz')).T)))
#M = self.oMp*self.data.oMi[idx].rotation
Jtask.append(
se3.jacobian(self.model, self.data, motion[i], idx, True,
True))
elif variable is 'com':
for i in range(0, len(motion)):
se3.forwardKinematics(self.model, self.data, motion[i])
task.append(self.com(motion[i]).getA()[:, idx])
Jtask.append(self.Jcom(motion[i]))
return task, Jtask
def constraint(self, x):
q = a2m(x)
pinocchio.forwardKinematics(self.rmodel, self.rdata, q)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
M = self.rdata.oMf[self.idEff]
self.eq = m2a(M.translation) - self.refEff
return self.eq.flat
def test_com_1(self):
data = self.data
v = rand(self.model.nv)
c0 = pin.centerOfMass(self.model, self.data, self.q, v)
c0_bis = pin.centerOfMass(self.model, self.data, self.q, v, False)
self.assertApprox(c0, c0_bis)
data2 = self.model.createData()
pin.forwardKinematics(self.model, data, self.q, v)
c0 = pin.centerOfMass(self.model, data, pin.VELOCITY)
pin.forwardKinematics(self.model, data2, self.q, v)
c0_bis = pin.centerOfMass(self.model, data2, pin.VELOCITY, False)
self.assertApprox(c0, c0_bis)
c0_bis = pin.centerOfMass(self.model, data2, 1)
self.assertApprox(c0, c0_bis)
data3 = self.model.createData()
pin.centerOfMass(self.model, data3, self.q)
self.assertApprox(self.data.com[0], data2.com[0])
self.assertApprox(self.data.vcom[0], data2.vcom[0])
self.assertApprox(self.data.com[0], data3.com[0])
def display(self,q):
pio.forwardKinematics(self.model,self.data,q)
pio.updateGeometryPlacements(self.model,self.data,self.gmodel,self.gdata)
if self.viewer is None: return
for i,g in enumerate(self.gmodel.geometryObjects):
self.viewer.applyConfiguration(g.name, pio.se3ToXYZQUATtuple(self.gdata.oMg[i]))
self.viewer.refresh()
def constraint(self, x):
q = a2m(x)
pinocchio.forwardKinematics(self.rmodel, self.rdata, q)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
refMeff = self.refEff.inverse() * self.rdata.oMf[self.idEff]
self.eq = m2a(pinocchio.log(refMeff).vector)
return self.eq.tolist()
def finiteDifferencesddA_ddq(model, data, q, v, h):
'''
d(A_dot)/d(q_dot)
'''
q0 = q.copy()
v0 = v.copy()
tensor_ddA_ddq = np.zeros((6, model.nv, model.nv))
se3.forwardKinematics(model, data, q0, v0)
vcom_ref = data.vcom[0].copy()
se3.dccrba(model, data, q0, v0)
dA0 = np.nan_to_num(data.dAg).copy()
oMc_ref = se3.SE3.Identity()
oMc_ref.translation = vcom_ref
for j in range(model.nv):
# vary dq
ah = np.matrix(np.zeros((model.nv, 1)))
ah[j] = h
v_new = v0 + ah
se3.forwardKinematics(model, data, q0, v_new)
vcom_new = data.vcom[0].copy()
se3.dcrba(model, data, q0, v_new)
dA0i_int = np.nan_to_num(data.dAg).copy()
oMc_int = se3.SE3.Identity()
oMc_int.translation = vcom_new
cMc_int = oMc_ref.inverse() * oMc_int
dA0_int = cMc_int.dualAction * dA0_int
tensor_ddA_ddq[:, :, j] = (dA0_int - dA0) / h
return tensor_dA_dq
def initialize(self, planner_setting):
package_dirs = [
os.path.dirname(
os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
]
urdf = str(
os.path.dirname(
os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) +
'/urdf/quadruped.urdf')
self.robot = RobotWrapper.BuildFromURDF(
urdf,
package_dirs=package_dirs,
root_joint=pin.JointModelFreeFlyer())
self.robot.data = self.robot.model.createData()
self.initDisplay(loadModel=True)
self.robot.viewer.gui.addFloor('world/floor')
self.z_floor = planner_setting.get(PlannerDoubleParam_FloorHeight)
self.robot.viewer.gui.applyConfiguration(
'world/floor', [0.0, 0.0, self.z_floor, 0.0, 0.0, 0.0, 1.0])
self.robot.viewer.gui.refresh()
self.robot.q = self.robot.model.referenceConfigurations.copy()
self.robot.dq = zero(self.robot.model.nv)
self.robot.q[7:] = np.transpose(
np.matrix(
planner_setting.get(
PlannerVectorParam_KinematicDefaultJointPositions)))
pin.forwardKinematics(self.robot.model, self.robot.data, self.robot.q)
self.robot.display(self.robot.q)
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 ik(robot,oMdes, JOINT_ID,eps = 1e-4):
q = robot.q0.copy()
IT_MAX = 500
DT = 1e-1
damp = 1e-12
i=0
success = False
while True:
pinocchio.forwardKinematics(robot.model,robot.data,q)
dMi = oMdes.actInv(robot.data.oMi[JOINT_ID])
err = pinocchio.log(dMi).vector
if norm(err) < eps:
success = True
break
if i >= IT_MAX:
success = False
break
J = pinocchio.computeJointJacobian(robot.model,robot.data,q,JOINT_ID)
v = - J.T.dot(solve(J.dot(J.T) + damp * np.eye(6), err))
q = pinocchio.integrate(robot.model,q,v*DT)
i += 1
if not success:
#raise( Exception('ik did not converged') )
q[:] = np.nan
if ((q < robot.model.lowerPositionLimit).any() or
(q > robot.model.upperPositionLimit).any()):
#Violate joint limits
q[:] = np.nan
return (q)
def finiteDifferencesdA_dq(model, data, q, v, h):
'''
dAi/dq
'''
q0 = q.copy()
v0 = v.copy()
tensor_dAi_dq = np.zeros((6, model.nv, model.nv))
se3.forwardKinematics(model, data, q0, v0)
se3.computeJacobians(model, data, q0)
pcom_ref = se3.centerOfMass(model, data, q0).copy()
se3.ccrba(model, data, q0, v0)
A0i = data.Ag.copy()
oMc_ref = se3.SE3.Identity() #data.oMi[1].copy()
oMc_ref.translation = pcom_ref #oMc_ref.translation - pcom_ref
for j in range(model.nv):
#vary q
vh = np.matrix(np.zeros((model.nv, 1)))
vh[j] = h
q_integrated = se3.integrate(model, q0.copy(), vh)
se3.forwardKinematics(model, data, q_integrated)
se3.computeJacobians(model, data, q_integrated)
pcom_int = se3.centerOfMass(model, data, q_integrated).copy()
se3.ccrba(model, data, q_integrated, v0)
A0_int = data.Ag.copy()
oMc_int = se3.SE3.Identity() #data.oMi[1].copy()
oMc_int.translation = pcom_int #oMc_int.translation - pcom_int
cMc_int = oMc_ref.inverse() * oMc_int
A0_int = cMc_int.dualAction * A0_int
tensor_dAi_dq[:, :, j] = (A0_int - A0i) / h
return tensor_dAi_dq
def updateState(robot, q, v, sensor_data):
# Get dcm from current state
pin.forwardKinematics(robot.pinocchio_model_th, robot.pinocchio_data_th, q,
v)
comOut = pin.centerOfMass(robot.pinocchio_model_th,
robot.pinocchio_data_th, q, v)
vcomOut = robot.pinocchio_data_th.vcom[0]
dcmOut = comOut + vcomOut / omega
# Create zmp from forces
forces = np.asarray(sensor_data[ForceSensor.type])
newWrench = pin.Force.Zero()
for i, name in enumerate(contact_points):
update_frame(robot.pinocchio_model_th, robot.pinocchio_data_th, name)
placement = get_frame_placement(robot.pinocchio_model_th,
robot.pinocchio_data_th, name)
wrench = pin.Force(np.array([0.0, 0.0, forces[2, i]]), np.zeros(3))
newWrench += placement.act(wrench)
totalWrenchOut = newWrench
if totalWrenchOut.linear[2] > 0:
zmpOut = [
-totalWrenchOut.angular[1] / totalWrenchOut.linear[2],
totalWrenchOut.angular[0] / totalWrenchOut.linear[2]
]
else:
zmpOut = zmp_log
return comOut, vcomOut, dcmOut, zmpOut, totalWrenchOut
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 test_com_2(self):
data = self.data
v = rand(self.model.nv)
a = rand(self.model.nv)
c0 = pin.centerOfMass(self.model, self.data, self.q, v, a)
c0_bis = pin.centerOfMass(self.model, self.data, self.q, v, a, False)
self.assertApprox(c0, c0_bis)
data2 = self.model.createData()
pin.forwardKinematics(self.model, data, self.q, v, a)
c0 = pin.centerOfMass(self.model, data, pin.ACCELERATION)
pin.forwardKinematics(self.model, data2, self.q, v, a)
c0_bis = pin.centerOfMass(self.model, data2, pin.ACCELERATION, False)
self.assertApprox(c0, c0_bis)
c0_bis = pin.centerOfMass(self.model, data2, 2)
self.assertApprox(c0, c0_bis)
data3 = self.model.createData()
pin.centerOfMass(self.model, data3, self.q)
data4 = self.model.createData()
pin.centerOfMass(self.model, data4, self.q, v)
self.assertApprox(self.data.com[0], data2.com[0])
self.assertApprox(self.data.vcom[0], data2.vcom[0])
self.assertApprox(self.data.acom[0], data2.acom[0])
self.assertApprox(self.data.com[0], data3.com[0])
self.assertApprox(self.data.com[0], data4.com[0])
self.assertApprox(self.data.vcom[0], data4.vcom[0])
def calc(self, data, x, u=None):
self.costsData.shareMemory(data)
if u is None:
u = self.unone
q, v = x[:self.state.nq], x[-self.state.nv:]
self.actuation.calc(self.actuationData, x, u)
tau = self.actuationData.tau
# Computing the dynamics using ABA or manually for armature case
if self.enable_force:
data.xout[:] = pinocchio.aba(self.state.pinocchio,
self.pinocchioData, q, v, tau)
else:
pinocchio.computeAllTerms(self.state.pinocchio, self.pinocchioData,
q, v)
data.M = self.pinocchioData.M
if self.armature.size == self.state.nv:
data.M[range(self.state.nv),
range(self.state.nv)] += self.armature
self.Minv = np.linalg.inv(data.M)
data.xout = self.Minv * (tau - self.pinocchioData.nle)
# Computing the cost value and residuals
pinocchio.forwardKinematics(self.state.pinocchio, self.pinocchioData,
q, v)
pinocchio.updateFramePlacements(self.state.pinocchio,
self.pinocchioData)
self.costs.calc(self.costsData, x, u)
data.cost = self.costsData.cost
def test_all(self):
model = pin.buildSampleModelHumanoidRandom()
joint_name = "larm6_joint"
joint_id = model.getJointId(joint_name)
frame_id = model.addBodyFrame("test_body", joint_id,
pin.SE3.Identity(), -1)
data = model.createData()
model.lowerPositionLimit[:7] = -1.
model.upperPositionLimit[:7] = 1.
q = pin.randomConfiguration(model)
pin.forwardKinematics(model, data, q)
R1 = pin.computeJointKinematicRegressor(model, data, joint_id,
pin.ReferenceFrame.LOCAL,
pin.SE3.Identity())
R2 = pin.computeJointKinematicRegressor(model, data, joint_id,
pin.ReferenceFrame.LOCAL)
self.assertApprox(R1, R2)
R3 = pin.computeFrameKinematicRegressor(model, data, frame_id,
pin.ReferenceFrame.LOCAL)
self.assertApprox(R1, R3)
def cost(self, x):
q = a2m(x)
pinocchio.forwardKinematics(self.rmodel, self.rdata, q)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
M = self.rdata.oMf[self.idEff]
self.residuals = m2a(pinocchio.log(M.inverse() * self.ref).vector)
return sum(self.residuals**2)
def collision(self, qw):
if isinstance(qw, ConfigurationWrapper):
q = qw.q
elif isinstance(qw, np.ndarray):
qw = ConfigurationWrapper(self, qw)
q = qw.q
else:
raise ValueError(
"qw should either be a ConfigurationWrapper or a numpy array.")
if q.shape[0] != self._model.nq:
raise ValueError(
f"The given configuration vector is of shape {q.shape[0]} while \
the model requires a configuration vector of shape {self._model.nq}"
)
model = self._model
data = self._data
geom_model = self._geom_model
geom_data = self._geom_data
pin.forwardKinematics(model, data, q)
pin.updateGeometryPlacements(model, data, geom_model, geom_data)
qw.oMi = data.oMi.tolist()
qw.oMg = geom_data.oMg.tolist()
# stop at the first collision
collide = pin.computeCollisions(geom_model, geom_data, True)
return collide
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)
# Perform the forward kinematics over the kinematic tree
forwardKinematics(model, data, numpy_vector_joint_pos)
# 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
joint_number = model.njoints
for i in range(joint_number):
joint = model.joints[i]
joint_placement = data.oMi[i]
frame_number = model.nframes
for i in range(frame_number):
frame = model.frames[i]
frame_placement = data.oMf[i]
def constraint_rightfoot(self, x, nc=0):
q = self.vq2q(a2m(x))
pinocchio.forwardKinematics(self.rmodel, self.rdata, q)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
refMr = self.refR.inverse() * self.rdata.oMf[self.idR]
self.eq[nc:nc + 6] = m2a(pinocchio.log(refMr).vector)
return self.eq[nc:nc + 6].tolist()
def compute_freeflyer_state_from_fixed_body(jiminy_model,
fixed_body_name,
position,
velocity=None,
acceleration=None,
use_theoretical_model=True):
"""
@brief Fill rootjoint data from articular data when a body is fixed parallel to world.
@details The hypothesis is that 'fixed_body_name' is fixed parallel to world frame.
So this method computes the position of freeflyer rootjoint in the fixed body frame.
@note This function modifies internal data.
@param model The Jiminy model
@param fixed_body_name The name of the body that is considered fixed parallel to world frame
@param[inout] position Must contain current articular data. The rootjoint data can
contain any value, it will be ignored and replaced
The method fills in rootjoint data
@param[inout] velocity Same as positionInOut but for velocity
@pre Rootjoint must be a freeflyer
@pre positionInOut.size() == model_->nq
"""
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
position[:6].fill(0)
position[6] = 1.0
if velocity is not None:
velocity[:6].fill(0)
else:
velocity = np.zeros((pnc_model.nv, ))
if acceleration is not None:
acceleration[:6].fill(0)
else:
acceleration = np.zeros((pnc_model.nv, ))
pnc.forwardKinematics(pnc_model, pnc_data, position, velocity,
acceleration)
pnc.framesForwardKinematics(pnc_model, pnc_data, position)
ff_M_fixed_body = get_body_world_transform(jiminy_model, fixed_body_name)
w_M_ff = ff_M_fixed_body.inverse()
base_link_translation = w_M_ff.translation
base_link_quaternion = Quaternion(w_M_ff.rotation)
position[:3] = base_link_translation
position[3:7] = base_link_quaternion.coeffs()
ff_v_fixed_body = get_body_world_velocity(jiminy_model, fixed_body_name)
base_link_velocity = -ff_v_fixed_body
velocity[:6] = base_link_velocity.vector
ff_a_fixedBody = get_body_world_acceleration(jiminy_model, fixed_body_name)
base_link_acceleration = -ff_a_fixedBody
acceleration[:6] = base_link_acceleration.vector
def createProblem(self, x0, comGoTo, timeStep, numKnots):
# Compute the current foot positions
q0 = a2m(x0[:self.rmodel.nq])
pinocchio.forwardKinematics(self.rmodel, self.rdata, q0)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
com0 = m2a(pinocchio.centerOfMass(self.rmodel, self.rdata, q0))
# Defining the action models along the time instances
comModels = []
# Creating the action model for the CoM task
comForwardModels = [
self.createModels(
timeStep,
[self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
) for k in range(numKnots)
]
comForwardTermModel = self.createModels(
timeStep,
[self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
com0 + [comGoTo, 0., 0.])
comForwardTermModel.differential.costs['comTrack'].weight = 1e6
# Adding the CoM tasks
comModels += comForwardModels + [comForwardTermModel]
# Defining the shooting problem
problem = ShootingProblem(x0, comModels, comModels[-1])
return problem
def display(self, q):
se3.forwardKinematics(self.model, self.data, q)
for visual in self.visuals:
visual.place(self.viewer,
self.data.oMi[visual.jointParent],
refresh=False)
self.viewer.refresh()
def solve(self, M_des, robot, joint_id, q=None):
""" Inverse kinematics for specified joint (joint_id) and desired pose M_des (array 4x4)
NOTE The code below is adopted from here (01.03.2020):
https://gepettoweb.laas.fr/doc/stack-of-tasks/pinocchio/master/doxygen-html/md_doc_b-examples_i-inverse-kinematics.html
"""
oMdes = pinocchio.SE3(M_des[:3, :3], M_des[:3, 3])
if np.all(q == None): q = pinocchio.neutral(robot.model)
i = 0
iter_resample = 0
while True:
# forward
pinocchio.forwardKinematics(robot.model, robot.data, q)
# error between desired pose and the current one
dMi = oMdes.actInv(robot.data.oMi[joint_id])
err = pinocchio.log(dMi).vector
# Termination criteria
if norm(err) < self.inv_kin_sol_params.eps:
success = True
break
if i >= self.inv_kin_sol_params.max_iter:
if iter_resample <= self.inv_kin_sol_params.max_resample:
q = pinocchio.randomConfiguration(robot.model)
iter_resample += 1
continue
else:
success = False
break
# Jacobian
J = pinocchio.computeJointJacobian(robot.model, robot.data, q,
joint_id)
# P controller (?)
# v* =~ A * e
v = -J.T.dot(
solve(
J.dot(J.T) + self.inv_kin_sol_params.damp * np.eye(6),
err))
# integrate (in this case only sum)
q = pinocchio.integrate(robot.model, q,
v * self.inv_kin_sol_params.dt)
i += 1
if not success: q = pinocchio.neutral(robot.model)
q_arr = np.squeeze(np.array(q))
q_arr = np.mod(np.abs(q_arr), 2 * np.pi) * np.sign(q_arr)
mask = np.abs(q_arr) > np.pi
# If angle abs(q) is larger than pi, represent angle as the shorter angle inv_sign(q) * (2*pi - abs(q))
# This is to avoid conflicting with angle limits.
q_arr[mask] = (-1) * np.sign(
q_arr[mask]) * (2 * np.pi - np.abs(q_arr[mask]))
return success, q_arr
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 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 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_com_0(self):
c0 = pin.centerOfMass(self.model,self.data,self.q)
data2 = self.model.createData()
pin.forwardKinematics(self.model,data2,self.q)
pin.centerOfMass(self.model,data2,0)
self.assertApprox(c0,data2.com[0])
self.assertApprox(self.data.com[0],data2.com[0])
def test_copy(self):
data2 = self.data.copy()
q = pin.neutral(self.model)
pin.forwardKinematics(self.model,data2,q)
jointId = self.model.njoints-1
self.assertNotEqual(self.data.oMi[jointId], data2.oMi[jointId])
data3 = data2.copy()
self.assertEqual(data2.oMi[jointId], data3.oMi[jointId])
def test_Jcom_noupdate2(self):
data_no = self.data
data_up = self.model.createData()
pin.forwardKinematics(self.model,data_no,self.q)
Jcom_no = pin.jacobianCenterOfMass(self.model,data_no)
Jcom_up = pin.jacobianCenterOfMass(self.model,data_up,self.q)
self.assertTrue((Jcom_no==Jcom_up).all())
def test_com_default(self):
v = rand(self.model.nv)
a = rand(self.model.nv)
pin.centerOfMass(self.model,self.data,self.q,v,a)
data2 = self.model.createData()
pin.forwardKinematics(self.model,data2,self.q,v,a)
pin.centerOfMass(self.model,data2)
for i in range(self.model.njoints):
self.assertApprox(self.data.com[i],data2.com[i])
self.assertApprox(self.data.vcom[i],data2.vcom[i])
self.assertApprox(self.data.acom[i],data2.acom[i])
def test_com_1(self):
v = rand(self.model.nv)
pin.centerOfMass(self.model,self.data,self.q,v)
data2 = self.model.createData()
pin.forwardKinematics(self.model,data2,self.q,v)
pin.centerOfMass(self.model,data2,1)
data3 = self.model.createData()
pin.centerOfMass(self.model,data3,self.q)
self.assertApprox(self.data.com[0],data2.com[0])
self.assertApprox(self.data.vcom[0],data2.vcom[0])
self.assertApprox(self.data.com[0],data3.com[0])
def test_com_2(self):
v = rand(self.model.nv)
a = rand(self.model.nv)
pin.centerOfMass(self.model,self.data,self.q,v,a)
data2 = self.model.createData()
pin.forwardKinematics(self.model,data2,self.q,v,a)
pin.centerOfMass(self.model,data2,2)
data3 = self.model.createData()
pin.centerOfMass(self.model,data3,self.q)
data4 = self.model.createData()
pin.centerOfMass(self.model,data4,self.q,v)
self.assertApprox(self.data.com[0],data2.com[0])
self.assertApprox(self.data.vcom[0],data2.vcom[0])
self.assertApprox(self.data.acom[0],data2.acom[0])
self.assertApprox(self.data.com[0],data3.com[0])
self.assertApprox(self.data.com[0],data4.com[0])
self.assertApprox(self.data.vcom[0],data4.vcom[0])
import pinocchio
from sys import argv
filename = "ur5.urdf" if len(argv)<2 else argv[1]
model = pinocchio.buildModelFromUrdf(filename)
data = model.createData()
q = pinocchio.randomConfiguration(model)
print 'q = ', q.T
pinocchio.forwardKinematics(model,data,q)
for k in range(model.njoints):
print("{:<24} : {: .2f} {: .2f} {: .2f}"
.format( model.names[k], *data.oMi[k].translation.T.flat ))
def display(self, q):
pin.forwardKinematics(self.model, self.data, q)
for visual in self.visuals:
visual.place(self.viewer, self.data.oMi[visual.jointParent])
self.viewer.viewer.gui.refresh()
