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 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 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 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 = se3.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 = np.eye(4)
self.assertApprox(exp(log(m)).homogeneous, m)
with self.assertRaises(ValueError):
exp(np.eye(4))
with self.assertRaises(ValueError):
exp(list(range(3)))
with self.assertRaises(ValueError):
log(list(range(3)))
with self.assertRaises(ValueError):
log(np.zeros(5))
with self.assertRaises(ValueError):
log(np.zeros((3, 1)))
def test_placement_get_set(self):
m = se3.SE3(self.m3ones, self.v3zero)
new_m = se3.SE3(rand([3,3]), rand(3))
col = self.robot.collision_model.geometryObjects[1]
self.assertTrue(np.allclose(col.placement.homogeneous,m.homogeneous))
col.placement = new_m
self.assertTrue(np.allclose(col.placement.homogeneous , new_m.homogeneous))
def test_placement_get_set(self):
m = se3.SE3(self.m3ones, np.array([0,0,0],np.double))
new_m = se3.SE3(rand([3,3]), rand(3))
f = self.robot.model.frames[2]
self.assertTrue(np.allclose(f.placement.homogeneous, m.homogeneous))
f.placement = new_m
self.assertTrue(np.allclose(f.placement.homogeneous, new_m.homogeneous))
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 test_placement_get_set(self):
m = se3.SE3(self.m3ones, self.v3zero)
new_m = se3.SE3(rand([3,3]), rand(3))
col = self.robot.geometry_model.collision_objects[1]
self.assertTrue(np.allclose(col.placement.homogeneous,m.homogeneous))
col.placement = new_m
self.assertTrue(np.allclose(col.placement.homogeneous , new_m.homogeneous))
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 test_placement_get_set(self):
m = se3.SE3(self.m3ones, np.array([0,0,-0.0684],np.double))
new_m = se3.SE3(rand([3,3]), rand(3))
f = self.robot.model.operational_frames[1]
self.assertTrue(np.allclose(f.placement.homogeneous,m.homogeneous))
f.placement = new_m
self.assertTrue(np.allclose(f.placement.homogeneous , new_m.homogeneous))
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_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_set_inertia(self):
Y = se3.Inertia.Zero()
iner = rand([3,3])
iner = (iner + iner.T) / 2. # Symmetrize the matrix
vec6_iner = np.matrix([iner[0,0],iner[0,1], iner[1,1], iner[0,2], iner[1,2], iner[2,2]], np.double)
Y.inertia = vec6_iner
self.assertTrue(np.allclose(Y.inertia, iner))
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 test_set_linear(self):
v = se3.Motion.Zero()
lin = rand(
3
) # TODO np.matrix([1,2,3],np.double) OR np.matrix( np.array([1,2,3], np.double), np.double)
v.linear = lin
self.assertTrue(np.allclose(v.linear, lin))
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_set_linear(self):
f = se3.Force.Zero()
lin = rand(
3
) # TODO np.matrix([1,2,3],np.double) OR np.matrix( np.array([1,2,3], np.double), np.double)
f.linear = lin
self.assertTrue(np.allclose(f.linear, lin))
def test_set_linear(self):
v = pin.Motion.Zero()
lin = rand(3)
v.linear = lin
self.assertTrue(np.allclose(v.linear, lin))
v.linear[1] = 1.
self.assertTrue(v.linear[1] == 1.)
def test_set_angular(self):
f = pin.Force.Zero()
ang = rand(3)
f.angular = ang
self.assertTrue(np.allclose(f.angular, ang))
f.angular[1] = 1.
self.assertTrue(f.angular[1] == 1.)
def test_set_linear(self):
f = pin.Force.Zero()
lin = rand(3)
f.linear = lin
self.assertTrue(np.allclose(f.linear, lin))
f.linear[1] = 1.
self.assertTrue(f.linear[1] == 1.)
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_set_angular(self):
v = pin.Motion.Zero()
ang = rand(3)
v.angular = ang
self.assertTrue(np.allclose(v.angular, ang))
v.angular[1] = 1.
self.assertTrue(v.angular[1] == 1.)
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)
## 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 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])
def test_point_action(self):
amb = pin.SE3.Random()
aMb = amb.homogeneous
p_homogeneous = rand(4)
p_homogeneous[3] = 1
p = p_homogeneous[0:3].copy()
# act
self.assertTrue(np.allclose(amb.act(p),(aMb*p_homogeneous)[0:3]))
# actinv
bMa = np.linalg.inv(aMb)
bma = amb.inverse()
self.assertTrue(np.allclose(bma.act(p), (bMa * p_homogeneous)[0:3]))
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])
def test_point_action(self):
amb = se3.SE3.Random()
aMb = amb.homogeneous
p_homogeneous = rand(4)
p_homogeneous[3] = 1
p = p_homogeneous[0:3].copy()
# act
self.assertTrue(np.allclose(amb.act_point(p),(aMb*p_homogeneous)[0:3]))
# actinv
bMa = np.linalg.inv(aMb)
bma = amb.inverse()
self.assertTrue(np.allclose(bma.act_point(p), (bMa * p_homogeneous)[0:3]))
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_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])
for i in range(nv):
Md[i, i] = (GEAR_RATIO**2) * MOTOR_INERTIA
# acceleration upper bounds as a function of the joint position
ddq_ub_of_q = np.zeros((nq, N_SAMPLING, 2))
for i in range(N_SAMPLING):
q = se3.randomConfiguration(robot.model, Q_MIN, Q_MAX)
while (CHECK_COLLISIONS and robot.isInCollision(q)):
print("* Robot is in collision at sample %d" % i)
q = se3.randomConfiguration(robot.model, Q_MIN, Q_MAX)
if (DISPLAY_ROBOT_CONFIGURATIONS):
robot.display(q)
v = np.multiply(DQ_MAX, 2.0 * rand(nv) - 1.0)
M = robot.mass(q) + Md
h = robot.biais(q, v)
# dM = dcrba(q); # d/dq M(q) so that d/dqi M = Mp[:,:,i] (symmetric), then dtau = tensordot(Mp,dq,[2,0])
dh_dv = coriolis(q, v)
# d/dvq RNEA(q,vq) = C(q,vq)
dh_dq = drnea(q, v, dv)
# d/dq RNEA(q,vq,aq)
ddqUb = np.divide(TAU_MAX - h, mat_diag(M))
ddqLb = np.divide(-TAU_MAX - h, mat_diag(M))
for j in range(nq):
ddq_ub_of_q[j, i, 0] = q[j]
ddq_ub_of_q[j, i, 1] = TAU_MAX[j] - h[j]
for k in range(nv):
if (k != j):
ddq_ub_of_q[j, i, 1] -= abs(M[j, k] * DDQ_MAX[k])
def absmin(A):
return np.min(abs(A))
# if np.any(np.isinf(model.upperPositionLimit)):
# qmin = rmodel.lowerPositionLimit
# qmin[:7] = -1
# rmodel.lowerPositionLimit = qmin
# qmax = rmodel.upperPositionLimit
# qmax[:7] = 1
# rmodel.upperPositionLimit = qmax
q = pinocchio.randomConfiguration(model)
v = rand(model.nv) * 2 - 1
a = rand(model.nv) * 2 - 1
tau = pinocchio.rnea(model, data, q, v, a)
# Basic check of calling the derivatives.
pinocchio.computeABADerivatives(model, data, q, v, tau)
pinocchio.computeRNEADerivatives(model, data, q, v, a)
'''
a = M^-1 ( tau - b)
a' = -M^-1 M' M^-1 (tau-b) - M^-1 b'
= -M^-1 M' a - M^-1 b'
= -M^-1 (M'a + b')
= -M^-1 tau'
'''
# Check that a = J aq + Jdot vq
def test_set_linear(self):
v = se3.Motion.Zero()
lin = rand(3) # TODO np.matrix([1,2,3],np.double) OR np.matrix( np.array([1,2,3], np.double), np.double)
v.linear = lin
self.assertTrue(np.allclose(v.linear, lin))
def test_set_lever(self):
Y = pin.Inertia.Zero()
lev = rand(3)
Y.lever = lev
self.assertTrue(np.allclose(Y.lever, lev))
import numpy as np import pinocchio as pin from pinocchio.utils import rand from example_robot_data import loadANYmal robot = loadANYmal() rm = robot.model rd = robot.data q = robot.q0 qv = rand(robot.nv) C = pin.computeCoriolisMatrix(rm, rd, q, qv) dM_dt = pin.dccrba(rm, rd, q, qv) # verify dM/dt = C + C.T C_sum = C + C.T # Why is dM_dt only 6 x nqv and not nqv x nqv? print(dM_dt.shape) # not zero print(np.linalg.norm(C_sum[:6, :] - dM_dt))
def test_set_inertia(self):
Y = pin.Inertia.Zero()
iner = rand([3,3])
iner = (iner + iner.T) / 2. # Symmetrize the matrix
Y.inertia = iner
self.assertTrue(np.allclose(Y.inertia, iner))
def test_set_vector(self):
f = se3.Force.Zero()
vec = rand(6)
f.vector = vec
self.assertTrue(np.allclose(f.vector, vec))
def setUp(self):
self.R = rand([3, 3])
self.R, _, _ = npl.svd(self.R)
self.p = rand(3)
self.m = se3.SE3(self.R, self.p)
def test_set_linear(self):
f = se3.Force.Zero()
lin = rand(3) # TODO np.matrix([1,2,3],np.double) OR np.matrix( np.array([1,2,3], np.double), np.double)
f.linear = lin
self.assertTrue(np.allclose(f.linear, lin))
def test_set_vector(self):
f = pin.Force.Zero()
vec = rand(6)
f.vector = vec
self.assertTrue(np.allclose(f.vector, vec))
def test_set_angular(self):
f = se3.Force.Zero()
ang = rand(3)
f.angular = ang
self.assertTrue(np.allclose(f.angular, ang))
def test_set_vector(self):
v = se3.Motion.Zero()
vec = rand(6)
v.vector = vec
self.assertTrue(np.allclose(v.vector, vec))
def test_set_vector(self):
v = se3.Motion.Zero()
vec = rand(6)
v.vector = vec
self.assertTrue(np.allclose(v.vector, vec))
def setUp(self):
self.R = rand([3, 3])
self.R, _, _ = npl.svd(self.R)
self.p = rand(3)
self.m = se3.SE3(self.R, self.p)
def test_set_linear(self):
v = se3.Motion.Zero()
lin = rand(3)
v.linear = lin
self.assertTrue(np.allclose(v.linear, lin))
def test_set_angular(self):
v = se3.Motion.Zero()
ang = rand(3)
v.angular = ang
self.assertTrue(np.allclose(v.angular, ang))
def test_set_inertia(self):
Y = pin.Inertia.Zero()
iner = rand([3, 3])
iner = (iner + iner.T) / 2. # Symmetrize the matrix
Y.inertia = iner
self.assertTrue(np.allclose(Y.inertia, iner))
import time
import numpy as np
from pinocchio.utils import rand
p0 = rand(3)
p1 = rand(3)
for t in np.arange(0, 1, .01):
p = p0 * (1 - t) + p1 * t
gv.applyConfiguration('world/box', p.T.tolist()[0] + quat.coeffs().T.tolist()[0]) # noqa
gv.refresh() # noqa
time.sleep(.01)
# 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)
contactModel.addContact("LF_FOOT_contact", contactModel6)
def test_set_linear(self):
f = pin.Force.Zero()
lin = rand(3)
f.linear = lin
self.assertTrue(np.allclose(f.linear, lin))
