def test_all_sampled_models(self):
huamnoid_1 = pin.buildSampleModelHumanoidRandom()
huamnoid_2 = pin.buildSampleModelHumanoidRandom(True)
huamnoid_3 = pin.buildSampleModelHumanoidRandom(False)
self.assertTrue(huamnoid_1 != huamnoid_2)
self.assertTrue(huamnoid_1 != huamnoid_3)
manipulator_1 = pin.buildSampleModelManipulator()
if pin.WITH_HPP_FCL:
geometry_manipulator_1 = pin.buildSampleGeometryModelManipulator(
manipulator_1)
humanoid_4 = pin.buildSampleModelHumanoid()
humanoid_5 = pin.buildSampleModelHumanoid(True)
humanoid_6 = pin.buildSampleModelHumanoid(False)
self.assertTrue(humanoid_4 == humanoid_5)
self.assertTrue(humanoid_4 != humanoid_6)
if pin.WITH_HPP_FCL:
geometry_humanoid_2 = pin.buildSampleGeometryModelHumanoid(
humanoid_4)
class FreeFwdDynamicsTest(ActionModelAbstractTestCase):
ROBOT_MODEL = pinocchio.buildSampleModelManipulator()
STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
COST_SUM = crocoddyl.CostModelSum(STATE, ROBOT_MODEL.nv)
COST_SUM.addCost('xReg', crocoddyl.CostModelState(STATE), 1.)
COST_SUM.addCost(
'frTrack',
crocoddyl.CostModelFramePlacement(
STATE,
crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId("effector_body"),
pinocchio.SE3.Random())), 1.)
MODEL = crocoddyl.DifferentialActionModelFreeFwdDynamics(STATE, COST_SUM)
MODEL_DER = DifferentialFreeFwdDynamicsDerived(STATE, COST_SUM)
class ManipulatorDDPTest(SolverAbstractTestCase):
ROBOT_MODEL = pinocchio.buildSampleModelManipulator()
STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
COST_SUM = crocoddyl.CostModelSum(STATE, ROBOT_MODEL.nv)
COST_SUM.addCost('xReg', crocoddyl.CostModelState(STATE), 1e-7)
COST_SUM.addCost('uReg', crocoddyl.CostModelControl(STATE), 1e-7)
COST_SUM.addCost(
'frTrack',
crocoddyl.CostModelFramePlacement(
STATE, crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId("effector_body"), pinocchio.SE3.Random())), 1.)
DIFF_MODEL = crocoddyl.DifferentialActionModelFreeFwdDynamics(STATE, COST_SUM)
MODEL = crocoddyl.IntegratedActionModelEuler(crocoddyl.DifferentialActionModelFreeFwdDynamics(STATE, COST_SUM),
1e-3)
SOLVER = crocoddyl.SolverDDP
SOLVER_DER = DDPDerived
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 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)
import pinocchio model = pinocchio.buildSampleModelManipulator() data = model.createData() q = model.neutralConfiguration v = pinocchio.utils.zero(model.nv) a = pinocchio.utils.zero(model.nv) tau = pinocchio.rnea(model, data, q, v, a) print 'tau = ', tau.T
class FullActuationTest(ActuationModelAbstractTestCase):
STATE = crocoddyl.StateMultibody(pinocchio.buildSampleModelManipulator())
ACTUATION = crocoddyl.ActuationModelFull(STATE)
ACTUATION_DER = FullActuationDerived(STATE)
class StateMultibodyManipulatorTest(StateAbstractTestCase):
MODEL = pinocchio.buildSampleModelManipulator()
NX = MODEL.nq + MODEL.nv
NDX = 2 * MODEL.nv
STATE = crocoddyl.StateMultibody(MODEL)
STATE_DER = StateMultibodyDerived(MODEL)
from __future__ import print_function
import numpy as np
import pinocchio
model = pinocchio.buildSampleModelManipulator()
data = model.createData()
JOINT_ID = 6
xdes = np.matrix([ 0.5,-0.5,0.5]).T
q = model.neutralConfiguration
eps = 1e-4
IT_MAX = 1000
DT = 1e-1
for i in range(IT_MAX):
pinocchio.forwardKinematics(model,data,q)
x = data.oMi[JOINT_ID].translation
R = data.oMi[JOINT_ID].rotation
err = R.T*(x-xdes)
if np.linalg.norm(err) < eps:
print("Convergence achieved!")
break
J = pinocchio.jointJacobian(model,data,q,JOINT_ID,pinocchio.ReferenceFrame.LOCAL,True)[:3,:]
v = - np.linalg.pinv(J)*err
q = pinocchio.integrate(model,q,v*DT)
if not i % 10: print('error = %s' % (x-xdes).T)
else:
print("\nWarning: the iterative algorithm has not reached convergence to the desired precision")
