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