Unverified Commit 0277fa4f authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #1442 from jcarpent/topic/reduced-model

Fix bug in buildReducedModel + enhance Python bindings
parents 44c631c0 b064cc01
Pipeline #14225 passed with stage
in 151 minutes and 19 seconds
//
// Copyright (c) 2015-2020 CNRS INRIA
// Copyright (c) 2015-2021 CNRS INRIA
//
#ifndef __pinocchio_python_joint_variants_hpp__
......@@ -34,6 +34,9 @@ namespace pinocchio
.add_property("nv",&get_nv)
.def("setIndexes",&JointModelDerived::setIndexes)
.def("shortname",&JointModelDerived::shortname)
.def(bp::self == bp::self)
.def(bp::self != bp::self)
;
}
......@@ -72,6 +75,9 @@ namespace pinocchio
.add_property("Dinv",&get_Dinv)
.add_property("UDinv",&get_UDinv)
.def("shortname",&JointDataDerived::shortname)
.def(bp::self == bp::self)
.def(bp::self != bp::self)
;
}
......
......@@ -33,6 +33,9 @@ namespace pinocchio
.add_property("nv",&getNv)
.def("setIndexes",&JointModel::setIndexes)
.def("shortname",&JointModel::shortname)
.def(bp::self == bp::self)
.def(bp::self != bp::self)
;
}
......
......@@ -114,35 +114,39 @@ namespace pinocchio
}
template<>
inline bp::class_<JointModelComposite>& expose_joint_model<JointModelComposite> (bp::class_<JointModelComposite> & cl)
bp::class_<JointModelComposite> & expose_joint_model<JointModelComposite>(bp::class_<JointModelComposite> & cl)
{
return cl
.def(bp::init<const size_t> (bp::args("self","size"),
"Init JointModelComposite with a defined size"))
.def("__init__",
bp::make_constructor(init_proxy1,
bp::default_call_policies(),
bp::args("joint_model")
),
"Init JointModelComposite from a joint"
)
.def("__init__",
bp::make_constructor(init_proxy2,
bp::default_call_policies(),
bp::args("joint_model","joint_placement")
),
"Init JointModelComposite from a joint and a placement"
)
.add_property("joints",&JointModelComposite::joints)
.add_property("jointPlacements",&JointModelComposite::jointPlacements)
.add_property("njoints",&JointModelComposite::njoints)
.def("addJoint",
&addJoint_proxy,
addJoint_proxy_overloads(bp::args("self","joint_model","joint_placement"),
"Add a joint to the vector of joints."
)[bp::return_internal_reference<>()]
)
;
.def(bp::init<const size_t> (bp::args("self","size"),
"Init JointModelComposite with a defined size"))
.def("__init__",
bp::make_constructor(init_proxy1,
bp::default_call_policies(),
bp::args("joint_model")
),
"Init JointModelComposite from a joint"
)
.def("__init__",
bp::make_constructor(init_proxy2,
bp::default_call_policies(),
bp::args("joint_model","joint_placement")
),
"Init JointModelComposite from a joint and a placement"
)
.add_property("joints",&JointModelComposite::joints)
.add_property("jointPlacements",&JointModelComposite::jointPlacements)
.add_property("njoints",&JointModelComposite::njoints)
.def("addJoint",
&addJoint_proxy,
addJoint_proxy_overloads(bp::args("self","joint_model","joint_placement"),
"Add a joint to the vector of joints."
)[bp::return_internal_reference<>()]
)
.def(bp::self == bp::self)
.def(bp::self != bp::self)
;
}
} // namespace python
} // namespace pinocchio
......
......@@ -95,6 +95,8 @@ namespace pinocchio
typedef typename Model::VectorXs VectorXs;
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(addFrame_overload,Model::addFrame,1,2)
public:
/* --- Exposing C++ API to python through the handler ----------------- */
......@@ -189,7 +191,10 @@ namespace pinocchio
.def("existFrame",&Model::existFrame,existFrame_overload(bp::args("self","name","type"),"Returns true if the frame given by its name exists inside the Model with the given type."))
.def("addFrame",(std::size_t (Model::*)(const Frame &)) &Model::addFrame,bp::args("self","frame"),"Add a frame to the vector of frames.")
.def("addFrame",&Model::addFrame,
addFrame_overload((bp::arg("self"), bp::arg("frame"), bp::arg("append_inertia") = true),
"Add a frame to the vector of frames. If append_inertia set to True, "
"the inertia value contained in frame will be added to the inertia supported by the parent joint."))
.def("createData",
&ModelPythonVisitor::createData,bp::arg("self"),
......
......@@ -300,7 +300,7 @@ namespace pinocchio
typedef typename Model::JointData JointData;
typedef typename Model::Frame Frame;
typedef typename Model::SE3 SE3;
// Sort indexes
std::sort(list_of_joints_to_lock.begin(),list_of_joints_to_lock.end());
......@@ -317,7 +317,6 @@ namespace pinocchio
reduced_model.joints .reserve((size_t)njoints);
reduced_model.jointPlacements .reserve((size_t)njoints);
reduced_model.parents .reserve((size_t)njoints);
reduced_model.inertias .reserve((size_t)njoints);
reduced_model.names[0] = input_model.names[0];
reduced_model.joints[0] = input_model.joints[0];
......@@ -332,7 +331,7 @@ namespace pinocchio
for(JointIndex joint_id = 1; joint_id < (JointIndex)input_model.njoints; ++joint_id)
{
const JointIndex joint_id_to_lock = (current_index_to_lock < list_of_joints_to_lock.size()) ? list_of_joints_to_lock[current_index_to_lock] : input_model.joints.size();
const JointIndex joint_id_to_lock = (current_index_to_lock < list_of_joints_to_lock.size()) ? list_of_joints_to_lock[current_index_to_lock] : 0;
const JointIndex input_parent_joint_index = input_model.parents[joint_id];
const std::string & joint_name = input_model.names[joint_id];
......@@ -369,16 +368,12 @@ namespace pinocchio
Frame frame = Frame(joint_name,
reduced_parent_joint_index, reduced_previous_frame_index,
liMi,
FIXED_JOINT);
FIXED_JOINT,
input_model.inertias[joint_id]);
FrameIndex frame_id = reduced_model.addFrame(frame);
reduced_model.frames[frame_id].previousFrame = frame_id; // a bit weird, but this is a solution for missing parent frame
// Add the Inertia of the link supported by joint_id
reduced_model.appendBodyToJoint(reduced_parent_joint_index,
input_model.inertias[joint_id],
frame.placement);
current_index_to_lock++;
}
else
......@@ -436,34 +431,34 @@ namespace pinocchio
for(++frame_it;frame_it != input_model.frames.end(); ++frame_it)
{
const Frame & input_frame = *frame_it;
const std::string & input_joint_name = input_model.names[input_frame.parent];
const std::string & support_joint_name = input_model.names[input_frame.parent];
std::vector<JointIndex>::const_iterator joint_id_it = std::find(list_of_joints_to_lock.begin(),
list_of_joints_to_lock.end(),
input_frame.parent);
std::vector<JointIndex>::const_iterator support_joint_it = std::find(list_of_joints_to_lock.begin(),
list_of_joints_to_lock.end(),
input_frame.parent);
if(joint_id_it != list_of_joints_to_lock.end())
if(support_joint_it != list_of_joints_to_lock.end())
{
if( input_frame.type == JOINT
&& reduced_model.existFrame(input_frame.name)
&& input_joint_name == input_frame.name)
&& support_joint_name == input_frame.name)
continue; // this means that the Joint is now fixed and has been replaced by a Frame. No need to add a new one.
// The joint has been removed and replaced by a Frame
const FrameIndex joint_frame_id = reduced_model.getFrameId(input_joint_name);
const FrameIndex joint_frame_id = reduced_model.getFrameId(support_joint_name);
const Frame & joint_frame = reduced_model.frames[joint_frame_id];
Frame reduced_frame = input_frame;
reduced_frame.placement = joint_frame.placement * input_frame.placement;
reduced_frame.parent = joint_frame.parent;
reduced_frame.previousFrame = reduced_model.getFrameId(input_model.frames[input_frame.previousFrame].name);
reduced_model.addFrame(reduced_frame);
reduced_model.addFrame(reduced_frame, false);
}
else
{
Frame reduced_frame = input_frame;
reduced_frame.parent = reduced_model.getJointId(input_model.names[input_frame.parent]);
reduced_frame.previousFrame = reduced_model.getFrameId(input_model.frames[input_frame.previousFrame].name);
reduced_model.addFrame(reduced_frame);
reduced_model.addFrame(reduced_frame, false);
}
}
}
......
......@@ -528,10 +528,12 @@ namespace pinocchio
/// The inertia stored within the frame will be happened to the inertia supported by the joint (frame.parent).
///
/// \param[in] frame The frame to add to the kinematic tree.
/// \param[in] append_inertia Append the inertia contained in the Frame to the inertia supported by the joint.
///
/// \return Returns the index of the frame if it has been successfully added or if it already exists in the kinematic tree.
///
FrameIndex addFrame(const Frame & frame);
FrameIndex addFrame(const Frame & frame,
const bool append_inertia = true);
///
/// \brief Check the validity of the attributes of Model with respect to the specification of some
......
......@@ -276,7 +276,7 @@ namespace pinocchio
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
inline typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex
ModelTpl<Scalar,Options,JointCollectionTpl>::
addFrame(const Frame & frame)
addFrame(const Frame & frame, const bool append_inertia)
{
PINOCCHIO_CHECK_INPUT_ARGUMENT(frame.parent < (JointIndex)njoints,
"The index of the parent frame is not valid.");
......@@ -290,7 +290,7 @@ namespace pinocchio
return getFrameId(frame.name,frame.type);
frames.push_back(frame);
// if(frame.inertia.mass() > Scalar(0))
if(append_inertia)
inertias[frame.parent] += frame.placement.act(frame.inertia);
nframes++;
return FrameIndex(nframes - 1);
......
......@@ -281,6 +281,39 @@ BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
}
#endif
BOOST_AUTO_TEST_CASE(test_buildReducedModel_empty)
{
Model humanoid_model;
buildModels::humanoid(humanoid_model);
static const std::vector<JointIndex> empty_index_vector;
humanoid_model.lowerPositionLimit.head<3>().fill(-1.);
humanoid_model.upperPositionLimit.head<3>().fill( 1.);
humanoid_model.referenceConfigurations.insert(std::pair<std::string,Eigen::VectorXd>("neutral",neutral(humanoid_model)));
Eigen::VectorXd reference_config_humanoid = randomConfiguration(humanoid_model);
Model humanoid_copy_model = buildReducedModel(humanoid_model,empty_index_vector,reference_config_humanoid);
BOOST_CHECK(humanoid_copy_model.names == humanoid_model.names);
BOOST_CHECK(humanoid_copy_model.joints == humanoid_model.joints);
BOOST_CHECK(humanoid_copy_model == humanoid_model);
BOOST_CHECK(humanoid_copy_model.referenceConfigurations["neutral"].isApprox(neutral(humanoid_copy_model)));
const std::vector<JointIndex> empty_joints_to_lock;
const Model reduced_humanoid_model = buildReducedModel(humanoid_model,empty_joints_to_lock,reference_config_humanoid);
BOOST_CHECK(reduced_humanoid_model.njoints == humanoid_model.njoints);
BOOST_CHECK(reduced_humanoid_model.frames == humanoid_model.frames);
BOOST_CHECK(reduced_humanoid_model.jointPlacements == humanoid_model.jointPlacements);
BOOST_CHECK(reduced_humanoid_model.joints == humanoid_model.joints);
for(JointIndex joint_id = 1; joint_id < (JointIndex)reduced_humanoid_model.njoints; ++joint_id)
{
BOOST_CHECK(reduced_humanoid_model.inertias[joint_id].isApprox(humanoid_model.inertias[joint_id]));
}
}
BOOST_AUTO_TEST_CASE(test_buildReducedModel)
{
Model humanoid_model;
......
#
# Copyright (c) 2015-2020 CNRS INRIA
# Copyright (c) 2015-2021 CNRS INRIA
#
SET(${PROJECT_NAME}_PYTHON_TESTS
......@@ -7,6 +7,7 @@ SET(${PROJECT_NAME}_PYTHON_TESTS
# Multibody
bindings_joint_composite
bindings_joints
bindings_model
bindings_data
bindings_geometry_model
......@@ -30,7 +31,7 @@ SET(${PROJECT_NAME}_PYTHON_TESTS
bindings_kinematics
bindings_rnea
bindings_aba
bindings_joints
bindings_joint_algorithms
# Algo derivatives
bindings_kinematics_derivatives
......
import unittest
from test_case import PinocchioTestCase as TestCase
import pinocchio as pin
import numpy as np
class TestJointsAlgo(TestCase):
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)
def test_basic(self):
model = self.model
q_ones = np.ones((model.nq))
self.assertFalse(pin.isNormalized(model, q_ones))
self.assertFalse(pin.isNormalized(model, q_ones, 1e-8))
self.assertTrue(pin.isNormalized(model, q_ones, 1e2))
q_rand = np.random.rand((model.nq))
q_rand = pin.normalize(model,q_rand)
self.assertTrue(pin.isNormalized(model, q_rand))
self.assertTrue(pin.isNormalized(model, q_rand, 1e-8))
self.assertTrue(abs(np.linalg.norm(q_rand[3:7])-1.) <= 1e-8)
q_next = pin.integrate(model,self.q,np.zeros((model.nv)))
self.assertApprox(q_next,self.q)
v_diff = pin.difference(model,self.q,q_next)
self.assertApprox(v_diff,np.zeros((model.nv)))
q_next = pin.integrate(model,self.q,self.v)
q_int = pin.interpolate(model,self.q,q_next,0.5)
self.assertApprox(q_int,q_int)
value = pin.squaredDistance(model,self.q,self.q)
self.assertTrue((value <= 1e-8).all())
dist = pin.distance(model,self.q,self.q)
self.assertTrue(dist <= 1e-8)
q_neutral = pin.neutral(model)
self.assertApprox(q_neutral,q_neutral)
q_rand1 = pin.randomConfiguration(model)
q_rand2 = pin.randomConfiguration(model,-np.ones((model.nq)),np.ones((model.nq)))
self.assertTrue(pin.isSameConfiguration(model,self.q,self.q,1e-8))
self.assertFalse(pin.isSameConfiguration(model,q_rand1,q_rand2,1e-8))
def test_derivatives(self):
model = self.model
q = self.q
v = self.v
J0, J1 = pin.dIntegrate(model,q,v)
res_0 = pin.dIntegrate(model,q,v,pin.ARG0)
res_1 = pin.dIntegrate(model,q,v,pin.ARG1)
self.assertApprox(J0,res_0)
self.assertApprox(J1,res_1)
q_next = pin.integrate(model,q,v)
J0, J1 = pin.dDifference(model,q,q_next)
res_0 = pin.dDifference(model,q,q_next,pin.ARG0)
res_1 = pin.dDifference(model,q,q_next,pin.ARG1)
self.assertApprox(J0,res_0)
self.assertApprox(J1,res_1)
if __name__ == '__main__':
unittest.main()
import unittest
from test_case import PinocchioTestCase as TestCase
import pinocchio as pin
import numpy as np
class TestJointsAlgo(TestCase):
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)
def test_basic(self):
model = self.model
q_ones = np.ones((model.nq))
self.assertFalse(pin.isNormalized(model, q_ones))
self.assertFalse(pin.isNormalized(model, q_ones, 1e-8))
self.assertTrue(pin.isNormalized(model, q_ones, 1e2))
q_rand = np.random.rand((model.nq))
q_rand = pin.normalize(model,q_rand)
self.assertTrue(pin.isNormalized(model, q_rand))
self.assertTrue(pin.isNormalized(model, q_rand, 1e-8))
self.assertTrue(abs(np.linalg.norm(q_rand[3:7])-1.) <= 1e-8)
q_next = pin.integrate(model,self.q,np.zeros((model.nv)))
self.assertApprox(q_next,self.q)
v_diff = pin.difference(model,self.q,q_next)
self.assertApprox(v_diff,np.zeros((model.nv)))
q_next = pin.integrate(model,self.q,self.v)
q_int = pin.interpolate(model,self.q,q_next,0.5)
self.assertApprox(q_int,q_int)
value = pin.squaredDistance(model,self.q,self.q)
self.assertTrue((value <= 1e-8).all())
dist = pin.distance(model,self.q,self.q)
self.assertTrue(dist <= 1e-8)
q_neutral = pin.neutral(model)
self.assertApprox(q_neutral,q_neutral)
q_rand1 = pin.randomConfiguration(model)
q_rand2 = pin.randomConfiguration(model,-np.ones((model.nq)),np.ones((model.nq)))
self.assertTrue(pin.isSameConfiguration(model,self.q,self.q,1e-8))
self.assertFalse(pin.isSameConfiguration(model,q_rand1,q_rand2,1e-8))
def test_derivatives(self):
model = self.model
q = self.q
v = self.v
joint_types = [
pin.JointModelRX, pin.JointModelRY, pin.JointModelRZ,
pin.JointModelPX, pin.JointModelPY, pin.JointModelPZ,
pin.JointModelFreeFlyer, pin.JointModelSpherical, pin.JointModelSphericalZYX,
pin.JointModelPlanar, pin.JointModelTranslation
]
J0, J1 = pin.dIntegrate(model,q,v)
res_0 = pin.dIntegrate(model,q,v,pin.ARG0)
res_1 = pin.dIntegrate(model,q,v,pin.ARG1)
class TestJoints(unittest.TestCase):
self.assertApprox(J0,res_0)
self.assertApprox(J1,res_1)
def test_comparison_operators(self):
for joint_type in joint_types:
q_next = pin.integrate(model,q,v)
j = joint_type()
joint_model = pin.JointModel(j)
J0, J1 = pin.dDifference(model,q,q_next)
res_0 = pin.dDifference(model,q,q_next,pin.ARG0)
res_1 = pin.dDifference(model,q,q_next,pin.ARG1)
self.assertTrue(j == joint_type())
self.assertTrue(j == joint_model)
self.assertApprox(J0,res_0)
self.assertApprox(J1,res_1)
j.setIndexes(0,0,0)
self.assertFalse(j == joint_model)
self.assertTrue(j != joint_model)
if __name__ == '__main__':
unittest.main()
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment