def test_setZero(self):
Y = pin.Inertia.Zero()
Y.setRandom()
Y.setZero()
self.assertTrue(Y.mass == 0)
self.assertTrue(np.allclose(zero(3), Y.lever))
self.assertTrue(np.allclose(zero([3,3]), Y.inertia))
def t_poseDisplay(self):
q = zero(self.model.nq)
q[6] = 1
v = zero(self.model.nv)
# Right Arm
# This is used to obtain the index of the joint that will be rotated
idx = self.model.getJointId('shoulder_r')
idx = self.model.joints[idx].idx_q
# The shoulder is a spherical joint expressed as quaterion to avoid the singularities of Euler angles
# We first rotate the DoF in se3
M = se3.SE3.Identity()
M.rotation = rotate('y', -np.pi / 2)
# Now we convert it in a quaternion
Mquat = se3ToXYZQUAT(M)
q[idx] = Mquat[3]
q[idx + 1] = Mquat[4]
q[idx + 2] = Mquat[5]
q[idx + 3] = Mquat[6]
# Rotate left arm
idx = self.model.getJointId('shoulder_l')
idx = self.model.joints[idx].idx_q
M = se3.SE3.Identity()
M.rotation = rotate('y', np.pi / 2)
Mquat = se3ToXYZQUAT(M)
q[idx] = Mquat[3]
q[idx + 1] = Mquat[4]
q[idx + 2] = Mquat[5]
q[idx + 3] = Mquat[6]
# Now the forward dynamics is computed to obtain the T pose
self.display(q, v)
self.q = q
def test_explog(self):
self.assertApprox(exp(42), math.exp(42))
self.assertApprox(log(42), math.log(42))
self.assertApprox(exp(log(42)), 42)
self.assertApprox(log(exp(42)), 42)
m = rand(3)
self.assertTrue(np.linalg.norm(m) < np.pi) # necessary for next test
self.assertApprox(log(exp(m)), m)
m = pin.SE3.Random()
self.assertApprox(exp(log(m)), m)
m = rand(6)
self.assertTrue(np.linalg.norm(m) < np.pi) # necessary for next test (actually, only angular part)
self.assertApprox(log(exp(m)), m)
m = eye(4)
self.assertApprox(exp(log(m)).homogeneous, m)
with self.assertRaises(ValueError):
exp(eye(4))
with self.assertRaises(ValueError):
exp(list(range(3)))
with self.assertRaises(ValueError):
log(list(range(3)))
with self.assertRaises(ValueError):
log(zero(5))
with self.assertRaises(ValueError):
log(zero((3,1)))
def PlaceForceArrow(self,
name,
oMc,
cPhic,
color_linear_force=None,
refreshGui=False):
name_linForce = "force_" + name
name_torque = "torque_" + name
linForce = cPhic[:3, 0]
torque = cPhic[3:, 0]
val_linForce = max(LA.norm(linForce), 1e-4)
val_torque = max(LA.norm(torque), 1e-4)
dir_linForce = linForce / val_linForce
dir_torque = torque / val_torque
dir0 = np.matrix([1., 0., 0.]).T
oMlinForce = oMc * se3.SE3(rotation_matrix(dir0, dir_linForce),
zero(3))
oMtorque = oMc * se3.SE3(rotation_matrix(dir0, dir_torque), zero(3))
# divid by gravity to normalize the forces
self.viewer.gui.resizeArrow("world/" + name_linForce,
self.force_arrow_radius,
val_linForce / 728.22)
self.viewer.gui.resizeArrow("world/" + name_torque,
self.force_arrow_radius,
val_torque / 728.22)
# set color for linear force
if color_linear_force is not None:
self.viewer.gui.setColor("world/" + name_linForce,
color_linear_force)
self.viewer.gui.applyConfiguration("world/" + name_linForce,
se3ToXYZQUAT(oMlinForce))
self.viewer.gui.applyConfiguration("world/" + name_torque,
se3ToXYZQUAT(oMtorque))
if refreshGui:
self.viewer.gui.refresh()
def test_explog(self):
self.assertApprox(exp(42), math.exp(42))
self.assertApprox(log(42), math.log(42))
self.assertApprox(exp(log(42)), 42)
self.assertApprox(log(exp(42)), 42)
m = rand(3)
self.assertTrue(np.linalg.norm(m) < np.pi) # necessary for next test
self.assertApprox(log(exp(m)), m)
m = pin.SE3.Random()
self.assertApprox(exp(log(m)), m)
m = rand(6)
self.assertTrue(
np.linalg.norm(m) <
np.pi) # necessary for next test (actually, only angular part)
self.assertApprox(log(exp(m)), m)
m = eye(4)
self.assertApprox(exp(log(m)).homogeneous, m)
with self.assertRaises(ValueError):
exp(eye(4))
with self.assertRaises(ValueError):
exp(list(range(3)))
with self.assertRaises(ValueError):
log(list(range(3)))
with self.assertRaises(ValueError):
log(zero(5))
with self.assertRaises(ValueError):
log(zero((3, 1)))
def get_state(self):
# Returns a pinocchio like representation of the q, dq matrixes
q = zero(self.nq)
dq = zero(self.nv)
if not self.useFixedBase:
pos, orn = p.getBasePositionAndOrientation(self.robot_id)
q[:3] = pos
q[3:7] = orn
vel, orn = p.getBaseVelocity(self.robot_id)
dq[:3] = vel
dq[3:6] = orn
# Pinocchio assumes base velocity to be in body frame -> rotate.
rot = np.array(p.getMatrixFromQuaternion(q[3:7])).reshape((3, 3))
dq[0:3] = rot.T.dot(dq[0:3])
dq[3:6] = rot.T.dot(dq[3:6])
# Query the joint readings.
joint_states = p.getJointStates(self.robot_id, self.bullet_joint_ids)
if not self.useFixedBase:
for i in range(self.nj):
q[5 + self.pinocchio_joint_ids[i]] = joint_states[i][0]
dq[4 + self.pinocchio_joint_ids[i]] = joint_states[i][1]
else:
for i in range(self.nj):
q[self.pinocchio_joint_ids[i] - 1] = joint_states[i][0]
dq[self.pinocchio_joint_ids[i] - 1] = joint_states[i][1]
return q, dq
def __init__(self,
model_path=None,
mesh_path=None,
name='Robot',
OsimModel=True):
self.name = name
if model_path is None:
model_path = '/local/gmaldona/devel/biomechatronics/models/GX.osim'
r = model_parser.parseModel()
self.mesh_path = mesh_path
self.model_path = model_path
r.parseModel(self.model_path, self.mesh_path)
self.upperPositionLimitOsim = r.upperPositionLimit
self.upperPositionLimitOsim = r.upperPositionLimit
self.model = r.model
self.visuals = r.visuals
self.data = r.data
self.v0 = zero(self.model.nv)
self.q0 = zero(self.model.nq)
self.q = self.q0
self.oMp = se3.utils.rotate('z', np.pi / 2) * se3.utils.rotate(
'x', np.pi / 2)
self.markersFreq = np.float(400.)
self.dt = np.float(1 / 400.)
self.v = zero(self.model.nv)
self.a = zero(self.model.nv)
self.fext = self.data.hg.vector.copy()
def get_state(self):
# Returns a pinocchio like representation of the q, dq matrixes
q = zero(self.nq)
dq = zero(self.nv)
pos, orn = p.getBasePositionAndOrientation(self.robot_id)
q[:3, 0] = np.array(pos).reshape(3, 1)
q[3:7, 0] = np.array(orn).reshape(4, 1)
vel, orn = p.getBaseVelocity(self.robot_id)
dq[:3, 0] = np.array(vel).reshape(3, 1)
dq[3:6, 0] = np.array(orn).reshape(3, 1)
# Pinocchio assumes the base velocity to be in the body frame -> rotate.
rot = np.matrix(p.getMatrixFromQuaternion(q[3:7])).reshape((3, 3))
dq[0:3] = rot.T.dot(dq[0:3])
dq[3:6] = rot.T.dot(dq[3:6])
# Query the joint readings.
joint_states = p.getJointStates(self.robot_id, self.bullet_joint_ids)
for i in range(self.nj):
q[5 + self.pinocchio_joint_ids[i], 0] = joint_states[i][0]
dq[4 + self.pinocchio_joint_ids[i], 0] = joint_states[i][1]
return q, dq
def test_setZero(self):
f = pin.Force.Zero()
f.setRandom()
f.setZero()
self.assertTrue(np.allclose(zero(3), f.linear))
self.assertTrue(np.allclose(zero(3), f.angular))
self.assertTrue(np.allclose(zero(6), f.vector))
def test_setZero(self):
v = se3.Motion.Zero()
v.setRandom()
v.setZero()
self.assertTrue(np.allclose(zero(3), v.linear))
self.assertTrue(np.allclose(zero(3), v.angular))
self.assertTrue(np.allclose(zero(6), v.vector))
def test_setZero(self):
f = pin.Force.Zero()
f.setRandom()
f.setZero()
self.assertTrue(np.allclose(zero(3), f.linear))
self.assertTrue(np.allclose(zero(3), f.angular))
self.assertTrue(np.allclose(zero(6), f.vector))
def test_setZero(self):
Y = pin.Inertia.Zero()
Y.setRandom()
Y.setZero()
self.assertTrue(Y.mass == 0)
self.assertTrue(np.allclose(zero(3), Y.lever))
self.assertTrue(np.allclose(zero([3, 3]), Y.inertia))
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 __init__(self):
self.viewer = Display()
self.visuals = []
self.model = pin.Model()
self.createHand()
self.data = self.model.createData()
self.q0 = zero(self.model.nq)
# self.q0[3] = 1.0
self.v0 = zero(self.model.nv)
self.collisionPairs = []
def test_motion(self):
m = self.m
self.assertApprox(se3.Motion.Zero().vector, zero(6))
v = se3.Motion.Random()
self.assertApprox((m * v).vector, m.action * v.vector)
self.assertApprox((m.actInv(v)).vector, npl.inv(m.action) * v.vector)
vv = v.linear
vw = v.angular
self.assertApprox(v.vector, np.vstack([vv, vw]))
self.assertApprox((v ^ v).vector, zero(6))
def test_motion(self):
m = self.m
self.assertApprox(se3.Motion.Zero().vector, zero(6))
v = se3.Motion.Random()
self.assertApprox((m * v).vector, m.action * v.vector)
self.assertApprox((m.actInv(v)).vector, npl.inv(m.action) * v.vector)
vv = v.linear
vw = v.angular
self.assertApprox(v.vector, np.vstack([vv, vw]))
self.assertApprox((v ** v).vector, zero(6))
def __init__(self):
self.viewer = Display()
self.visuals = []
self.model = pin.Model()
self.createHand()
self.data = self.model.createData()
self.q0 = zero(self.model.nq)
# self.q0[3] = 1.0
self.v0 = zero(self.model.nv)
self.collisionPairs = []
def test_motion(self):
m = self.m
self.assertApprox(pin.Motion.Zero().vector, zero(6))
v = pin.Motion.Random()
self.assertApprox((m * v).vector, m.action.dot(v.vector))
self.assertApprox((m.actInv(v)).vector,
npl.inv(m.action).dot(v.vector))
vv = v.linear
vw = v.angular
self.assertApprox(v.vector, np.concatenate([vv, vw]))
self.assertApprox((v ^ v).vector, zero(6))
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 __init__(self, variableSize, nbVariables):
'''
Initialize a QP sparse problem as min || A x - b || so that C x = d
where x = (x1, .., xn), and dim(xi) = variableSize and n = nbVariables
After construction, A, b, C and d are allocated and set to 0.
'''
self.nx = variableSize
self.N = nbVariables
self.A = zero([0, self.N * self.nx])
self.b = zero(0)
self.C = zero([0, self.N * self.nx])
self.d = zero(0)
def __init__(self, variableSize, nbVariables):
'''
Initialize a QP sparse problem as min || A x - b || so that C x = d
where x = (x1, .., xn), and dim(xi) = variableSize and n = nbVariables
After construction, A, b, C and d are allocated and set to 0.
'''
self.nx = variableSize
self.N = nbVariables
self.A = zero([0, self.N * self.nx])
self.b = zero(0)
self.C = zero([0, self.N * self.nx])
self.d = zero(0)
def test_force(self):
m = self.m
self.assertApprox(pin.Force.Zero().vector, zero(6))
f = pin.Force.Random()
ff = f.linear
ft = f.angular
self.assertApprox(f.vector, np.concatenate([ff, ft]))
self.assertApprox((m * f).vector, npl.inv(m.action.T).dot(f.vector))
self.assertApprox((m.actInv(f)).vector, m.action.T.dot(f.vector))
v = pin.Motion.Random()
f = pin.Force(np.concatenate([v.vector[3:], v.vector[:3]]))
self.assertApprox((v ^ f).vector, zero(6))
def test_force(self):
m = self.m
self.assertApprox(se3.Force.Zero().vector, zero(6))
f = se3.Force.Random()
ff = f.linear
ft = f.angular
self.assertApprox(f.vector, np.vstack([ff, ft]))
self.assertApprox((m * f).vector, npl.inv(m.action.T) * f.vector)
self.assertApprox((m.actInv(f)).vector, m.action.T * f.vector)
v = se3.Motion.Random()
f = se3.Force(np.vstack([v.vector[3:], v.vector[:3]]))
self.assertApprox((v ** f).vector, zero(6))
def __init__(self, filename, mesh_path, name='the_model_wrapper'):
self.name = name
ms_system = mdp.parseModel(filename, mesh_path)
self.model = ms_system.model
self.data = ms_system.data
self.visuals = ms_system.visuals
self.forces = ms_system.forces
self.joint_transformations = ms_system.joint_transformations
self.v0 = zero(self.model.nv) #TODO get from model
self.q0 = zero(self.model.nq) #TODO get from model
self.q = self.q0
self.dq = zero(self.model.nv)
self.ddq = zero(self.model.nv)
def test_force(self):
m = self.m
self.assertApprox(se3.Force.Zero().vector, zero(6))
f = se3.Force.Random()
ff = f.linear
ft = f.angular
self.assertApprox(f.vector, np.vstack([ff, ft]))
self.assertApprox((m * f).vector, npl.inv(m.action.T) * f.vector)
self.assertApprox((m.actInv(f)).vector, m.action.T * f.vector)
v = se3.Motion.Random()
f = se3.Force(np.vstack([v.vector[3:], v.vector[:3]]))
self.assertApprox((v ^ f).vector, zero(6))
def get_state(self):
"""Returns a pinocchio-like representation of the q, dq matrices. Note that the base velocities are expressed in the base frame.
Returns:
ndarray: Generalized positions.
ndarray: Generalized velocities.
"""
q = zero(self.nq)
dq = zero(self.nv)
if not self.useFixedBase:
base_inertia_pos, base_inertia_quat = pybullet.getBasePositionAndOrientation(
self.robot_id)
# Get transform between inertial frame and link frame in base
base_stat = pybullet.getDynamicsInfo(self.robot_id, -1)
base_inertia_link_pos, base_inertia_link_quat = pybullet.invertTransform(
base_stat[3], base_stat[4])
pos, orn = pybullet.multiplyTransforms(base_inertia_pos,
base_inertia_quat,
base_inertia_link_pos,
base_inertia_link_quat)
q[:3] = pos
q[3:7] = orn
vel, orn = pybullet.getBaseVelocity(self.robot_id)
dq[:3] = vel
dq[3:6] = orn
# Pinocchio assumes the base velocity to be in the body frame -> rotate.
rot = np.array(pybullet.getMatrixFromQuaternion(q[3:7])).reshape(
(3, 3))
dq[0:3] = rot.T.dot(dq[0:3])
dq[3:6] = rot.T.dot(dq[3:6])
# Query the joint readings.
joint_states = pybullet.getJointStates(self.robot_id,
self.bullet_joint_ids)
if not self.useFixedBase:
for i in range(self.nj):
q[5 + self.pinocchio_joint_ids[i]] = joint_states[i][0]
dq[4 + self.pinocchio_joint_ids[i]] = joint_states[i][1]
else:
for i in range(self.nj):
q[self.pinocchio_joint_ids[i] - 1] = joint_states[i][0]
dq[self.pinocchio_joint_ids[i] - 1] = joint_states[i][1]
return q, dq
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 test_se3_action(self):
m = pin.SE3.Random()
v = pin.Motion.Random()
self.assertTrue(np.allclose((m * v).vector, m.action * v.vector))
self.assertTrue(np.allclose(m.act(v).vector, m.action * v.vector))
self.assertTrue(np.allclose((m.actInv(v)).vector, np.linalg.inv(m.action) * v.vector))
self.assertTrue(np.allclose((v ^ v).vector, zero(6)))
def test_se3(self):
R, p, m = self.R, self.p, self.m
X = np.vstack(
[np.hstack([R, pin.skew(p).dot(R)]),
np.hstack([zero([3, 3]), R])])
self.assertApprox(m.action, X)
M = np.vstack([
np.hstack([R, np.expand_dims(p, 1)]),
np.array([[0., 0., 0., 1.]])
])
self.assertApprox(m.homogeneous, M)
m2 = pin.SE3.Random()
self.assertApprox((m * m2).homogeneous,
m.homogeneous.dot(m2.homogeneous))
self.assertApprox((~m).homogeneous, npl.inv(m.homogeneous))
p = rand(3)
self.assertApprox(m * p, m.rotation.dot(p) + m.translation)
self.assertApprox(
m.actInv(p),
m.rotation.T.dot(p) - m.rotation.T.dot(m.translation))
# Currently, the different cases do not throw the same exception type.
# To have a more robust test, only Exception is checked.
# In the comments, the most specific actual exception class at the time of writing
p = rand(5)
with self.assertRaises(Exception): # RuntimeError
m * p
with self.assertRaises(Exception): # RuntimeError
m.actInv(p)
with self.assertRaises(
Exception
): # Boost.Python.ArgumentError (subclass of TypeError)
m.actInv('42')
def test_se3(self):
R, p, m = self.R, self.p, self.m
X = np.vstack([np.hstack([R, skew(p) * R]), np.hstack([zero([3, 3]), R])])
self.assertApprox(m.action, X)
M = np.vstack([np.hstack([R, p]), np.matrix([0., 0., 0., 1.], np.double)])
self.assertApprox(m.homogeneous, M)
m2 = pin.SE3.Random()
self.assertApprox((m * m2).homogeneous, m.homogeneous * m2.homogeneous)
self.assertApprox((~m).homogeneous, npl.inv(m.homogeneous))
p = rand(3)
self.assertApprox(m * p, m.rotation * p + m.translation)
self.assertApprox(m.actInv(p), m.rotation.T * p - m.rotation.T * m.translation)
## not supported
# p = np.vstack([p, 1])
# self.assertApprox(m * p, m.homogeneous * p)
# self.assertApprox(m.actInv(p), npl.inv(m.homogeneous) * p)
## not supported
# p = rand(6)
# self.assertApprox(m * p, m.action * p)
# self.assertApprox(m.actInv(p), npl.inv(m.action) * p)
# Currently, the different cases do not throw the same exception type.
# To have a more robust test, only Exception is checked.
# In the comments, the most specific actual exception class at the time of writing
p = rand(5)
with self.assertRaises(Exception): # RuntimeError
m * p
with self.assertRaises(Exception): # RuntimeError
m.actInv(p)
with self.assertRaises(Exception): # Boost.Python.ArgumentError (subclass of TypeError)
m.actInv('42')
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 policyOptim(x0=None):
X, U = rollout(x0)
guess = np.hstack([
np.matrix(np.arange(0, (NSTEPS + 1) * env.DT, env.DT)).T, X, U,
zero(NSTEPS + 1),
zero(NSTEPS + 1)
])[::FNODES]
np.savetxt('/tmp/state.txt', guess)
acado.options['istate'] = '/tmp/state.txt'
if 'icontrol' in acado.options:
del acado.options['icontrol']
u, cost = acado.run(X[0, :1], X[0, 1:])
return u, cost, X[0, :]
def df_dx(func, x, h=np.sqrt(2 * EPS)):
""" Perform df/dx by num_diff.
:params func: function to differentiate f : np.matrix -> np.matrix
:params x: value at which f is differentiated. type np.matrix
:params h: eps
:returns df/dx
"""
dx = zero(x.size)
f0 = func(x)
res = zero([len(f0), x.size])
for ix in range(x.size):
dx[ix] = h
res[:, ix] = (func(x + dx) - f0) / h
dx[ix] = 0
return res
def test_se3(self):
R, p, m = self.R, self.p, self.m
X = np.vstack(
[np.hstack([R, skew(p) * R]),
np.hstack([zero([3, 3]), R])])
self.assertApprox(m.action, X)
M = np.vstack([np.hstack([R, p]), np.matrix('0 0 0 1', np.double)])
self.assertApprox(m.homogeneous, M)
m2 = se3.SE3.Random()
self.assertApprox((m * m2).homogeneous, m.homogeneous * m2.homogeneous)
self.assertApprox((~m).homogeneous, npl.inv(m.homogeneous))
p = rand(3)
self.assertApprox(m * p, m.rotation * p + m.translation)
self.assertApprox(m.actInv(p),
m.rotation.T * p - m.rotation.T * m.translation)
p = np.vstack([p, 1])
self.assertApprox(m * p, m.homogeneous * p)
self.assertApprox(m.actInv(p), npl.inv(m.homogeneous) * p)
p = rand(6)
self.assertApprox(m * p, m.action * p)
self.assertApprox(m.actInv(p), npl.inv(m.action) * p)
p = rand(5)
with self.assertRaises(ValueError):
m * p
with self.assertRaises(ValueError):
m.actInv(p)
with self.assertRaises(ValueError):
m.actInv('42')
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 test_se3(self):
R, p, m = self.R, self.p, self.m
X = np.vstack([np.hstack([R, skew(p) * R]), np.hstack([zero([3, 3]), R])])
self.assertApprox(m.action, X)
M = np.vstack([np.hstack([R, p]), np.matrix('0 0 0 1', np.double)])
self.assertApprox(m.homogeneous, M)
m2 = se3.SE3.Random()
self.assertApprox((m * m2).homogeneous, m.homogeneous * m2.homogeneous)
self.assertApprox((~m).homogeneous, npl.inv(m.homogeneous))
p = rand(3)
self.assertApprox(m * p, m.rotation * p + m.translation)
self.assertApprox(m.actInv(p), m.rotation.T * p - m.rotation.T * m.translation)
p = np.vstack([p, 1])
self.assertApprox(m * p, m.homogeneous * p)
self.assertApprox(m.actInv(p), npl.inv(m.homogeneous) * p)
p = rand(6)
self.assertApprox(m * p, m.action * p)
self.assertApprox(m.actInv(p), npl.inv(m.action) * p)
p = rand(5)
with self.assertRaises(ValueError):
m * p
with self.assertRaises(ValueError):
m.actInv(p)
with self.assertRaises(ValueError):
m.actInv('42')
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 rotateREVJ(self, q, axis, angle, idx):
#M = se3.SE3.Identity()
#M.rotation = self.data.oMi[idx].rotation * rotate(axis, angle)
#Mquat = se3ToXYZQUAT(M)
v = zero(self.model.nv)
q[idx] = angle
self.play(q, v)
def df_dq(model, func, q, h=np.sqrt(2 * EPS)):
""" Perform df/dq by num_diff. q is in the lie manifold.
:params func: function to differentiate f : np.matrix -> np.matrix
:params q: configuration value at which f is differentiated. type np.matrix
:params h: eps
:returns df/dq
"""
dq = zero(model.nv)
f0 = func(q)
res = zero([len(f0), model.nv])
for iq in range(model.nv):
dq[iq] = h
res[:, iq] = (func(pinocchio.integrate(model, q, dq)) - f0) / h
dq[iq] = 0
return res
def test_se3_action(self):
f = pin.Force.Random()
m = pin.SE3.Random()
self.assertTrue(np.allclose((m * f).vector, np.linalg.inv(m.action.T) * f.vector))
self.assertTrue(np.allclose(m.act(f).vector, np.linalg.inv(m.action.T) * f.vector))
self.assertTrue(np.allclose((m.actInv(f)).vector, m.action.T * f.vector))
v = pin.Motion(np.vstack([f.vector[3:], f.vector[:3]]))
self.assertTrue(np.allclose((v ^ f).vector, zero(6)))
def test_se3_action(self):
f = se3.Force.Random()
m = se3.SE3.Random()
self.assertTrue(np.allclose((m * f).vector, np.linalg.inv(m.action.T) * f.vector))
self.assertTrue(np.allclose((m.actInv(f)).vector, m.action.T * f.vector))
v = se3.Motion.Random()
f = se3.Force(np.vstack([v.vector[3:], v.vector[:3]]))
self.assertTrue(np.allclose((v ** f).vector, zero(6)))
def test_se3_action(self):
f = pin.Force.Random()
m = pin.SE3.Random()
self.assertTrue(np.allclose((m * f).vector, np.linalg.inv(m.action.T) * f.vector))
self.assertTrue(np.allclose(m.act(f).vector, np.linalg.inv(m.action.T) * f.vector))
self.assertTrue(np.allclose((m.actInv(f)).vector, m.action.T * f.vector))
v = pin.Motion(np.vstack([f.vector[3:], f.vector[:3]]))
self.assertTrue(np.allclose((v ^ f).vector, zero(6)))
def test_action_matrix(self):
amb = pin.SE3.Random()
aXb = amb.action
self.assertTrue(np.allclose(aXb[:3,:3], amb.rotation)) # top left 33 corner = rotation of amb
self.assertTrue(np.allclose(aXb[3:,3:], amb.rotation)) # bottom right 33 corner = rotation of amb
tblock = skew(amb.translation)*amb.rotation
self.assertTrue(np.allclose(aXb[:3,3:], tblock)) # top right 33 corner = translation + rotation
self.assertTrue(np.allclose(aXb[3:,:3], zero([3,3]))) # bottom left 33 corner = 0
def test_identity(self):
transform = pin.SE3.Identity()
self.assertTrue(np.allclose(zero(3),transform.translation))
self.assertTrue(np.allclose(eye(3), transform.rotation))
self.assertTrue(np.allclose(eye(4), transform.homogeneous))
self.assertTrue(np.allclose(eye(6), transform.action))
transform.setRandom()
transform.setIdentity()
self.assertTrue(np.allclose(eye(4), transform.homogeneous))
def matrix_form_factor(self, factors):
'''
Internal function: not designed to be called by the user.
Create a factor matrix [ A1 0 A2 0 A3 ... ] where the Ai's are placed at
the indexes of the factors.
'''
assert(len(factors) > 0)
nr = factors[0].matrix.shape[0] # nb rows of the factor
nc = self.nx * self.N # nb cols
# Define and fill the new rows to be added
A = zero([nr, nc]) # new factor to be added to self.A
for factor in factors:
assert(factor.matrix.shape == (nr, self.nx))
A[:, self.nx * factor.index:self.nx * (factor.index + 1)] = factor.matrix
return A
def test_log3(self):
m = eye(3)
v = pin.log3(m)
self.assertApprox(v, zero(3))
def test_log6_homogeneous(self):
m = eye(4)
v = pin.log6(m)
self.assertApprox(v.vector, zero(6))
and add the corresponding visuals in gepetto viewer with name prefix given by string <name>.
It returns the robot wrapper (model,data).
'''
robot = RobotWrapper(urdf, [PKG])
robot.model.jointPlacements[1] = M0 * robot.model.jointPlacements[1]
robot.visual_model.geometryObjects[0].placement = M0 * robot.visual_model.geometryObjects[0].placement
robot.visual_data.oMg[0] = M0 * robot.visual_data.oMg[0]
robot.initDisplay(loadModel=True, viewerRootNodeName="world/" + name)
return robot
robots = []
# Load 4 Ur5 robots, placed at 0.3m from origin in the 4 directions x,y,-x,-y.
Mt = SE3(eye(3), np.matrix([.3, 0, 0]).T) # First robot is simply translated
for i in range(4):
robots.append(loadRobot(SE3(rotate('z', np.pi / 2 * i), zero(3)) * Mt, "robot%d" % i))
# Set up the robots configuration with end effector pointed upward.
q0 = np.matrix([np.pi / 4, -np.pi / 4, -np.pi / 2, np.pi / 4, np.pi / 2, 0]).T
for i in range(4):
robots[i].display(q0)
# Add a new object featuring the parallel robot tool plate.
gepettoViewer = robots[0].viewer.gui
w, h, d = 0.25, 0.25, 0.005
color = [red, green, blue, transparency] = [1, 1, 0.78, 1.0]
gepettoViewer.addBox('world/toolplate', w, h, d, color)
Mtool = SE3(rotate('z', 1.268), np.matrix([0, 0, .77]).T)
gepettoViewer.applyConfiguration('world/toolplate', se3ToXYZQUATtuple(Mtool))
gepettoViewer.refresh()
#ancestorsOf.__init__.__defaults__ = (robot,)
iv.__defaults__ = (robot,)
parent.__defaults__ = (robot,)
jFromIdx.__defaults__ = (robot,)
# --- SE3 operators
Mcross = lambda x,y: Motion(x).cross(Motion(y)).vector
Mcross.__doc__ = "Motion cross product"
Fcross = lambda x,y: Motion(x).cross(Force(y)).vector
Fcross.__doc__ = "Force cross product"
MCross = lambda V,v: np.bmat([ Mcross(V[:,i],v) for i in range(V.shape[1]) ])
FCross = lambda V,f: np.bmat([ Fcross(V[:,i],f) for i in range(V.shape[1]) ])
adj = lambda nu: np.bmat([[ skew(nu[3:]),skew(nu[:3])],[zero([3,3]),skew(nu[3:])]])
adj.__doc__ = "Motion pre-cross product (ie adjoint, lie bracket operator)"
adjdual = lambda nu: np.bmat([[ skew(nu[3:]),zero([3,3])],[skew(nu[:3]),skew(nu[3:])]])
adjdual.__doc__ = "Force pre-cross product adjdual(a) = -adj(a)' "
td = np.tensordot
quad = lambda H,v: np.matrix(td(td(H,v,[2,0]),v,[1,0])).T
quad.__doc__ = '''Tensor product v'*H*v, with H.shape = [ nop, nv, nv ]'''
def np_prettyprint(sarg = '{: 0.5f}',eps=5e-7):
mformat = lambda x,sarg = sarg,eps=eps: sarg.format(x) if abs(x)>eps else ' 0. '
np.set_printoptions(formatter={'float': mformat})
def test_Jexp3(self):
v = zero(3)
m = pin.Jexp3(v)
self.assertApprox(m, eye(3))
def test_setIdentity(self):
Y = pin.Inertia.Zero()
Y.setIdentity()
self.assertTrue(Y.mass == 1)
self.assertTrue(np.allclose(zero(3), Y.lever))
self.assertTrue(np.allclose(eye(3), Y.inertia))
def test_zero_getters(self):
Y = pin.Inertia.Zero()
self.assertTrue(Y.mass == 0)
self.assertTrue(np.allclose(zero(3), Y.lever))
self.assertTrue(np.allclose(zero([3,3]), Y.inertia))
def createHand(self, root_id=0, prefix='', joint_placement=None):
def trans(x, y, z):
return pin.SE3(eye(3), np.matrix([x, y, z]).T)
def inertia(m, c):
return pin.Inertia(m, np.matrix(c, np.double).T, eye(3) * m ** 2)
def joint_name(body):
return prefix + body + '_joint'
def body_name(body):
return 'world/' + prefix + body
color = [red, green, blue, transparency] = [1, 1, 0.78, 1.0]
joint_id = root_id
cm = 1e-2
joint_placement = joint_placement if joint_placement is not None else pin.SE3.Identity()
joint_id = self.model.addJoint(joint_id, pin.JointModelRY(), joint_placement, joint_name('wrist'))
self.model.appendBodyToJoint(joint_id, inertia(3, [0, 0, 0]), pin.SE3.Identity())
L, W, H = 3 * cm, 5 * cm, 1 * cm
self.viewer.viewer.gui.addSphere(body_name('wrist'), .02, color)
self.viewer.viewer.gui.addBox(body_name('wpalm'), L / 2, W / 2, H, color)
self.visuals.append(Visual(body_name('wpalm'), joint_id, trans(L / 2, 0, 0)))
self.viewer.viewer.gui.addCapsule(body_name('wpalmb'), H, W, color)
self.visuals.append(Visual(body_name('wpalmb'), joint_id, pin.SE3(rotate('x', pi / 2), zero(3)), H, W))
self.viewer.viewer.gui.addCapsule(body_name('wpalmt'), H, W, color)
pos = pin.SE3(rotate('x', pi / 2), np.matrix([L, 0, 0]).T)
self.visuals.append(Visual(body_name('wpalmt'), joint_id, pos, H, W))
self.viewer.viewer.gui.addCapsule(body_name('wpalml'), H, L, color)
pos = pin.SE3(rotate('y', pi / 2), np.matrix([L / 2, -W / 2, 0]).T)
self.visuals.append(Visual(body_name('wpalml'), joint_id, pos, H, L))
self.viewer.viewer.gui.addCapsule(body_name('wpalmr'), H, L, color)
pos = pin.SE3(rotate('y', pi / 2), np.matrix([L / 2, +W / 2, 0]).T)
self.visuals.append(Visual(body_name('wpalmr'), joint_id, pos, H, L))
joint_placement = pin.SE3(eye(3), np.matrix([5 * cm, 0, 0]).T)
joint_id = self.model.addJoint(joint_id, pin.JointModelRY(), joint_placement, joint_name('palm'))
self.model.appendBodyToJoint(joint_id, inertia(2, [0, 0, 0]), pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('palm2'), 1 * cm, W, color)
self.visuals.append(Visual(body_name('palm2'), joint_id, pin.SE3(rotate('x', pi / 2), zero(3)), H, W))
FL = 4 * cm
palmIdx = joint_id
joint_placement = pin.SE3(eye(3), np.matrix([2 * cm, W / 2, 0]).T)
joint_id = self.model.addJoint(palmIdx, pin.JointModelRY(), joint_placement, joint_name('finger11'))
self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]), pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('finger11'), H, FL - 2 * H, color)
pos = pin.SE3(rotate('y', pi / 2), np.matrix([FL / 2 - H, 0, 0]).T)
self.visuals.append(Visual(body_name('finger11'), joint_id, pos, H, FL - 2 * H))
joint_placement = pin.SE3(eye(3), np.matrix([FL, 0, 0]).T)
joint_id = self.model.addJoint(joint_id, pin.JointModelRY(), joint_placement, joint_name('finger12'))
self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]), pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('finger12'), H, FL - 2 * H, color)
pos = pin.SE3(rotate('y', pi / 2), np.matrix([FL / 2 - H, 0, 0]).T)
self.visuals.append(Visual(body_name('finger12'), joint_id, pos, H, FL - 2 * H))
joint_placement = pin.SE3(eye(3), np.matrix([FL - 2 * H, 0, 0]).T)
joint_id = self.model.addJoint(joint_id, pin.JointModelRY(), joint_placement, joint_name('finger13'))
self.model.appendBodyToJoint(joint_id, inertia(.3, [0, 0, 0]), pin.SE3.Identity())
self.viewer.viewer.gui.addSphere(body_name('finger13'), H, color)
self.visuals.append(Visual(body_name('finger13'), joint_id, trans(2 * H, 0, 0), H, 0))
joint_placement = pin.SE3(eye(3), np.matrix([2 * cm, 0, 0]).T)
joint_id = self.model.addJoint(palmIdx, pin.JointModelRY(), joint_placement, joint_name('finger21'))
self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]), pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('finger21'), H, FL - 2 * H, color)
pos = pin.SE3(rotate('y', pi / 2), np.matrix([FL / 2 - H, 0, 0]).T)
self.visuals.append(Visual(body_name('finger21'), joint_id, pos, H, FL - 2 * H))
joint_placement = pin.SE3(eye(3), np.matrix([FL, 0, 0]).T)
joint_id = self.model.addJoint(joint_id, pin.JointModelRY(), joint_placement, joint_name('finger22'))
self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]), pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('finger22'), H, FL - 2 * H, color)
pos = pin.SE3(rotate('y', pi / 2), np.matrix([FL / 2 - H, 0, 0]).T)
self.visuals.append(Visual(body_name('finger22'), joint_id, pos, H, FL - 2 * H))
joint_placement = pin.SE3(eye(3), np.matrix([FL - H, 0, 0]).T)
joint_id = self.model.addJoint(joint_id, pin.JointModelRY(), joint_placement, joint_name('finger23'))
self.model.appendBodyToJoint(joint_id, inertia(.3, [0, 0, 0]), pin.SE3.Identity())
self.viewer.viewer.gui.addSphere(body_name('finger23'), H, color)
self.visuals.append(Visual(body_name('finger23'), joint_id, trans(H, 0, 0), H, 0))
joint_placement = pin.SE3(eye(3), np.matrix([2 * cm, -W / 2, 0]).T)
joint_id = self.model.addJoint(palmIdx, pin.JointModelRY(), joint_placement, joint_name('finger31'))
self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]), pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('finger31'), H, FL - 2 * H, color)
pos = pin.SE3(rotate('y', pi / 2), np.matrix([FL / 2 - H, 0, 0]).T)
self.visuals.append(Visual(body_name('finger31'), joint_id, pos, H, FL - 2 * H))
joint_placement = pin.SE3(eye(3), np.matrix([FL, 0, 0]).T)
joint_id = self.model.addJoint(joint_id, pin.JointModelRY(), joint_placement, joint_name('finger32'))
self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]), pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('finger32'), H, FL - 2 * H, color)
pos = pin.SE3(rotate('y', pi / 2), np.matrix([FL / 2 - H, 0, 0]).T)
self.visuals.append(Visual(body_name('finger32'), joint_id, pos, H, FL - 2 * H))
joint_placement = pin.SE3(eye(3), np.matrix([FL - 2 * H, 0, 0]).T)
joint_id = self.model.addJoint(joint_id, pin.JointModelRY(), joint_placement, joint_name('finger33'))
self.model.appendBodyToJoint(joint_id, inertia(.3, [0, 0, 0]), pin.SE3.Identity())
self.viewer.viewer.gui.addSphere(body_name('finger33'), H, color)
self.visuals.append(Visual(body_name('finger33'), joint_id, trans(2 * H, 0, 0), H, 0))
joint_placement = pin.SE3(eye(3), np.matrix([1 * cm, -W / 2 - H * 1.5, 0]).T)
joint_id = self.model.addJoint(1, pin.JointModelRY(), joint_placement, joint_name('thumb1'))
self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]), pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('thumb1'), H, 2 * cm, color)
pos = pin.SE3(rotate('z', pi / 3) * rotate('x', pi / 2), np.matrix([1 * cm, -1 * cm, 0]).T)
self.visuals.append(Visual(body_name('thumb1'), joint_id, pos, 2 * cm))
joint_placement = pin.SE3(rotate('z', pi / 3) * rotate('x', pi), np.matrix([3 * cm, -1.8 * cm, 0]).T)
joint_id = self.model.addJoint(joint_id, pin.JointModelRZ(), joint_placement, joint_name('thumb2'))
self.model.appendBodyToJoint(joint_id, inertia(.4, [0, 0, 0]), pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('thumb2'), H, FL - 2 * H, color)
pos = pin.SE3(rotate('x', pi / 3), np.matrix([-0.7 * cm, .8 * cm, -0.5 * cm]).T)
self.visuals.append(Visual(body_name('thumb2'), joint_id, pos, H, FL - 2 * H))
# Prepare some patches to represent collision points. Yet unvisible.
for i in range(10):
self.viewer.viewer.gui.addCylinder('world/wa%i' % i, .01, .003, [1.0, 0, 0, 1])
self.viewer.viewer.gui.addCylinder('world/wb%i' % i, .01, .003, [1.0, 0, 0, 1])
self.viewer.viewer.gui.setVisibility('world/wa%i' % i, 'OFF')
self.viewer.viewer.gui.setVisibility('world/wb%i' % i, 'OFF')
def test_log6(self):
m = pin.SE3.Identity()
v = pin.log6(m)
self.assertApprox(v.vector, zero(6))
def test_set_rotation(self):
transform = pin.SE3.Identity()
transform.rotation = zero([3,3])
self.assertFalse(np.allclose(transform.rotation, eye(3)))
self.assertTrue(np.allclose(transform.rotation, zero([3,3])))
def test_set_translation(self):
transform = pin.SE3.Identity()
transform.translation = ones(3)
self.assertFalse(np.allclose(transform.translation, zero(3)))
self.assertTrue(np.allclose(transform.translation, ones(3)))
def test_get_translation(self):
transform = pin.SE3.Identity()
self.assertTrue(np.allclose(transform.translation, zero(3)))
def test_setRandom(self):
Y = pin.Inertia.Identity()
Y.setRandom()
self.assertFalse(Y.mass == 1)
self.assertFalse(np.allclose(zero(3), Y.lever))
self.assertFalse(np.allclose(eye(3), Y.inertia))
def test_identity_getters(self):
Y = pin.Inertia.Identity()
self.assertTrue(Y.mass == 1)
self.assertTrue(np.allclose(zero(3), Y.lever))
self.assertTrue(np.allclose(eye(3), Y.inertia))
