def generate_random_trajectory(robot, npts, maxd, randskip=0):
# Use to skip configuration. Have no idea how to set random seed
# in pinocchio.
for i in range(randskip):
pinocchio.randomConfiguration(robot.model)
ts = [
0.0,
]
qs = [
pinocchio.randomConfiguration(robot.model),
]
while len(qs) < npts:
qlast = qs[-1]
q = pinocchio.randomConfiguration(robot.model)
d = pinocchio.distance(robot.model, qlast, q)
if d > maxd:
q = pinocchio.interpolate(robot.model, qlast, q, maxd / d)
qs.append(q)
ts.append(ts[-1] + pinocchio.distance(robot.model, qlast, q))
# Compute velocities and accelerations
vs = [
np.zeros(robot.model.nv),
]
accs = [
np.zeros(robot.model.nv),
]
eps = 0.01
for q0, q1, q2 in zip(qs, qs[1:], qs[2:]):
qprev = pinocchio.interpolate(
robot.model, q0, q1, 1 - eps / pinocchio.distance(robot.model, q0, q1)
)
qnext = pinocchio.interpolate(
robot.model, q1, q2, eps / pinocchio.distance(robot.model, q1, q2)
)
# (qnext - qprev) / eps
vs.append(pinocchio.difference(robot.model, qprev, qnext) / eps)
# ((qnext - q) - (q - qprev)) / eps^2
accs.append(
(
pinocchio.difference(robot.model, q, qnext)
- pinocchio.difference(robot.model, qprev, q)
)
/ (eps ** 2)
)
vs.append(np.zeros(robot.model.nv))
accs.append(np.zeros(robot.model.nv))
from toppra.cpp import PiecewisePolyPath
return PiecewisePolyPath.constructHermite(qs, vs, ts)
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 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 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 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 setUp(self):
self.model = pin.buildSampleModelHumanoidRandom()
self.data = self.model.createData()
qmax = np.matrix(np.full((self.model.nq, 1), np.pi))
self.q = pin.randomConfiguration(self.model, -qmax, qmax)
self.v = np.random.rand(self.model.nv)
self.a = np.random.rand(self.model.nv)
def random_configuration(self, project_fn=None):
q = pin.randomConfiguration(self.model)
# q = np.random.uniform(-np.pi, np.pi, size=(6,))
qw = ConfigurationWrapper(self, q)
qw = self.clip(qw, self._clip_bounds[0], self._clip_bounds[1])[1]
if project_fn:
qw = project_fn(qw)
return qw
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 setUp(self):
self.rmodel = rmodel = pin.buildSampleModelHumanoid()
self.rdata = rmodel.createData()
self.q = pin.randomConfiguration(rmodel)
self.vq = rand(rmodel.nv) * 2 - 1
self.aq = zero(rmodel.nv)
self.dq = rand(rmodel.nv) * 2 - 1
self.precision = 1e-8
def setUp(self):
self.rmodel = rmodel = pin.buildSampleModelHumanoid()
self.rdata = rmodel.createData()
self.q = pin.randomConfiguration(rmodel)
self.vq = rand(rmodel.nv)*2-1
self.aq = zero(rmodel.nv)
self.dq = rand(rmodel.nv)*2-1
self.precision = 1e-8
def test_basic(self):
model = self.model
q_ones = np.ones((model.nq))
self.assertFalse(pin.isNormalized(model, q_ones))
self.assertFalse(pin.isNormalized(model, q_ones, 1e-8))
self.assertTrue(pin.isNormalized(model, q_ones, 1e2))
q_rand = np.random.rand((model.nq))
q_rand = pin.normalize(model, q_rand)
self.assertTrue(pin.isNormalized(model, q_rand))
self.assertTrue(pin.isNormalized(model, q_rand, 1e-8))
self.assertTrue(abs(np.linalg.norm(q_rand[3:7]) - 1.) <= 1e-8)
q_next = pin.integrate(model, self.q, np.zeros((model.nv)))
self.assertApprox(q_next, self.q)
v_diff = pin.difference(model, self.q, q_next)
self.assertApprox(v_diff, np.zeros((model.nv)))
q_next = pin.integrate(model, self.q, self.v)
q_int = pin.interpolate(model, self.q, q_next, 0.5)
self.assertApprox(q_int, q_int)
value = pin.squaredDistance(model, self.q, self.q)
self.assertTrue((value <= 1e-8).all())
dist = pin.distance(model, self.q, self.q)
self.assertTrue(dist <= 1e-8)
q_neutral = pin.neutral(model)
self.assertApprox(q_neutral, q_neutral)
q_rand1 = pin.randomConfiguration(model)
q_rand2 = pin.randomConfiguration(model, -np.ones((model.nq)),
np.ones((model.nq)))
self.assertTrue(pin.isSameConfiguration(model, self.q, self.q, 1e-8))
self.assertFalse(pin.isSameConfiguration(model, q_rand1, q_rand2,
1e-8))
def test_getters(self):
data = self.model.createData()
q = pin.randomConfiguration(self.model)
v = np.random.rand(self.model.nv)
a = np.random.rand(self.model.nv)
pin.forwardKinematics(self.model, data, q, v, a)
T = pin.updateFramePlacement(self.model, data, self.frame_idx)
self.assertApprox(T,
data.oMi[self.parent_idx].act(self.frame_placement))
v = pin.getFrameVelocity(self.model, data, self.frame_idx)
self.assertApprox(v,
self.frame_placement.actInv(data.v[self.parent_idx]))
v = pin.getFrameVelocity(self.model, data, self.frame_idx,
pin.ReferenceFrame.LOCAL)
self.assertApprox(v,
self.frame_placement.actInv(data.v[self.parent_idx]))
v = pin.getFrameVelocity(self.model, data, self.frame_idx,
pin.ReferenceFrame.WORLD)
self.assertApprox(
v, data.oMi[self.parent_idx].act(data.v[self.parent_idx]))
v = pin.getFrameVelocity(self.model, data, self.frame_idx,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
self.assertApprox(
v,
pin.SE3(T.rotation, np.zeros(3)).act(
self.frame_placement.actInv(data.v[self.parent_idx])))
a = pin.getFrameAcceleration(self.model, data, self.frame_idx)
self.assertApprox(a,
self.frame_placement.actInv(data.a[self.parent_idx]))
a = pin.getFrameAcceleration(self.model, data, self.frame_idx,
pin.ReferenceFrame.LOCAL)
self.assertApprox(a,
self.frame_placement.actInv(data.a[self.parent_idx]))
a = pin.getFrameAcceleration(self.model, data, self.frame_idx,
pin.ReferenceFrame.WORLD)
self.assertApprox(
a, data.oMi[self.parent_idx].act(data.a[self.parent_idx]))
a = pin.getFrameAcceleration(self.model, data, self.frame_idx,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
self.assertApprox(
a,
pin.SE3(T.rotation, np.zeros(3)).act(
self.frame_placement.actInv(data.a[self.parent_idx])))
a = pin.getFrameClassicalAcceleration(self.model, data, self.frame_idx)
a = pin.getFrameClassicalAcceleration(self.model, data, self.frame_idx,
pin.ReferenceFrame.LOCAL)
a = pin.getFrameClassicalAcceleration(self.model, data, self.frame_idx,
pin.ReferenceFrame.WORLD)
a = pin.getFrameClassicalAcceleration(
self.model, data, self.frame_idx,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
def setUp(self):
self.rmodel = rmodel = pin.buildSampleModelHumanoid()
self.rdata = rmodel.createData()
self.rdata_fd = rmodel.createData()
self.rmodel.lowerPositionLimit[:3] = -1.
self.rmodel.upperPositionLimit[:3] = -1.
self.q = pin.randomConfiguration(rmodel)
self.vq = rand(rmodel.nv) * 2 - 1
self.precision = 1e-8
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 setUp(self):
self.model = pin.buildSampleModelHumanoidRandom()
self.data = self.model.createData()
qmax = np.full((self.model.nq, 1), np.pi)
self.q = pin.randomConfiguration(self.model, -qmax, qmax)
self.v = np.random.rand(self.model.nv)
self.ddq = np.random.rand(self.model.nv)
self.fext = []
for _ in range(self.model.njoints):
self.fext.append(pin.Force.Random())
def test_regressor_matrix(self):
for _ in range(100):
angle = pinocchio.randomConfiguration(self.model)
velocity = pinocchio.utils.rand(self.model.nv)
acceleration = pinocchio.utils.rand(self.model.nv)
Y = self.compute_regressor_matrix(angle, velocity, acceleration)
theta = self.get_params()
other_tau = Y * theta
torque = self.inverse_dynamics(angle, velocity, acceleration)
assert ((abs(torque - other_tau) <= 1e-9).all())
def setUp(self):
self.model = pin.buildSampleModelHumanoidRandom()
self.data = self.model.createData()
qmax = np.matrix(np.full((self.model.nq,1),np.pi))
self.q = pin.randomConfiguration(self.model,-qmax,qmax)
self.v = rand(self.model.nv)
self.tau = rand(self.model.nv)
self.v0 = zero(self.model.nv)
self.tau0 = zero(self.model.nv)
self.tolerance = 1e-9
# we compute J on a different self.data
self.J = pin.jointJacobian(self.model,self.model.createData(),self.q,self.model.getJointId('lleg6_joint'))
self.gamma = zero(6)
def setUp(self):
self.model = pin.buildSampleModelHumanoidRandom()
self.parent_idx = self.model.getJointId(
"rarm2_joint") if self.model.existJointName("rarm2_joint") else (
self.model.njoints - 1)
self.frame_name = self.model.names[self.parent_idx] + "_frame"
self.frame_placement = pin.SE3.Random()
self.frame_type = pin.FrameType.OP_FRAME
self.model.addFrame(
pin.Frame(self.frame_name, self.parent_idx, 0,
self.frame_placement, self.frame_type))
self.frame_idx = self.model.getFrameId(self.frame_name)
self.data = self.model.createData()
self.q = pin.randomConfiguration(self.model)
self.v = np.random.rand((self.model.nv))
self.a = np.random.rand((self.model.nv))
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 setUp(self):
self.model = pin.buildSampleModelHumanoidRandom()
self.data = self.model.createData()
qmax = np.matrix(np.full((self.model.nq, 1), np.pi))
self.q = pin.randomConfiguration(self.model, -qmax, qmax)
self.v = rand(self.model.nv)
self.tau = rand(self.model.nv)
self.v0 = zero(self.model.nv)
self.tau0 = zero(self.model.nv)
self.tolerance = 1e-9
# we compute J on a different self.data
self.J = pin.jointJacobian(self.model, self.model.createData(), self.q,
self.model.getJointId('lleg6_joint'))
self.gamma = zero(6)
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 searchContactPosture(self, numberOfContacts, qguess=None, ntrial=100):
"""
Search (randomly) a contact configuration with a given number of contacts.
- number of contacts: between 1 and len(self.contactCandidates).
- qguess is a configuration, of dim rmodel.nq. If None, a random configuration is first
sampled.
- ntrial: number of random trial.
If successfull, set self.success to True, and store the contact posture found in self.qcontact.
If not sucessfull, set self.success to False.
Returns self.success.
"""
assert 0 < numberOfContacts < len(self.contactCandidates)
if qguess is None:
qguess = pin.randomConfiguration(self.rmodel)
for itrial in range(ntrial):
limbs = random.sample(list(self.contactCandidates.values()), numberOfContacts)
stones = random.sample(self.terrain, numberOfContacts)
constraints = [Constraint(self.rmodel, limb.frameIndex, stone, limb.contactType)
for limb, stone in zip(limbs, stones)]
self.postureGenerator.run(qguess, constraints)
if self.postureGenerator.sucess:
# We found a candidate posture, let' s check that it is acceptable.
qcandidate = self.postureGenerator.qopt.copy()
# computeCollisions check collision among the collision pairs.
collision = pin.computeCollisions(self.rmodel, self.rdata,
self.collision_model, self.collision_data, qcandidate, True)
if not collision:
self.qcontact = qcandidate
self.success = True
return True
self.success = False
return False
def test_frameBodyRegressor(self):
model = pin.buildSampleModelManipulator()
JOINT_ID = model.njoints - 1
framePlacement = pin.SE3.Random()
FRAME_ID = model.addBodyFrame ("test_body", JOINT_ID, framePlacement, -1)
data = model.createData()
q = pin.randomConfiguration(model)
v = pin.utils.rand(model.nv)
a = pin.utils.rand(model.nv)
pin.rnea(model,data,q,v,a)
f = framePlacement.actInv(data.f[JOINT_ID])
I = framePlacement.actInv(model.inertias[JOINT_ID])
f_regressor = pin.frameBodyRegressor(model,data,FRAME_ID).dot(I.toDynamicParameters())
self.assertApprox(f_regressor, f.vector)
def test_staticRegressor(self):
model = pin.buildSampleModelHumanoidRandom()
data = model.createData()
data_ref = model.createData()
model.lowerPositionLimit[:7] = -1.
model.upperPositionLimit[:7] = 1.
q = pin.randomConfiguration(model)
pin.computeStaticRegressor(model, data, q)
phi = zero(4 * (model.njoints - 1))
for k in range(1, model.njoints):
Y = model.inertias[k]
phi[4 * (k - 1)] = Y.mass
phi[4 * k - 3:4 * k] = Y.mass * Y.lever
static_com_ref = pin.centerOfMass(model, data_ref, q)
static_com = data.staticRegressor * phi
self.assertApprox(static_com, static_com_ref)
def test_joint_torque_regressor(self):
model = pin.buildSampleModelHumanoidRandom()
model.lowerPositionLimit[:7] = -1.
model.upperPositionLimit[:7] = 1.
data = model.createData()
data_ref = model.createData()
q = pin.randomConfiguration(model)
v = pin.utils.rand(model.nv)
a = pin.utils.rand(model.nv)
pin.rnea(model,data_ref,q,v,a)
params = zero(10*(model.njoints-1))
for i in range(1, model.njoints):
params[(i-1)*10:i*10] = model.inertias[i].toDynamicParameters()
pin.computeJointTorqueRegressor(model,data,q,v,a)
tau_regressor = data.jointTorqueRegressor.dot(params)
self.assertApprox(tau_regressor, data_ref.tau)
# Loading Talos arm with FF TODO use a biped or quadruped
# -----------------------------------------------------------------------------
robot = loadANYmal()
robot.model.armature[6:] = 1.
qmin = robot.model.lowerPositionLimit
qmin[:7] = -1
robot.model.lowerPositionLimit = qmin
qmax = robot.model.upperPositionLimit
qmax[:7] = 1
robot.model.upperPositionLimit = qmax
rmodel = robot.model
rdata = rmodel.createData()
# -----------------------------------------
q = pinocchio.randomConfiguration(rmodel)
v = rand(rmodel.nv)
x = m2a(np.concatenate([q, v]))
u = m2a(rand(rmodel.nv - 6))
# -------------------------------------------------
np.set_printoptions(linewidth=400, suppress=True)
State = StatePinocchio(rmodel)
actModel = ActuationModelFreeFloating(State)
gains = pinocchio.utils.rand(2)
Mref_lf = FramePlacement(rmodel.getFrameId('LF_FOOT'), pinocchio.SE3.Random())
contactModel6 = ContactModel6D(State, Mref_lf, actModel.nu, gains)
rmodel.frames[Mref_lf.frame].placement = pinocchio.SE3.Random()
contactModel = ContactModelMultiple(State, actModel.nu)
## ## In this short script, we show how to compute the derivatives of the ## forward dynamics, using the algorithms explained in: ## ## Analytical Derivatives of Rigid Body Dynamics Algorithms, Justin Carpentier and Nicolas Mansard, Robotics: Science and Systems, 2018 ## # Create model and data model = pin.buildSampleModelHumanoidRandom() data = model.createData() # Set bounds (by default they are undefinded for a the Simple Humanoid model) model.lowerPositionLimit = -np.matrix(np.ones((model.nq, 1))) model.upperPositionLimit = np.matrix(np.ones((model.nq, 1))) q = pin.randomConfiguration(model) # joint configuration v = np.matrix(np.random.rand(model.nv, 1)) # joint velocity tau = np.matrix(np.random.rand(model.nv, 1)) # joint acceleration # Evaluate the derivatives pin.computeABADerivatives(model, data, q, v, tau) # Retrieve the derivatives in data ddq_dq = data.ddq_dq # Derivatives of the FD w.r.t. the joint config vector ddq_dv = data.ddq_dv # Derivatives of the FD w.r.t. the joint velocity vector ddq_dtau = data.Minv # Derivatives of the FD w.r.t. the joint acceleration vector
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 rand(self):
q = pinocchio.randomConfiguration(self.model)
v = pinocchio.utils.rand(self.nv)
return np.concatenate([q, v])
def setUp(self):
self.model = pin.buildSampleModelHumanoidRandom()
self.data = self.model.createData()
qmax = np.matrix(np.full((self.model.nq,1),np.pi))
self.q = pin.randomConfiguration(self.model,-qmax,qmax)
NON_ACTIVE_COLLISION_PAIRS += [i]
robot.deactivateCollisionPairs(NON_ACTIVE_COLLISION_PAIRS)
dcrba = DCRBA(robot)
coriolis = Coriolis(robot)
drnea = DRNEA(robot)
robot.initDisplay(loadModel=True)
#robot.loadDisplayModel("world/pinocchio", "pinocchio", model_path)
robot.viewer.gui.setLightingMode('world/floor', 'OFF')
#print robot.model
Q_MIN = robot.model.lowerPositionLimit
Q_MAX = robot.model.upperPositionLimit
DQ_MAX = robot.model.velocityLimit
TAU_MAX = robot.model.effortLimit
q = se3.randomConfiguration(robot.model, Q_MIN, Q_MAX)
robot.display(q)
# Display the robot in Gepetto-Viewer.
nq = robot.nq
nv = robot.nv
v = mat_zeros(robot.nv)
dv = mat_zeros(robot.nv)
ddq_ub_min = mat_zeros(nq) + 1e10
ddq_lb_max = mat_zeros(nq) - 1e10
M_max = mat_zeros((nv, nv)) - 1e10
M_min = mat_zeros((nv, nv)) + 1e10
h_max = mat_zeros(nv) - 1e10
h_min = mat_zeros(nv) + 1e10
dh_dq_max = mat_zeros((nv, nq)) - 1e10
dh_dq_min = mat_zeros((nv, nq)) + 1e10
## ## In this short script, we show how to compute the derivatives of the ## forward dynamics, using the algorithms explained in: ## ## Analytical Derivatives of Rigid Body Dynamics Algorithms, Justin Carpentier and Nicolas Mansard, Robotics: Science and Systems, 2018 ## # Create model and data model = pin.buildSampleModelHumanoidRandom() data = model.createData() # Set bounds (by default they are undefinded for a the Simple Humanoid model) model.lowerPositionLimit = -np.matrix(np.ones((model.nq,1))) model.upperPositionLimit = np.matrix(np.ones((model.nq,1))) q = pin.randomConfiguration(model) # joint configuration v = np.matrix(np.random.rand(model.nv,1)) # joint velocity tau = np.matrix(np.random.rand(model.nv,1)) # joint acceleration # Evaluate the derivatives pin.computeABADerivatives(model,data,q,v,tau) # Retrieve the derivatives in data ddq_dq = data.ddq_dq # Derivatives of the FD w.r.t. the joint config vector ddq_dv = data.ddq_dv # Derivatives of the FD w.r.t. the joint velocity vector ddq_dtau = data.Minv # Derivatives of the FD w.r.t. the joint acceleration vector
print "" import os filename = str(os.path.dirname(os.path.abspath(__file__))) mesh_dir = filename + '/../models/romeo' urdf_model_path = mesh_dir + '/urdf/romeo.urdf' vector = se3.StdVec_StdString() vector.extend(item for item in mesh_dir) robot = tsid.RobotWrapper(urdf_model_path, vector, se3.JointModelFreeFlyer(), False) # model = robot.model() model, collision_model, visual_model = se3.buildModelsFromUrdf(urdf_model_path, mesh_dir, se3.JointModelFreeFlyer()) lb = model.lowerPositionLimit lb[0:3] = -10.0*np.matrix(np.ones(3)).transpose() lb[3:7] = -1.0*np.matrix(np.ones(4)).transpose() ub = model.upperPositionLimit ub[0:3] = 10.0*np.matrix(np.ones(3)).transpose() ub[3:7] = 1.0*np.matrix(np.ones(4)).transpose() q = se3.randomConfiguration(model, lb, ub) print q.transpose() # robot.data() data = model.createData() v = np.matrix(np.ones(robot.nv)).transpose() robot.computeAllTerms(data, q, v) print robot.com(data) print "All test is done"
def setUp(self):
self.model = pin.buildSampleModelHumanoidRandom()
qmax = np.full((self.model.nq, 1), np.pi)
self.q = pin.randomConfiguration(self.model, -qmax, qmax)
self.v = np.random.rand(self.model.nv)
M_dist = pin.crba(rmodel_dist, rdata_dist, r.q0)
print('M_dist - M')
print(M_dist[:6, :6] - M[:6, :6])
# save the result as a txt file:
rmodel_dist.saveToText(TXT_FILE)
if PLOT_DISPARITY:
# funny plots
# number of configuration to sample
N = 5000
biases = np.zeros((N, 3))
for i in range(N):
q = pin.randomConfiguration(rmodel)
# Base at the origin
q[:6] = 0
q[6] = 1
p_bc = pin.centerOfMass(rmodel, rdata, q)
p_bc_dist = pin.centerOfMass(rmodel_dist, rdata_dist, q)
biases[i, :] = p_bc_dist - p_bc
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
X, Y, Z = biases[:, 0], biases[:, 1], biases[:, 2]
# 2D figures
plt.figure('y=f(x)')
plt.scatter(X, Y, 'rx')
