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")