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 CoMPositionCostTest(CostModelAbstractTestCase): ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom() ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) cref = pinocchio.utils.rand(3) COST = crocoddyl.CostModelCoMPosition(ROBOT_STATE, cref) COST_DER = CoMPositionCostDerived(ROBOT_STATE, cref=cref)
class Contact3DMultipleTest(ContactModelMultipleAbstractTestCase): ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom() ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) gains = pinocchio.utils.rand(2) xref = crocoddyl.FrameTranslation(ROBOT_MODEL.getFrameId('rleg5_joint'), pinocchio.SE3.Random().translation) CONTACT = crocoddyl.ContactModel3D(ROBOT_STATE, xref, gains)
class Contact6DMultipleTest(ContactModelMultipleAbstractTestCase): ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom() ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) gains = pinocchio.utils.rand(2) Mref = crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('rleg5_joint'), pinocchio.SE3.Random()) CONTACT = crocoddyl.ContactModel6D(ROBOT_STATE, Mref, gains)
class FrameTranslationCostSumTest(CostModelSumTestCase): ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom() ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) xref = crocoddyl.FrameTranslation(ROBOT_MODEL.getFrameId('rleg5_joint'), pinocchio.utils.rand(3)) COST = crocoddyl.CostModelFrameTranslation(ROBOT_STATE, xref)
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)
class FrameVelocityCostSumTest(CostModelSumTestCase): ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom() ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) vref = crocoddyl.FrameMotion(ROBOT_MODEL.getFrameId('rleg5_joint'), pinocchio.Motion.Random()) COST = crocoddyl.CostModelFrameVelocity(ROBOT_STATE, vref)
class FramePlacementCostSumTest(CostModelSumTestCase): ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom() ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) Mref = crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('rleg5_joint'), pinocchio.SE3.Random()) COST = crocoddyl.CostModelFramePlacement(ROBOT_STATE, Mref)
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)
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 testBIN(self): model = pin.buildSampleModelHumanoidRandom() filename = main_path + "/model.bin" model.saveToBinary(filename) model2 = pin.Model() model2.loadFromBinary(filename) self.assertTrue(model == model2)
def testTXT(self): model = pin.buildSampleModelHumanoidRandom() filename = main_path + "/model.txt" model.saveToText(filename) model2 = pin.Model() model2.loadFromText(filename) self.assertTrue(model == model2)
def testXML(self): model = pin.buildSampleModelHumanoidRandom() filename = main_path + "/model.xml" tag_name = "Model" model.saveToXML(filename, tag_name) model2 = pin.Model() model2.loadFromXML(filename, tag_name) self.assertTrue(model == model2)
def testXML(self): model = pin.buildSampleModelHumanoidRandom() filename = main_path + "/model.xml" tag_name = "Model" model.saveToXML(filename,tag_name) model2 = pin.Model() model2.loadFromXML(filename,tag_name) self.assertTrue(model == model2)
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 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)
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.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_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)
def setUp(self): self.model = pin.buildSampleModelHumanoidRandom() self.data = self.model.createData()
def setUp(self): self.model = pin.buildSampleModelHumanoidRandom()
class ControlCostTest(CostModelAbstractTestCase): ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom() ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) COST = crocoddyl.CostModelControl(ROBOT_STATE) COST_DER = ControlCostDerived(ROBOT_STATE)
class ControlCostSumTest(CostModelSumTestCase): ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom() ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) COST = crocoddyl.CostModelControl(ROBOT_STATE)
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)
## <http:##www.gnu.org/licenses/>. ## import pinocchio as pin import numpy as np ## ## 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)
class FloatingBaseActuationTest(ActuationModelAbstractTestCase): STATE = crocoddyl.StateMultibody(pinocchio.buildSampleModelHumanoidRandom()) ACTUATION = crocoddyl.ActuationModelFloatingBase(STATE) ACTUATION_DER = FreeFloatingActuationDerived(STATE)
## <http:##www.gnu.org/licenses/>. ## import pinocchio as pin import numpy as np ## ## 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)
class StateMultibodyHumanoidTest(StateAbstractTestCase): MODEL = pinocchio.buildSampleModelHumanoidRandom() NX = MODEL.nq + MODEL.nv NDX = 2 * MODEL.nv STATE = crocoddyl.StateMultibody(MODEL) STATE_DER = StateMultibodyDerived(MODEL)
def setUp(self): self.model = pin.buildSampleModelHumanoidRandom() self.joint_idx = self.model.getJointId( "rarm2_joint") if self.model.existJointName("rarm2_joint") else ( self.model.njoints - 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)