diff --git a/CHANGELOG.md b/CHANGELOG.md index 6abe0d8f97..5de7deac8d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -31,6 +31,8 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). - Allow use of `pathlib.Path | str` for paths in python bindings ([#2431](https://github.com/stack-of-tasks/pinocchio/pull/2431)) - Add Pseudo inertia and Log-cholesky parametrization ([#2296](https://github.com/stack-of-tasks/pinocchio/pull/2296)) - Add Pixi support ([#2459](https://github.com/stack-of-tasks/pinocchio/pull/2459)) +- Extend support for mimic joint: rnea, crba, forward kinematics, centroidal, jacobians and frames ([#2441](https://github.com/stack-of-tasks/pinocchio/pull/2441)) +- Add idx_j, nj members in joint models, with their respective column/row/block selectors. ([#2441](https://github.com/stack-of-tasks/pinocchio/pull/2441)) ### Fixed - Fix linkage of Boost.Serialization on Windows ([#2400](https://github.com/stack-of-tasks/pinocchio/pull/2400)) diff --git a/bindings/python/multibody/sample-models.cpp b/bindings/python/multibody/sample-models.cpp index 295e04f723..d47338a474 100644 --- a/bindings/python/multibody/sample-models.cpp +++ b/bindings/python/multibody/sample-models.cpp @@ -26,6 +26,13 @@ namespace pinocchio return model; } + Model buildSampleModelHumanoidRandom(bool usingFF, bool mimic) + { + Model model; + buildModels::humanoidRandom(model, usingFF, mimic); + return model; + } + Model buildSampleModelManipulator() { Model model; @@ -33,6 +40,13 @@ namespace pinocchio return model; } + Model buildSampleModelManipulator(bool mimic) + { + Model model; + buildModels::manipulator(model, mimic); + return model; + } + #ifdef PINOCCHIO_WITH_HPP_FCL GeometryModel buildSampleGeometryModelManipulator(const Model & model) { @@ -80,11 +94,23 @@ namespace pinocchio "Generate a (hard-coded) model of a humanoid robot with 6-DOF limbs and random joint " "placements.\nOnly meant for unit tests."); + bp::def( + "buildSampleModelHumanoidRandom", + static_cast(pinocchio::python::buildSampleModelHumanoidRandom), + bp::args("using_free_flyer", "mimic"), + "Generate a (hard-coded) model of a humanoid robot with 6-DOF limbs and random joint " + "placements.\nOnly meant for unit tests."); + bp::def( "buildSampleModelManipulator", static_cast(pinocchio::python::buildSampleModelManipulator), "Generate a (hard-coded) model of a simple manipulator."); + bp::def( + "buildSampleModelManipulator", + static_cast(pinocchio::python::buildSampleModelManipulator), + bp::args("mimic"), "Generate a (hard-coded) model of a simple manipulator."); + #ifdef PINOCCHIO_WITH_HPP_FCL bp::def( "buildSampleGeometryModelManipulator", diff --git a/bindings/python/parsers/urdf/model.cpp b/bindings/python/parsers/urdf/model.cpp index a1003f1191..27d62666ab 100644 --- a/bindings/python/parsers/urdf/model.cpp +++ b/bindings/python/parsers/urdf/model.cpp @@ -20,71 +20,82 @@ namespace pinocchio #ifdef PINOCCHIO_WITH_URDFDOM - Model buildModelFromUrdf(const bp::object & filename) + Model buildModelFromUrdf(const bp::object & filename, const bool mimic = false) { Model model; - pinocchio::urdf::buildModel(path(filename), model); + pinocchio::urdf::buildModel(path(filename), model, mimic); return model; } - Model & buildModelFromUrdf(const bp::object & filename, Model & model) + Model & buildModelFromUrdf(const bp::object & filename, Model & model, const bool mimic = false) { - return pinocchio::urdf::buildModel(path(filename), model); + return pinocchio::urdf::buildModel(path(filename), model, mimic); } - Model buildModelFromUrdf(const bp::object & filename, const JointModel & root_joint) + Model buildModelFromUrdf( + const bp::object & filename, const JointModel & root_joint, const bool mimic = false) { Model model; - pinocchio::urdf::buildModel(path(filename), root_joint, model); + pinocchio::urdf::buildModel(path(filename), root_joint, model, mimic); return model; } Model buildModelFromUrdf( const bp::object & filename, const JointModel & root_joint, - const std::string & root_joint_name) + const std::string & root_joint_name, + const bool mimic = false) { Model model; - pinocchio::urdf::buildModel(path(filename), root_joint, root_joint_name, model); + pinocchio::urdf::buildModel(path(filename), root_joint, root_joint_name, model, mimic); return model; } - Model & - buildModelFromUrdf(const bp::object & filename, const JointModel & root_joint, Model & model) + Model & buildModelFromUrdf( + const bp::object & filename, + const JointModel & root_joint, + Model & model, + const bool mimic = false) { - return pinocchio::urdf::buildModel(path(filename), root_joint, model); + return pinocchio::urdf::buildModel(path(filename), root_joint, model, mimic); } Model & buildModelFromUrdf( const bp::object & filename, const JointModel & root_joint, const std::string & root_joint_name, - Model & model) + Model & model, + const bool mimic = false) { - return pinocchio::urdf::buildModel(path(filename), root_joint, root_joint_name, model); + return pinocchio::urdf::buildModel(path(filename), root_joint, root_joint_name, model, mimic); } - Model buildModelFromXML(const std::string & xml_stream, const JointModel & root_joint) + Model buildModelFromXML( + const std::string & xml_stream, const JointModel & root_joint, const bool mimic = false) { Model model; - pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, model); + pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, model, mimic); return model; } Model buildModelFromXML( const std::string & xml_stream, const JointModel & root_joint, - const std::string & root_joint_name) + const std::string & root_joint_name, + const bool mimic = false) { Model model; - pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, root_joint_name, model); + pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, root_joint_name, model, mimic); return model; } - Model & - buildModelFromXML(const std::string & xml_stream, const JointModel & root_joint, Model & model) + Model & buildModelFromXML( + const std::string & xml_stream, + const JointModel & root_joint, + Model & model, + const bool mimic = false) { - pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, model); + pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, model, mimic); return model; } @@ -92,22 +103,24 @@ namespace pinocchio const std::string & xml_stream, const JointModel & root_joint, const std::string & root_joint_name, - Model & model) + Model & model, + const bool mimic = false) { - pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, root_joint_name, model); + pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, root_joint_name, model, mimic); return model; } - Model buildModelFromXML(const std::string & xml_stream) + Model buildModelFromXML(const std::string & xml_stream, const bool mimic = true) { Model model; - pinocchio::urdf::buildModelFromXML(xml_stream, model); + pinocchio::urdf::buildModelFromXML(xml_stream, model, mimic); return model; } - Model & buildModelFromXML(const std::string & xml_stream, Model & model) + Model & + buildModelFromXML(const std::string & xml_stream, Model & model, const bool mimic = true) { - pinocchio::urdf::buildModelFromXML(xml_stream, model); + pinocchio::urdf::buildModelFromXML(xml_stream, model, mimic); return model; } @@ -120,39 +133,41 @@ namespace pinocchio bp::def( "buildModelFromUrdf", - static_cast( + static_cast( pinocchio::python::buildModelFromUrdf), - bp::args("urdf_filename", "root_joint"), + bp::args("urdf_filename", "root_joint", "mimic"), "Parse the URDF file given in input and return a pinocchio Model starting with the " "given root joint."); bp::def( "buildModelFromUrdf", - static_cast( + static_cast( pinocchio::python::buildModelFromUrdf), - bp::args("urdf_filename", "root_joint", "root_joint_name"), + bp::args("urdf_filename", "root_joint", "root_joint_name", "mimic"), "Parse the URDF file given in input and return a pinocchio Model starting with the " "given root joint with its specified name."); bp::def( "buildModelFromUrdf", - static_cast(pinocchio::python::buildModelFromUrdf), - bp::args("urdf_filename"), + static_cast( + pinocchio::python::buildModelFromUrdf), + bp::args("urdf_filename", "mimic"), "Parse the URDF file given in input and return a pinocchio Model."); bp::def( "buildModelFromUrdf", - static_cast( + static_cast( pinocchio::python::buildModelFromUrdf), - bp::args("urdf_filename", "model"), + bp::args("urdf_filename", "model", "mimic"), "Append to a given model a URDF structure given by its filename.", bp::return_internal_reference<2>()); bp::def( "buildModelFromUrdf", - static_cast( + static_cast( pinocchio::python::buildModelFromUrdf), - bp::args("urdf_filename", "root_joint", "model"), + bp::args("urdf_filename", "root_joint", "model", "mimic"), "Append to a given model a URDF structure given by its filename and the root joint.\n" "Remark: In the URDF format, a joint of type fixed can be defined. For efficiency reasons," "it is treated as operational frame and not as a joint of the model.", @@ -161,8 +176,8 @@ namespace pinocchio bp::def( "buildModelFromUrdf", static_cast(pinocchio::python::buildModelFromUrdf), - bp::args("urdf_filename", "root_joint", "root_joint_name", "model"), + Model &, const bool)>(pinocchio::python::buildModelFromUrdf), + bp::args("urdf_filename", "root_joint", "root_joint_name", "model", "mimic"), "Append to a given model a URDF structure given by its filename and the root joint with " "its specified name.\n" "Remark: In the URDF format, a joint of type fixed can be defined. For efficiency reasons," @@ -171,25 +186,38 @@ namespace pinocchio bp::def( "buildModelFromXML", - static_cast( + static_cast( pinocchio::python::buildModelFromXML), - bp::args("urdf_xml_stream", "root_joint"), + bp::args("urdf_xml_stream", "root_joint", "mimic"), "Parse the URDF XML stream given in input and return a pinocchio Model starting with " "the given root joint."); bp::def( "buildModelFromXML", - static_cast( + static_cast( pinocchio::python::buildModelFromXML), - bp::args("urdf_xml_stream", "root_joint", "root_joint_name"), + bp::args( + "urdf_xml_stream", "root_joint", + "root_joint_name" + "mimic"), "Parse the URDF XML stream given in input and return a pinocchio Model starting with " "the given root joint with its specified name."); bp::def( "buildModelFromXML", - static_cast( + static_cast( pinocchio::python::buildModelFromXML), - bp::args("urdf_xml_stream", "root_joint", "model"), + bp::args("urdf_xml_stream", "root_joint", "root_joint_name", "mimic"), + "Parse the URDF XML stream given in input and return a pinocchio Model starting with " + "the given root joint with its specified name."); + + bp::def( + "buildModelFromXML", + static_cast( + pinocchio::python::buildModelFromXML), + bp::args("urdf_xml_stream", "root_joint", "model", "mimic"), "Parse the URDF XML stream given in input and append it to the input model with the " "given interfacing joint.", bp::return_internal_reference<3>()); @@ -197,23 +225,33 @@ namespace pinocchio bp::def( "buildModelFromXML", static_cast(pinocchio::python::buildModelFromXML), - bp::args("urdf_xml_stream", "root_joint", "root_joint_name", "model"), + Model &, const bool)>(pinocchio::python::buildModelFromXML), + bp::args("urdf_xml_stream", "root_joint", "root_joint_name", "model", "mimic"), + "Parse the URDF XML stream given in input and append it to the input model with the " + "given interfacing joint with its specified name.", + bp::return_internal_reference<3>()); + + bp::def( + "buildModelFromXML", + static_cast(pinocchio::python::buildModelFromXML), + bp::args("urdf_xml_stream", "root_joint", "root_joint_name", "model", "mimic"), "Parse the URDF XML stream given in input and append it to the input model with the " "given interfacing joint with its specified name.", bp::return_internal_reference<3>()); bp::def( "buildModelFromXML", - static_cast(pinocchio::python::buildModelFromXML), - bp::args("urdf_xml_stream"), + static_cast( + pinocchio::python::buildModelFromXML), + bp::args("urdf_xml_stream", "mimic"), "Parse the URDF XML stream given in input and return a pinocchio Model."); bp::def( "buildModelFromXML", - static_cast( + static_cast( pinocchio::python::buildModelFromXML), - bp::args("urdf_xml_stream", "model"), + bp::args("urdf_xml_stream", "model", "mimic"), "Parse the URDF XML stream given in input and append it to the input model.", bp::return_internal_reference<2>()); #endif diff --git a/bindings/python/pinocchio/shortcuts.py b/bindings/python/pinocchio/shortcuts.py index 8d01baec0d..85d101bac2 100644 --- a/bindings/python/pinocchio/shortcuts.py +++ b/bindings/python/pinocchio/shortcuts.py @@ -25,6 +25,7 @@ def buildModelsFromUrdf( - verbose - print information of parsing (default - False) - meshLoader - object used to load meshes (default - hpp::fcl::MeshLoader) - geometry_types - Which geometry model to load. Can be pin.GeometryType.COLLISION, pin.GeometryType.VISUAL or both. (default - [pin.GeometryType.COLLISION, pin.GeometryType.VISUAL]) + - mimic - If urdf mimic joints should be parsed or not (default - False) Return: Tuple of the models, in this order : model, collision model, and visual model. @@ -34,7 +35,14 @@ def buildModelsFromUrdf( Remark: In the URDF format, a joint of type fixed can be defined. For efficiency reasons, it is treated as operational frame and not as a joint of the model. """ # Handle the switch from old to new api - arg_keys = ["package_dirs", "root_joint", "verbose", "meshLoader", "geometry_types"] + arg_keys = [ + "package_dirs", + "root_joint", + "verbose", + "meshLoader", + "geometry_types", + "mimic", + ] if len(args) >= 3: if isinstance(args[2], str): arg_keys = [ @@ -44,6 +52,7 @@ def buildModelsFromUrdf( "verbose", "meshLoader", "geometry_types", + "mimic", ] for key, arg in zip(arg_keys, args): @@ -63,16 +72,17 @@ def _buildModelsFromUrdf( verbose=False, meshLoader=None, geometry_types=None, + mimic=False, ) -> Tuple[pin.Model, pin.GeometryModel, pin.GeometryModel]: if geometry_types is None: geometry_types = [pin.GeometryType.COLLISION, pin.GeometryType.VISUAL] if root_joint is None: - model = pin.buildModelFromUrdf(filename) + model = pin.buildModelFromUrdf(filename, mimic) elif root_joint is not None and root_joint_name is None: - model = pin.buildModelFromUrdf(filename, root_joint) + model = pin.buildModelFromUrdf(filename, root_joint, mimic) else: - model = pin.buildModelFromUrdf(filename, root_joint, root_joint_name) + model = pin.buildModelFromUrdf(filename, root_joint, root_joint_name, mimic) if verbose and not WITH_HPP_FCL and meshLoader is not None: print( diff --git a/examples/build-reduced-model.py b/examples/build-reduced-model.py index 85269ae598..d61ad4da36 100644 --- a/examples/build-reduced-model.py +++ b/examples/build-reduced-model.py @@ -78,7 +78,7 @@ # reference_configuration is optional: if not provided, neutral configuration used # you can even mix joint names and joint ids mixed_jointsToLockIDs = [jointsToLockIDs[0], "wrist_2_joint", "wrist_3_joint"] -robot = pin.RobotWrapper.BuildFromURDF(urdf_filename, mesh_dir) +robot = pin.RobotWrapper.BuildFromURDF(urdf_filename, mesh_dir, mimic=True) reduced_robot = robot.buildReducedRobot( list_of_joints_to_lock=mixed_jointsToLockIDs, reference_configuration=initialJointConfig, diff --git a/examples/collision-with-point-clouds.py b/examples/collision-with-point-clouds.py index dac0939b0d..5a933f18de 100644 --- a/examples/collision-with-point-clouds.py +++ b/examples/collision-with-point-clouds.py @@ -22,7 +22,7 @@ urdf_model_path = model_path / "panda_description/urdf" / urdf_filename model, collision_model, visual_model = pin.buildModelsFromUrdf( - urdf_model_path, mesh_dir + urdf_model_path, package_dirs=mesh_dir ) # Add point clouds diff --git a/examples/collisions.py b/examples/collisions.py index 1578efa4d9..d323f1fc1e 100644 --- a/examples/collisions.py +++ b/examples/collisions.py @@ -10,7 +10,7 @@ urdf_model_path = model_path / "romeo_description/urdf" / urdf_filename # Load model -model = pin.buildModelFromUrdf(urdf_model_path, pin.JointModelFreeFlyer()) +model = pin.buildModelFromUrdf(urdf_model_path, pin.JointModelFreeFlyer(), False) # Load collision geometries geom_model = pin.buildGeomFromUrdf( diff --git a/examples/geometry-models.py b/examples/geometry-models.py index c1ae8d4579..7544003005 100644 --- a/examples/geometry-models.py +++ b/examples/geometry-models.py @@ -14,7 +14,7 @@ # Load the urdf model model, collision_model, visual_model = pinocchio.buildModelsFromUrdf( - urdf_model_path, mesh_dir + urdf_model_path, mesh_dir, mimic=False ) print("model name: " + model.name) diff --git a/examples/overview-urdf.py b/examples/overview-urdf.py index 8aee4be196..9e01ccf037 100644 --- a/examples/overview-urdf.py +++ b/examples/overview-urdf.py @@ -15,7 +15,7 @@ ) # Load the urdf model -model = pinocchio.buildModelFromUrdf(urdf_filename) +model = pinocchio.buildModelFromUrdf(urdf_filename, False) print("model name: " + model.name) # Create data required by the algorithms diff --git a/include/pinocchio/algorithm/aba-derivatives.hxx b/include/pinocchio/algorithm/aba-derivatives.hxx index 2e2205c471..1d2198a740 100644 --- a/include/pinocchio/algorithm/aba-derivatives.hxx +++ b/include/pinocchio/algorithm/aba-derivatives.hxx @@ -74,7 +74,7 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointCols(data.J); + ColsBlock J_cols = jmodel.jointJacCols(data.J); J_cols = data.oMi[i].act(jdata.S()); } }; @@ -119,14 +119,14 @@ namespace pinocchio Force & fi = data.of[i]; typedef typename SizeDepType::template ColsReturn::Type ColBlock; - const ColBlock J_cols = jmodel.jointCols(data.J); + const ColBlock J_cols = jmodel.jointJacCols(data.J); - jmodel.jointVelocitySelector(data.u).noalias() -= J_cols.transpose() * fi.toVector(); + jmodel.jointVelocityFromNvSelector(data.u).noalias() -= J_cols.transpose() * fi.toVector(); jdata.U().noalias() = Ia * J_cols; jdata.StU().noalias() = J_cols.transpose() * jdata.U(); - jdata.StU().diagonal() += jmodel.jointVelocitySelector(model.armature); + jdata.StU().diagonal() += jmodel.jointVelocityFromNvSelector(model.armature); ::pinocchio::internal::PerformStYSInversion::run(jdata.StU(), jdata.Dinv()); jdata.UDinv().noalias() = jdata.U() * jdata.Dinv(); @@ -137,7 +137,7 @@ namespace pinocchio const int nv_children = data.nvSubtree[i] - jmodel.nv(); if (nv_children > 0) { - ColsBlock SDinv_cols = jmodel.jointCols(data.SDinv); + ColsBlock SDinv_cols = jmodel.jointVelCols(data.SDinv); SDinv_cols.noalias() = J_cols * jdata.Dinv(); Minv_.block(jmodel.idx_v(), jmodel.idx_v() + jmodel.nv(), jmodel.nv(), nv_children) @@ -162,8 +162,8 @@ namespace pinocchio { Ia.noalias() -= jdata.UDinv() * jdata.U().transpose(); - fi.toVector().noalias() += - Ia * data.oa_gf[i].toVector() + jdata.UDinv() * jmodel.jointVelocitySelector(data.u); + fi.toVector().noalias() += Ia * data.oa_gf[i].toVector() + + jdata.UDinv() * jmodel.jointVelocityFromNvSelector(data.u); data.oYaba[parent] += Ia; data.of[parent] += fi; } @@ -204,14 +204,14 @@ namespace pinocchio typename Data::Motion & oa_gf = data.oa_gf[i]; typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointCols(data.J); + ColsBlock J_cols = jmodel.jointJacCols(data.J); oa_gf += data.oa_gf[parent]; - jmodel.jointVelocitySelector(data.ddq).noalias() = - jdata.Dinv() * jmodel.jointVelocitySelector(data.u) + jmodel.jointVelocityFromNvSelector(data.ddq).noalias() = + jdata.Dinv() * jmodel.jointVelocityFromNvSelector(data.u) - jdata.UDinv().transpose() * oa_gf.toVector(); - oa_gf.toVector().noalias() += J_cols * jmodel.jointVelocitySelector(data.ddq); + oa_gf.toVector().noalias() += J_cols * jmodel.jointVelocityFromNvSelector(data.ddq); oa = oa_gf + model.gravity; of = data.oYcrb[i] * oa_gf + ov.cross(data.oh[i]); @@ -233,10 +233,10 @@ namespace pinocchio data.Fcrb[i].rightCols(model.nv - jmodel.idx_v()) += data.Fcrb[parent].rightCols(model.nv - jmodel.idx_v()); - ColsBlock dJ_cols = jmodel.jointCols(data.dJ); - ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); - ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); - ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); + ColsBlock dJ_cols = jmodel.jointJacCols(data.dJ); + ColsBlock dVdq_cols = jmodel.jointVelCols(data.dVdq); + ColsBlock dAdq_cols = jmodel.jointVelCols(data.dAdq); + ColsBlock dAdv_cols = jmodel.jointVelCols(data.dAdv); motionSet::motionAction(ov, J_cols, dJ_cols); motionSet::motionAction(data.oa_gf[parent], J_cols, dAdq_cols); @@ -303,13 +303,13 @@ namespace pinocchio typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointCols(data.J); - ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); - ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); - ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); - ColsBlock dFdq_cols = jmodel.jointCols(data.dFdq); - ColsBlock dFdv_cols = jmodel.jointCols(data.dFdv); - ColsBlock dFda_cols = jmodel.jointCols(data.dFda); + ColsBlock J_cols = jmodel.jointJacCols(data.J); + ColsBlock dVdq_cols = jmodel.jointVelCols(data.dVdq); + ColsBlock dAdq_cols = jmodel.jointVelCols(data.dAdq); + ColsBlock dAdv_cols = jmodel.jointVelCols(data.dAdv); + ColsBlock dFdq_cols = jmodel.jointVelCols(data.dFdq); + ColsBlock dFdv_cols = jmodel.jointVelCols(data.dFdv); + ColsBlock dFda_cols = jmodel.jointVelCols(data.dFda); // dtau/dv motionSet::inertiaAction(data.oYcrb[i], dAdv_cols, dFdv_cols); @@ -578,7 +578,7 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColBlock; - const ColBlock J_cols = jmodel.jointCols(data.J); + const ColBlock J_cols = jmodel.jointJacCols(data.J); MatrixType & Minv_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType, Minv); @@ -586,7 +586,7 @@ namespace pinocchio const int nv_children = data.nvSubtree[i] - jmodel.nv(); if (nv_children > 0) { - ColsBlock SDinv_cols = jmodel.jointCols(data.SDinv); + ColsBlock SDinv_cols = jmodel.jointVelCols(data.SDinv); SDinv_cols.noalias() = J_cols * jdata.Dinv(); Minv_.block(jmodel.idx_v(), jmodel.idx_v() + jmodel.nv(), jmodel.nv(), nv_children) @@ -649,7 +649,7 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - const ColsBlock J_cols = jmodel.jointCols(data.J); + const ColsBlock J_cols = jmodel.jointJacCols(data.J); // Already done in optimized::aba // oa = oa_gf + model.gravity; @@ -674,10 +674,10 @@ namespace pinocchio data.Fcrb[i].rightCols(model.nv - jmodel.idx_v()) += data.Fcrb[parent].rightCols(model.nv - jmodel.idx_v()); - ColsBlock dJ_cols = jmodel.jointCols(data.dJ); - ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); - ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); - ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); + ColsBlock dJ_cols = jmodel.jointJacCols(data.dJ); + ColsBlock dVdq_cols = jmodel.jointVelCols(data.dVdq); + ColsBlock dAdq_cols = jmodel.jointVelCols(data.dAdq); + ColsBlock dAdv_cols = jmodel.jointVelCols(data.dAdv); motionSet::motionAction(ov, J_cols, dJ_cols); motionSet::motionAction(data.oa_gf[parent], J_cols, dAdq_cols); diff --git a/include/pinocchio/algorithm/aba.hxx b/include/pinocchio/algorithm/aba.hxx index e0798504d4..fc75b85cd1 100644 --- a/include/pinocchio/algorithm/aba.hxx +++ b/include/pinocchio/algorithm/aba.hxx @@ -120,7 +120,7 @@ namespace pinocchio else data.oMi[i] = data.liMi[i]; - jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S()); + jmodel.jointJacCols(data.J) = data.oMi[i].act(jdata.S()); ov = data.oMi[i].act(jdata.v()); if (parent > 0) @@ -165,17 +165,17 @@ namespace pinocchio typename Inertia::Matrix6 & Ia = data.oYaba[i]; typedef typename SizeDepType::template ColsReturn::Type ColBlock; - ColBlock Jcols = jmodel.jointCols(data.J); + ColBlock Jcols = jmodel.jointJacCols(data.J); Force & fi = data.of[i]; - jmodel.jointVelocitySelector(data.u).noalias() -= Jcols.transpose() * fi.toVector(); + jmodel.jointVelocityFromDofSelector(data.u).noalias() -= Jcols.transpose() * fi.toVector(); jdata.U().noalias() = Ia * Jcols; jdata.StU().noalias() = Jcols.transpose() * jdata.U(); // Account for the rotor inertia contribution - jdata.StU().diagonal() += jmodel.jointVelocitySelector(model.armature); + jdata.StU().diagonal() += jmodel.jointVelocityFromDofSelector(model.armature); ::pinocchio::internal::PerformStYSInversion::run(jdata.StU(), jdata.Dinv()); jdata.UDinv().noalias() = jdata.U() * jdata.Dinv(); @@ -184,8 +184,8 @@ namespace pinocchio { Ia.noalias() -= jdata.UDinv() * jdata.U().transpose(); - fi.toVector().noalias() += - Ia * data.oa_gf[i].toVector() + jdata.UDinv() * jmodel.jointVelocitySelector(data.u); + fi.toVector().noalias() += Ia * data.oa_gf[i].toVector() + + jdata.UDinv() * jmodel.jointVelocityFromDofSelector(data.u); data.oYaba[parent] += Ia; data.of[parent] += fi; } @@ -214,16 +214,17 @@ namespace pinocchio typedef typename Data::Matrix6x Matrix6x; typedef typename SizeDepType::template ColsReturn::Type ColBlock; - ColBlock J_cols = jmodel.jointCols(data.J); + ColBlock J_cols = jmodel.jointJacCols(data.J); const JointIndex i = jmodel.id(); const JointIndex parent = model.parents[i]; data.oa_gf[i] += data.oa_gf[parent]; // does take into account the gravity field - jmodel.jointVelocitySelector(data.ddq).noalias() = - jdata.Dinv() * jmodel.jointVelocitySelector(data.u) + jmodel.jointVelocityFromDofSelector(data.ddq).noalias() = + jdata.Dinv() * jmodel.jointVelocityFromDofSelector(data.u) - jdata.UDinv().transpose() * data.oa_gf[i].toVector(); - data.oa_gf[i].toVector().noalias() += J_cols * jmodel.jointVelocitySelector(data.ddq); + data.oa_gf[i].toVector().noalias() += + J_cols * jmodel.jointVelocityFromDofSelector(data.ddq); // Handle consistent output data.oa[i] = data.oa_gf[i] + model.gravity; @@ -423,15 +424,15 @@ namespace pinocchio const JointIndex parent = model.parents[i]; typename Inertia::Matrix6 & Ia = data.Yaba[i]; - jmodel.jointVelocitySelector(data.u) -= jdata.S().transpose() * data.f[i]; + jmodel.jointVelocityFromDofSelector(data.u) -= jdata.S().transpose() * data.f[i]; jmodel.calc_aba( - jdata.derived(), jmodel.jointVelocitySelector(model.armature), Ia, parent > 0); + jdata.derived(), jmodel.jointVelocityFromDofSelector(model.armature), Ia, parent > 0); if (parent > 0) { Force & pa = data.f[i]; - pa.toVector().noalias() += - Ia * data.a_gf[i].toVector() + jdata.UDinv() * jmodel.jointVelocitySelector(data.u); + pa.toVector().noalias() += Ia * data.a_gf[i].toVector() + + jdata.UDinv() * jmodel.jointVelocityFromDofSelector(data.u); data.Yaba[parent] += internal::SE3actOn::run(data.liMi[i], Ia); data.f[parent] += data.liMi[i].act(pa); } @@ -461,10 +462,10 @@ namespace pinocchio const JointIndex parent = model.parents[i]; data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]); - jmodel.jointVelocitySelector(data.ddq).noalias() = - jdata.Dinv() * jmodel.jointVelocitySelector(data.u) + jmodel.jointVelocityFromDofSelector(data.ddq).noalias() = + jdata.Dinv() * jmodel.jointVelocityFromDofSelector(data.u) - jdata.UDinv().transpose() * data.a_gf[i].toVector(); - data.a_gf[i] += jdata.S() * jmodel.jointVelocitySelector(data.ddq); + data.a_gf[i] += jdata.S() * jmodel.jointVelocityFromDofSelector(data.ddq); data.a[i] = data.a_gf[i]; data.a[i].linear().noalias() += data.oMi[i].rotation().transpose() * model.gravity.linear(); @@ -636,7 +637,7 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointCols(data.J); + ColsBlock J_cols = jmodel.jointJacCols(data.J); J_cols = data.oMi[i].act(jdata.S()); data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); @@ -674,13 +675,13 @@ namespace pinocchio typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointCols(data.J); + ColsBlock J_cols = jmodel.jointJacCols(data.J); jdata.U().noalias() = Ia * J_cols; jdata.StU().noalias() = J_cols.transpose() * jdata.U(); // Account for the rotor inertia contribution - jdata.StU().diagonal() += jmodel.jointVelocitySelector(model.armature); + jdata.StU().diagonal() += jmodel.jointVelocityFromDofSelector(model.armature); ::pinocchio::internal::PerformStYSInversion::run(jdata.StU(), jdata.Dinv()); jdata.UDinv().noalias() = jdata.U() * jdata.Dinv(); @@ -689,7 +690,7 @@ namespace pinocchio const int nv_children = data.nvSubtree[i] - jmodel.nv(); if (nv_children > 0) { - ColsBlock SDinv_cols = jmodel.jointCols(data.SDinv); + ColsBlock SDinv_cols = jmodel.jointVelCols(data.SDinv); SDinv_cols.noalias() = J_cols * jdata.Dinv(); Minv.block(jmodel.idx_v(), jmodel.idx_v() + jmodel.nv(), jmodel.nv(), nv_children) @@ -748,13 +749,13 @@ namespace pinocchio typename SizeDepType::template ColsReturn::Type ColsBlock; - const ColsBlock J_cols = jmodel.jointCols(data.J); + const ColsBlock J_cols = jmodel.jointJacCols(data.J); Minv.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), jmodel.nv()) = jdata.Dinv(); const int nv_children = data.nvSubtree[i] - jmodel.nv(); if (nv_children > 0) { - ColsBlock SDinv_cols = jmodel.jointCols(data.SDinv); + ColsBlock SDinv_cols = jmodel.jointVelCols(data.SDinv); SDinv_cols.noalias() = J_cols * jdata.Dinv(); Minv.block(jmodel.idx_v(), jmodel.idx_v() + jmodel.nv(), jmodel.nv(), nv_children) .noalias() = @@ -804,7 +805,7 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointCols(data.J); + ColsBlock J_cols = jmodel.jointJacCols(data.J); if (parent > 0) { diff --git a/include/pinocchio/algorithm/center-of-mass-derivatives.hxx b/include/pinocchio/algorithm/center-of-mass-derivatives.hxx index 6bd999b8f6..be9d6a0a81 100644 --- a/include/pinocchio/algorithm/center-of-mass-derivatives.hxx +++ b/include/pinocchio/algorithm/center-of-mass-derivatives.hxx @@ -48,7 +48,7 @@ namespace pinocchio Matrix3xOut & dvcom_dq = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut, vcom_partial_dq); typename SizeDepType::template ColsReturn::Type - dvcom_dqi = jmodel.jointCols(dvcom_dq); + dvcom_dqi = jmodel.jointVelCols(dvcom_dq); Motion vpc = (parent > 0) ? (data.v[i] - (Motion)jdata.v()) : Motion::Zero(); vpc.linear() -= data.vcom[i]; // vpc = v_{parent+c} = [ v_parent+vc; w_parent ] diff --git a/include/pinocchio/algorithm/center-of-mass.hxx b/include/pinocchio/algorithm/center-of-mass.hxx index 11ff1cea47..d67be398b8 100644 --- a/include/pinocchio/algorithm/center-of-mass.hxx +++ b/include/pinocchio/algorithm/center-of-mass.hxx @@ -257,12 +257,12 @@ namespace pinocchio Matrix3x & Jcom_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3x, Jcom); - ColBlock Jcols = jmodel.jointCols(data.J); + ColBlock Jcols = jmodel.jointJacCols(data.J); Jcols = data.oMi[i].act(jdata.S()); - for (Eigen::DenseIndex col_id = 0; col_id < jmodel.nv(); ++col_id) + for (Eigen::DenseIndex col_id = 0; col_id < jmodel.nj(); ++col_id) { - jmodel.jointCols(Jcom_).col(col_id) = + jmodel.jointVelCols(Jcom_).col(col_id) += data.mass[i] * Jcols.col(col_id).template segment<3>(Motion::LINEAR) - data.com[i].cross(Jcols.col(col_id).template segment<3>(Motion::ANGULAR)); } @@ -318,6 +318,7 @@ namespace pinocchio } // Backward step + data.Jcom.setZero(); typedef JacobianCenterOfMassBackwardStep Pass2; for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) { @@ -366,12 +367,12 @@ namespace pinocchio Matrix3x & Jcom_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3x, Jcom); - ColBlock Jcols = jmodel.jointCols(data.J); + ColBlock Jcols = jmodel.jointJacCols(data.J); Jcols = data.oMi[i].act(jdata.S()); - for (Eigen::DenseIndex col_id = 0; col_id < jmodel.nv(); ++col_id) + for (Eigen::DenseIndex col_id = 0; col_id < jmodel.nj(); ++col_id) { - jmodel.jointCols(Jcom_).col(col_id) = + jmodel.jointVelCols(Jcom_).col(col_id) += Jcols.col(col_id).template segment<3>(Motion::LINEAR) - data.com[subtree_root_id].cross(Jcols.col(col_id).template segment<3>(Motion::ANGULAR)); } @@ -444,6 +445,7 @@ namespace pinocchio } // Backward step + data.Jcom.setZero(); typedef JacobianCenterOfMassBackwardStep Pass2; for (Eigen::DenseIndex k = (Eigen::DenseIndex)subtree.size() - 1; k >= 0; --k) @@ -474,6 +476,7 @@ namespace pinocchio Jcom_subtree.middleCols(idx_v, nv_subtree) *= mass_inv_subtree; // Second backward step + typedef JacobianSubtreeCenterOfMassBackwardStep< Scalar, Options, JointCollectionTpl, Matrix3xLike> Pass3; diff --git a/include/pinocchio/algorithm/centroidal-derivatives.hxx b/include/pinocchio/algorithm/centroidal-derivatives.hxx index 68dafb3a06..35e68ef917 100644 --- a/include/pinocchio/algorithm/centroidal-derivatives.hxx +++ b/include/pinocchio/algorithm/centroidal-derivatives.hxx @@ -58,6 +58,11 @@ namespace pinocchio typedef typename Model::JointIndex JointIndex; typedef typename Data::Motion Motion; + assert( + (std::is_same>::value + == false) + && "Algorithm not supported for mimic joints"); + const JointIndex & i = jmodel.id(); const JointIndex & parent = model.parents[i]; Motion & ov = data.ov[i]; @@ -78,7 +83,7 @@ namespace pinocchio data.oMi[i] = data.liMi[i]; data.a[i] = - jdata.S() * jmodel.jointVelocitySelector(a) + jdata.c() + (data.v[i] ^ jdata.v()); + jdata.S() * jmodel.jointVelocityFromNvSelector(a) + jdata.c() + (data.v[i] ^ jdata.v()); if (parent > 0) { data.a[i] += data.liMi[i].actInv(data.a[parent]); @@ -94,11 +99,11 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointCols(data.J); - ColsBlock dJ_cols = jmodel.jointCols(data.dJ); - ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); - ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); - ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); + ColsBlock J_cols = jmodel.jointJacCols(data.J); + ColsBlock dJ_cols = jmodel.jointJacCols(data.dJ); + ColsBlock dVdq_cols = jmodel.jointVelCols(data.dVdq); + ColsBlock dAdq_cols = jmodel.jointVelCols(data.dAdq); + ColsBlock dAdv_cols = jmodel.jointVelCols(data.dAdv); J_cols = data.oMi[i].act(jdata.S()); motionSet::motionAction(ov, J_cols, dJ_cols); @@ -149,6 +154,7 @@ namespace pinocchio template static void algo(const JointModelBase & jmodel, const Model & model, Data & data) { + typedef typename Model::JointIndex JointIndex; const JointIndex & i = jmodel.id(); @@ -158,17 +164,17 @@ namespace pinocchio typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointCols(data.J); - ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); - ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); - ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); - ColsBlock dHdq_cols = jmodel.jointCols(data.dHdq); - ColsBlock dFdq_cols = jmodel.jointCols(data.dFdq); - ColsBlock dFdv_cols = jmodel.jointCols(data.dFdv); - ColsBlock dFda_cols = jmodel.jointCols(data.dFda); + ColsBlock J_cols = jmodel.jointJacCols(data.J); + ColsBlock dVdq_cols = jmodel.jointVelCols(data.dVdq); + ColsBlock dAdq_cols = jmodel.jointVelCols(data.dAdq); + ColsBlock dAdv_cols = jmodel.jointVelCols(data.dAdv); + ColsBlock dHdq_cols = jmodel.jointVelCols(data.dHdq); + ColsBlock dFdq_cols = jmodel.jointVelCols(data.dFdq); + ColsBlock dFdv_cols = jmodel.jointVelCols(data.dFdv); + ColsBlock dFda_cols = jmodel.jointVelCols(data.dFda); // tau - jmodel.jointVelocitySelector(data.tau).noalias() = + jmodel.jointVelocityFromNvSelector(data.tau).noalias() = J_cols.transpose() * data.of[i].toVector(); // dtau/da similar to data.M @@ -369,16 +375,21 @@ namespace pinocchio typename SizeDepType::template ColsReturn::Type ColsBlock; + assert( + (std::is_same>::value + == false) + && "Algorithm not supported for mimic joints"); + const JointIndex & i = jmodel.id(); const JointIndex & parent = model.parents[i]; typename Data::Motion & vtmp = data.v[0]; typename Data::Matrix6x & Ftmp = data.Fcrb[0]; - ColsBlock J_cols = jmodel.jointCols(data.J); - ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); - ColsBlock dHdq_cols = jmodel.jointCols(data.dHdq); - ColsBlock Ftmp_cols = jmodel.jointCols(Ftmp); + ColsBlock J_cols = jmodel.jointJacCols(data.J); + ColsBlock dVdq_cols = jmodel.jointVelCols(data.dVdq); + ColsBlock dHdq_cols = jmodel.jointVelCols(data.dHdq); + ColsBlock Ftmp_cols = jmodel.jointVelCols(Ftmp); const Vector3 mg = data.oYcrb[i].mass() * model.gravity.linear(); for (Eigen::DenseIndex k = 0; k < jmodel.nv(); ++k) diff --git a/include/pinocchio/algorithm/centroidal.hxx b/include/pinocchio/algorithm/centroidal.hxx index 3e9f1aaafe..1786cf33f8 100644 --- a/include/pinocchio/algorithm/centroidal.hxx +++ b/include/pinocchio/algorithm/centroidal.hxx @@ -136,11 +136,12 @@ namespace pinocchio const JointIndex & i = jmodel.id(); const JointIndex & parent = model.parents[i]; - ColsBlock J_cols = jmodel.jointCols(data.J); + ColsBlock J_cols = jmodel.jointJacCols(data.J); J_cols = data.oMi[i].act(jdata.S()); - ColsBlock Ag_cols = jmodel.jointCols(data.Ag); - motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); + ColsBlock Ag_cols = jmodel.jointVelCols(data.Ag); + motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); + data.oYcrb[parent] += data.oYcrb[i]; } @@ -173,6 +174,7 @@ namespace pinocchio data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); typedef CcrbaBackwardStep Pass2; + data.Ag.setZero(); for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) { Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data)); @@ -220,6 +222,7 @@ namespace pinocchio data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); typedef CcrbaBackwardStep Pass2; + data.Ag.setZero(); for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) { Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data)); @@ -264,10 +267,10 @@ namespace pinocchio const Inertia & Y = data.oYcrb[i]; const typename Inertia::Matrix6 & doYcrb = data.doYcrb[i]; - ColsBlock J_cols = jmodel.jointCols(data.J); + ColsBlock J_cols = jmodel.jointJacCols(data.J); J_cols = data.oMi[i].act(jdata.S()); - ColsBlock dJ_cols = jmodel.jointCols(data.dJ); + ColsBlock dJ_cols = jmodel.jointJacCols(data.dJ); motionSet::motionAction(data.ov[i], J_cols, dJ_cols); data.oYcrb[parent] += Y; @@ -275,11 +278,11 @@ namespace pinocchio data.doYcrb[parent] += doYcrb; // Calc Ag - ColsBlock Ag_cols = jmodel.jointCols(data.Ag); + ColsBlock Ag_cols = jmodel.jointVelCols(data.Ag); motionSet::inertiaAction(Y, J_cols, Ag_cols); // Calc dAg = Ivx + vxI - ColsBlock dAg_cols = jmodel.jointCols(data.dAg); + ColsBlock dAg_cols = jmodel.jointVelCols(data.dAg); dAg_cols.noalias() = doYcrb * J_cols; motionSet::inertiaAction(Y, dJ_cols, dAg_cols); } diff --git a/include/pinocchio/algorithm/check.hxx b/include/pinocchio/algorithm/check.hxx index 8b0e9045e6..f297b94591 100644 --- a/include/pinocchio/algorithm/check.hxx +++ b/include/pinocchio/algorithm/check.hxx @@ -124,7 +124,7 @@ namespace pinocchio CHECK_DATA(data.U.rows() == model.nv); CHECK_DATA(data.D.size() == model.nv); CHECK_DATA(data.tmp.size() >= model.nv); - CHECK_DATA(data.J.cols() == model.nv); + CHECK_DATA(data.J.cols() == model.nj); CHECK_DATA(data.Jcom.cols() == model.nv); CHECK_DATA(data.torque_residual.size() == model.nv); CHECK_DATA(data.dq_after.size() == model.nv); @@ -154,10 +154,14 @@ namespace pinocchio CHECK_DATA(model.idx_qs[joint_id] == jmodel.idx_q()); CHECK_DATA(model.nvs[joint_id] == jmodel.nv()); CHECK_DATA(model.idx_vs[joint_id] == jmodel.idx_v()); + CHECK_DATA(model.njs[joint_id] == jmodel.nj()); + CHECK_DATA(model.idx_js[joint_id] == jmodel.idx_j()); } for (JointIndex j = 1; int(j) < model.njoints; ++j) { + if (boost::get>(&model.joints[j])) + continue; JointIndex c = (JointIndex)data.lastChild[j]; CHECK_DATA((int)c < model.njoints); int nv = model.joints[j].nv(); @@ -172,6 +176,7 @@ namespace pinocchio CHECK_DATA((model.parents[d] < j) || (model.parents[d] > c)); int row = model.joints[j].idx_v(); + CHECK_DATA(data.nvSubtree[j] == data.nvSubtree_fromRow[(size_t)row]); const JointModel & jparent = model.joints[model.parents[j]]; diff --git a/include/pinocchio/algorithm/compute-all-terms.hxx b/include/pinocchio/algorithm/compute-all-terms.hxx index 3ca4fa2866..da517dbacb 100644 --- a/include/pinocchio/algorithm/compute-all-terms.hxx +++ b/include/pinocchio/algorithm/compute-all-terms.hxx @@ -70,10 +70,10 @@ namespace pinocchio data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); data.doYcrb[i] = data.oYcrb[i].variation(data.ov[i]); - ColsBlock J_cols = jmodel.jointCols(data.J); + ColsBlock J_cols = jmodel.jointJacCols(data.J); J_cols = data.oMi[i].act(jdata.S()); - ColsBlock dJ_cols = jmodel.jointCols(data.dJ); + ColsBlock dJ_cols = jmodel.jointJacCols(data.dJ); motionSet::motionAction(data.ov[i], J_cols, dJ_cols); data.a_gf[i] = data.a[i] = jdata.c() + (data.v[i] ^ jdata.v()); @@ -103,6 +103,11 @@ namespace pinocchio const Model & model, Data & data) { + assert( + (std::is_same>::value + == false) + && "Algorithm not supported for mimic joints"); + typedef typename Model::JointIndex JointIndex; typedef typename SizeDepType::template ColsReturn::Type @@ -111,10 +116,10 @@ namespace pinocchio const JointIndex i = jmodel.id(); const JointIndex parent = model.parents[i]; - ColsBlock J_cols = jmodel.jointCols(data.J); - ColsBlock dJ_cols = jmodel.jointCols(data.dJ); - ColsBlock Ag_cols = jmodel.jointCols(data.Ag); - ColsBlock dAg_cols = jmodel.jointCols(data.dAg); + ColsBlock J_cols = jmodel.jointJacCols(data.J); + ColsBlock dJ_cols = jmodel.jointJacCols(data.dJ); + ColsBlock Ag_cols = jmodel.jointVelCols(data.Ag); + ColsBlock dAg_cols = jmodel.jointVelCols(data.dAg); // Calc Ag = Y * S motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); @@ -124,10 +129,10 @@ namespace pinocchio motionSet::inertiaAction(data.oYcrb[i], dJ_cols, dAg_cols); /* M[i,SUBTREE] = S'*F[1:6,SUBTREE] */ - data.M.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]).noalias() = + jmodel.jointVelRows(data.M).middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() = J_cols.transpose() * data.Ag.middleCols(jmodel.idx_v(), data.nvSubtree[i]); - jmodel.jointVelocitySelector(data.nle) = jdata.S().transpose() * data.f[i]; + jmodel.jointVelocityFromDofSelector(data.nle) += jdata.S().transpose() * data.f[i]; data.oYcrb[parent] += data.oYcrb[i]; data.doYcrb[parent] += data.doYcrb[i]; @@ -160,6 +165,7 @@ namespace pinocchio typedef DataTpl Data; + data.nle.setZero(); data.v[0].setZero(); data.a[0].setZero(); data.h[0].setZero(); diff --git a/include/pinocchio/algorithm/constrained-dynamics-derivatives.hxx b/include/pinocchio/algorithm/constrained-dynamics-derivatives.hxx index fb986b8295..695bad7328 100644 --- a/include/pinocchio/algorithm/constrained-dynamics-derivatives.hxx +++ b/include/pinocchio/algorithm/constrained-dynamics-derivatives.hxx @@ -50,14 +50,14 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointCols(data.J); - ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); + ColsBlock J_cols = jmodel.jointJacCols(data.J); + ColsBlock dAdq_cols = jmodel.jointVelCols(data.dAdq); if (ContactMode) { const Motion & ov = data.ov[i]; - ColsBlock dJ_cols = jmodel.jointCols(data.dJ); - ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); + ColsBlock dJ_cols = jmodel.jointJacCols(data.dJ); + ColsBlock dVdq_cols = jmodel.jointVelCols(data.dVdq); motionSet::motionAction(ov, J_cols, dJ_cols); // TODO: make more efficient @@ -79,10 +79,10 @@ namespace pinocchio RNEAForwardStepType::addForceCrossMatrix(data.oh[i], data.doYcrb[i]); Motion & oa = data.oa[i]; Motion & oa_gf = data.oa_gf[i]; - ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); + ColsBlock dAdv_cols = jmodel.jointVelCols(data.dAdv); const typename Data::TangentVectorType & a = data.ddq; data.a[i] = - jdata.S() * jmodel.jointVelocitySelector(a) + jdata.c() + (data.v[i] ^ jdata.v()); + jdata.S() * jmodel.jointVelocityFromDofSelector(a) + jdata.c() + (data.v[i] ^ jdata.v()); if (parent > 0) data.a[i] += data.liMi[i].actInv(data.a[parent]); oa = data.oMi[i].act(data.a[i]); @@ -102,7 +102,7 @@ namespace pinocchio Motion & odvparent = data.oa[parent]; const typename Data::TangentVectorType & dimpulse = data.ddq; // Temporary calculation of J(dq_after) - odv = J_cols * jmodel.jointVelocitySelector(dimpulse); + odv = J_cols * jmodel.jointVelocityFromDofSelector(dimpulse); if (parent > 0) odv += odvparent; motionSet::motionAction(odvparent, J_cols, dAdq_cols); @@ -142,11 +142,11 @@ namespace pinocchio const JointIndex i = jmodel.id(); const JointIndex parent = model.parents[i]; - ColsBlock J_cols = jmodel.jointCols(data.J); - ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); - ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); - ColsBlock dFdq_cols = jmodel.jointCols(data.dFdq); - ColsBlock dFda_cols = jmodel.jointCols(data.dFda); + ColsBlock J_cols = jmodel.jointJacCols(data.J); + ColsBlock dVdq_cols = jmodel.jointVelCols(data.dVdq); + ColsBlock dAdq_cols = jmodel.jointVelCols(data.dAdq); + ColsBlock dFdq_cols = jmodel.jointVelCols(data.dFdq); + ColsBlock dFda_cols = jmodel.jointVelCols(data.dFda); typename Data::RowMatrixXs & dtau_dq = data.dtau_dq; @@ -185,8 +185,8 @@ namespace pinocchio if (ContactMode) { - ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); - ColsBlock dFdv_cols = jmodel.jointCols(data.dFdv); + ColsBlock dAdv_cols = jmodel.jointVelCols(data.dAdv); + ColsBlock dFdv_cols = jmodel.jointVelCols(data.dFdv); typename Data::RowMatrixXs & dtau_dv = data.dtau_dv; dFdv_cols.noalias() = data.doYcrb[i] * J_cols; diff --git a/include/pinocchio/algorithm/constrained-dynamics.hxx b/include/pinocchio/algorithm/constrained-dynamics.hxx index f1bc7fbb3d..f88ea81777 100644 --- a/include/pinocchio/algorithm/constrained-dynamics.hxx +++ b/include/pinocchio/algorithm/constrained-dynamics.hxx @@ -115,7 +115,7 @@ namespace pinocchio if (parent > 0) ov += data.ov[parent]; - jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S()); + jmodel.jointJacCols(data.J) = data.oMi[i].act(jdata.S()); oinertias = data.oMi[i].act(model.inertias[i]); data.oYcrb[i] = data.oinertias[i]; if (ContactMode) @@ -162,17 +162,17 @@ namespace pinocchio const JointIndex i = jmodel.id(); const JointIndex parent = model.parents[i]; - ColsBlock dFda_cols = jmodel.jointCols(data.dFda); - const ColsBlock J_cols = jmodel.jointCols(data.J); + ColsBlock dFda_cols = jmodel.jointVelCols(data.dFda); + const ColsBlock J_cols = jmodel.jointJacCols(data.J); motionSet::inertiaAction(data.oYcrb[i], J_cols, dFda_cols); - data.M.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]).noalias() = + jmodel.jointVelRows(data.M).middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() = J_cols.transpose() * data.dFda.middleCols(jmodel.idx_v(), data.nvSubtree[i]); data.oYcrb[parent] += data.oYcrb[i]; if (ContactMode) { - jmodel.jointVelocitySelector(data.nle).noalias() = + jmodel.jointVelocityFromNvSelector(data.nle).noalias() = J_cols.transpose() * data.of[i].toVector(); data.of[parent] += data.of[i]; } @@ -562,7 +562,7 @@ namespace pinocchio else data.oMi[i] = data.liMi[i]; - jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S()); + jmodel.jointJacCols(data.J) = data.oMi[i].act(jdata.S()); data.ov[i] = data.oMi[i].act(jdata.v()); if (parent > 0) @@ -618,20 +618,20 @@ namespace pinocchio typename Inertia::Matrix6 & Ia = data.oYaba[i]; typedef typename SizeDepType::template ColsReturn::Type ColBlock; - ColBlock Jcols = jmodel.jointCols(data.J); + ColBlock Jcols = jmodel.jointJacCols(data.J); Force & fi_augmented = data.of_augmented[i]; const Force & fi = data.of[i]; fi_augmented += fi; - jmodel.jointVelocitySelector(data.u).noalias() = - jmodel.jointVelocitySelector(tau) - Jcols.transpose() * fi_augmented.toVector(); + jmodel.jointVelocityFromNvSelector(data.u).noalias() = + jmodel.jointVelocityFromNvSelector(tau) - Jcols.transpose() * fi_augmented.toVector(); jdata.U().noalias() = Ia * Jcols; jdata.StU().noalias() = Jcols.transpose() * jdata.U(); // Account for the rotor inertia contribution - jdata.StU().diagonal() += jmodel.jointVelocitySelector(model.armature); + jdata.StU().diagonal() += jmodel.jointVelocityFromNvSelector(model.armature); internal::PerformStYSInversion::run(jdata.StU(), jdata.Dinv()); jdata.UDinv().noalias() = jdata.U() * jdata.Dinv(); @@ -641,7 +641,7 @@ namespace pinocchio Ia.noalias() -= jdata.UDinv() * jdata.U().transpose(); fi_augmented.toVector().noalias() += - Ia * data.oa[i].toVector() + jdata.UDinv() * jmodel.jointVelocitySelector(data.u); + Ia * data.oa[i].toVector() + jdata.UDinv() * jmodel.jointVelocityFromNvSelector(data.u); data.oYaba[parent] += Ia; data.of_augmented[parent] += fi_augmented; } @@ -680,19 +680,19 @@ namespace pinocchio typename Inertia::Matrix6 & Ia = data.oYaba[i]; typedef typename SizeDepType::template ColsReturn::Type ColBlock; - ColBlock Jcols = jmodel.jointCols(data.J); + ColBlock Jcols = jmodel.jointJacCols(data.J); Force & fi_augmented = data.of_augmented[i]; const Force & fi = data.of[i]; fi_augmented += fi; - jmodel.jointVelocitySelector(data.u).noalias() = - jmodel.jointVelocitySelector(tau) - Jcols.transpose() * fi_augmented.toVector(); + jmodel.jointVelocityFromNvSelector(data.u).noalias() = + jmodel.jointVelocityFromNvSelector(tau) - Jcols.transpose() * fi_augmented.toVector(); if (parent > 0) { fi_augmented.toVector().noalias() += - Ia * data.oa[i].toVector() + jdata.UDinv() * jmodel.jointVelocitySelector(data.u); + Ia * data.oa[i].toVector() + jdata.UDinv() * jmodel.jointVelocityFromNvSelector(data.u); data.of_augmented[parent] += fi_augmented; } } @@ -719,7 +719,7 @@ namespace pinocchio typedef typename Data::Matrix6x Matrix6x; typedef typename SizeDepType::template ColsReturn::Type ColBlock; - ColBlock Jcols = jmodel.jointCols(data.J); + ColBlock Jcols = jmodel.jointJacCols(data.J); const JointIndex & i = jmodel.id(); const JointIndex & parent = model.parents[i]; @@ -728,10 +728,10 @@ namespace pinocchio if (parent > 0) data.oa_augmented[i] += data.oa_augmented[parent]; // does not take into account the gravity field - jmodel.jointVelocitySelector(data.ddq).noalias() = - jdata.Dinv() * jmodel.jointVelocitySelector(data.u) + jmodel.jointVelocityFromNvSelector(data.ddq).noalias() = + jdata.Dinv() * jmodel.jointVelocityFromNvSelector(data.u) - jdata.UDinv().transpose() * data.oa_augmented[i].toVector(); - data.oa_augmented[i].toVector() += Jcols * jmodel.jointVelocitySelector(data.ddq); + data.oa_augmented[i].toVector() += Jcols * jmodel.jointVelocityFromNvSelector(data.ddq); } }; @@ -1090,8 +1090,8 @@ namespace pinocchio data.of[parent] += data.of[i]; // subtract the bias forces from the torque to get Mv_dot_free - jmodel.jointVelocitySelector(data.tau).noalias() -= - jmodel.jointCols(data.J).transpose() * (data.of[i].toVector()); + jmodel.jointVelocityFromNvSelector(data.tau).noalias() -= + jmodel.jointJacCols(data.J).transpose() * (data.of[i].toVector()); data.of_augmented[i].toVector().setZero(); } @@ -1115,7 +1115,7 @@ namespace pinocchio const JointModel & jmodel = model.joints[i]; data.oa_augmented[i].toVector().noalias() = data.oa_augmented[model.parents[i]].toVector() - + jmodel.jointCols(data.J) * jmodel.jointVelocitySelector(data.u); + + jmodel.jointJacCols(data.J) * jmodel.jointVelocityFromNvSelector(data.u); data.of_augmented[i].toVector().setZero(); } } @@ -1158,8 +1158,8 @@ namespace pinocchio const JointModel & jmodel = model.joints[i]; data.of_augmented[parent] += data.of_augmented[i]; - jmodel.jointVelocitySelector(data.tau).noalias() = - -jmodel.jointCols(data.J).transpose() * (data.of_augmented[i].toVector()); + jmodel.jointVelocityFromNvSelector(data.tau).noalias() = + -jmodel.jointJacCols(data.J).transpose() * (data.of_augmented[i].toVector()); } } diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index a97fefa1bd..203b0a0468 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -9,6 +9,7 @@ #include "pinocchio/spatial/act-on-set.hpp" #include "pinocchio/algorithm/kinematics.hpp" #include "pinocchio/algorithm/check.hpp" +#include "pinocchio/algorithm/model.hpp" /// @cond DEV @@ -51,12 +52,70 @@ namespace pinocchio else data.oMi[i] = data.liMi[i]; - jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S()); + jmodel.jointJacCols(data.J) = data.oMi[i].act(jdata.S()); data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); } }; + /// \brief Patch to the crba algorithm for joint mimic (in local convention) + template< + typename JointModel, + typename Scalar, + int Options, + template class JointCollectionTpl> + static inline void mimic_patch_CrbaWorldConventionBackwardStep( + const JointModelBase &, + const ModelTpl &, + DataTpl &) + { + } + + /// \note For each joint the crba computation is done only for the sub cols/rows + /// from idx_v to joint.nvSubtree. In the case of the joint j=(i+n) mimicking the joint i, + /// the joints i+[1..n-1] will have idx_v greater than the joint j. This patch compute + /// this "left out" part of the M matrix. + template class JointCollectionTpl> + static inline void mimic_patch_CrbaWorldConventionBackwardStep( + const JointModelBase> & jmodel, + const ModelTpl & model, + DataTpl & data) + { + const JointIndex secondary_id = jmodel.id(); + const JointIndex primary_id = jmodel.derived().jmodel().id(); + + assert(secondary_id > primary_id && "Mimicking joint id is before the primary."); + + size_t ancestor_prim, ancestor_sec; + findCommonAncestor(model, primary_id, secondary_id, ancestor_prim, ancestor_sec); + + // Traverse the tree backward from parent of mimicking (secondary) joint to common ancestor + for (size_t k = model.supports[secondary_id].size() - 2; k >= ancestor_sec; k--) + { + const JointIndex i = model.supports[secondary_id].at(k); + + // Skip the common ancestor if it's not the primary id + // as this computation would be canceled by the next loop forward + if (k == ancestor_sec && i != primary_id) + continue; + + jmodel.jointVelRows(data.M) + .middleCols(model.joints[i].idx_v(), model.joints[i].nv()) + .noalias() += + data.Ag.middleCols(jmodel.idx_v(), jmodel.derived().jmodel().nv()).transpose() + * model.joints.at(i).jointJacCols(data.J); + } + // Traverse the kinematic tree forward from common ancestor to mimicked (primary) joint + for (size_t k = ancestor_prim + 1; k < model.supports[primary_id].size(); k++) + { + const JointIndex i = model.supports[primary_id].at(k); + jmodel.jointVelCols(data.M) + .middleRows(model.joints[i].idx_v(), model.joints[i].nv()) + .noalias() -= model.joints.at(i).jointJacCols(data.J).transpose() + * data.Ag.middleCols(jmodel.idx_v(), jmodel.derived().jmodel().nv()); + } + } + template class JointCollectionTpl> struct CrbaWorldConventionBackwardStep : public fusion::JointUnaryVisitorBase< @@ -77,14 +136,15 @@ namespace pinocchio const JointIndex & i = jmodel.id(); // Centroidal momentum map - ColsBlock Ag_cols = jmodel.jointCols(data.Ag); - ColsBlock J_cols = jmodel.jointCols(data.J); - motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); + ColsBlock Ag_cols = jmodel.jointVelCols(data.Ag); + ColsBlock J_cols = jmodel.jointJacCols(data.J); + motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); // Joint Space Inertia Matrix - data.M.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]).noalias() = + jmodel.jointVelRows(data.M).middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() += J_cols.transpose() * data.Ag.middleCols(jmodel.idx_v(), data.nvSubtree[i]); + mimic_patch_CrbaWorldConventionBackwardStep(jmodel, model, data); const JointIndex & parent = model.parents[i]; data.oYcrb[parent] += data.oYcrb[i]; } @@ -122,6 +182,106 @@ namespace pinocchio } }; + // /// \brief Patch to the crba algorithm for joint mimic (in local convention) + template< + typename JointModel, + typename Scalar, + int Options, + template class JointCollectionTpl> + static inline void mimic_patch_CrbaLocalConventionBackwardStep( + const JointModelBase &, + const ModelTpl &, + DataTpl &) + { + } + + template + struct GetMotionSubspaceTplNoMalloc : public boost::static_visitor + { + template + ReturnType operator()(const JointDataBase & jdata) const + { + assert(jdata.S().nv() <= ReturnType::MaxColsAtCompileTime); + return ReturnType(jdata.S().matrix()); + } + + static ReturnType run(const JointDataTpl & jdata) + { + return boost::apply_visitor(GetMotionSubspaceTplNoMalloc(), jdata); + } + }; + + /// \note For each joint the crba computation is done only for the sub cols/rows + /// from idx_v to joint.nvSubtree. In the case of the joint j=(i+n) mimicking the joint i, + /// the joints i+[1..n-1] will have idx_v greater than the joint j. This patch compute + /// this "left out" part of the M matrix. + template class JointCollectionTpl> + static inline void mimic_patch_CrbaLocalConventionBackwardStep( + const JointModelBase> & jmodel, + const ModelTpl & model, + DataTpl & data) + { + typedef JointModelMimicTpl JointModel; + typedef typename Eigen::Matrix< + Scalar, 6, Eigen::Dynamic, Data::Matrix6x::Options, 6, JointModel::MaxNJ> + SpatialForcesX; + typedef JointDataTpl JointData; + typedef typename Eigen::Matrix + MotionSubspace; + typedef GetMotionSubspaceTplNoMalloc GetSNoMalloc; + typedef SE3Tpl SE3; + + const JointIndex secondary_id = jmodel.id(); + const JointIndex primary_id = jmodel.derived().jmodel().id(); + + assert(secondary_id > primary_id && "Mimicking joint id is before the primary."); + + size_t ancestor_prim, ancestor_sec; + findCommonAncestor(model, primary_id, secondary_id, ancestor_prim, ancestor_sec); + + SpatialForcesX iF(6, jmodel.nj()); // Current joint forces + SE3 iMj = SE3::Identity(); // Transform from current joint to mimic joint + + // Traverse the tree backward from parent of mimicking (secondary) joint to common ancestor + for (size_t k = model.supports[secondary_id].size() - 2; k >= ancestor_sec; k--) + { + const JointIndex i = model.supports[secondary_id].at(k); + const JointIndex ui = + model.supports[secondary_id].at(k + 1); // Child link to compute placement + iMj = data.liMi[ui].act(iMj); + + // Skip the common ancestor if it's not the primary id + // as this computation would be canceled by the next loop forward + if (k == ancestor_sec && i != primary_id) + continue; + + forceSet::se3Action(iMj, jmodel.jointVelCols(data.Fcrb[secondary_id]), iF); + + jmodel.jointVelRows(data.M) + .middleCols(model.joints[i].idx_v(), model.joints[i].nv()) + .noalias() += GetSNoMalloc::run(data.joints[i]).transpose() * iF; + } + // Traverse the kinematic tree forward from common ancestor to mimicked (primary) joint + for (size_t k = ancestor_prim + 1; k < model.supports[primary_id].size(); k++) + { + const JointIndex i = model.supports[primary_id].at(k); + iMj = data.liMi[i].actInv(iMj); + + forceSet::se3Action(iMj, jmodel.jointVelCols(data.Fcrb[secondary_id]), iF); + + jmodel.jointVelCols(data.M) + .middleRows(model.joints[i].idx_v(), model.joints[i].nv()) + .noalias() -= iF.transpose() * GetSNoMalloc::run(data.joints[i]); + } + + if (model.parents[secondary_id] != primary_id) + { + forceSet::se3Action( + iMj, data.Fcrb[secondary_id].col(jmodel.idx_v()), + data.Fcrb[primary_id].col(jmodel.idx_v())); + } + } + template class JointCollectionTpl> struct CrbaLocalConventionBackwardStep : public fusion::JointUnaryVisitorBase< @@ -140,7 +300,7 @@ namespace pinocchio Data & data) { /* - * F[1:6,i] = Y*S + * F[1:6,i] += Y*S * M[i,SUBTREE] = S'*F[1:6,SUBTREE] * if li>0 * Yli += liXi Yi @@ -153,10 +313,10 @@ namespace pinocchio /* F[1:6,i] = Y*S */ // data.Fcrb[i].block<6,JointModel::NV>(0,jmodel.idx_v()) = data.Ycrb[i] * jdata.S(); - jmodel.jointCols(data.Fcrb[i]) = data.Ycrb[i] * jdata.S(); + jmodel.jointVelCols(data.Fcrb[i]) += data.Ycrb[i] * jdata.S(); /* M[i,SUBTREE] = S'*F[1:6,SUBTREE] */ - data.M.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]).noalias() = + jmodel.jointVelRows(data.M).middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() += jdata.S().transpose() * data.Fcrb[i].middleCols(jmodel.idx_v(), data.nvSubtree[i]); const JointIndex & parent = model.parents[i]; @@ -168,8 +328,10 @@ namespace pinocchio /* F[1:6,SUBTREE] = liXi F[1:6,SUBTREE] */ Block jF = data.Fcrb[parent].middleCols(jmodel.idx_v(), data.nvSubtree[i]); Block iF = data.Fcrb[i].middleCols(jmodel.idx_v(), data.nvSubtree[i]); - forceSet::se3Action(data.liMi[i], iF, jF); + forceSet::se3Action(data.liMi[i], iF, jF); } + + mimic_patch_CrbaLocalConventionBackwardStep(jmodel, model, data); } }; @@ -197,6 +359,11 @@ namespace pinocchio model.joints[i], data.joints[i], typename Pass1::ArgsType(model, data, q.derived())); } + data.M.setZero(); + for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) + { + data.Fcrb[i].setZero(); + } typedef CrbaLocalConventionBackwardStep Pass2; for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) { @@ -234,6 +401,8 @@ namespace pinocchio model.joints[i], data.joints[i], typename Pass1::ArgsType(model, data, q.derived())); } + data.M.setZero(); + data.Ag.setZero(); typedef CrbaWorldConventionBackwardStep Pass2; for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) { diff --git a/include/pinocchio/algorithm/delassus.hxx b/include/pinocchio/algorithm/delassus.hxx index 61c1efb033..5b07052aec 100644 --- a/include/pinocchio/algorithm/delassus.hxx +++ b/include/pinocchio/algorithm/delassus.hxx @@ -48,7 +48,7 @@ namespace pinocchio else data.oMi[i] = data.liMi[i]; - jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S()); + jmodel.jointJacCols(data.J) = data.oMi[i].act(jdata.S()); data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); data.oYaba[i] = data.oYcrb[i].matrix(); } @@ -81,13 +81,13 @@ namespace pinocchio typename Inertia::Matrix6 & Ia = data.oYaba[i]; typedef typename SizeDepType::template ColsReturn::Type ColBlock; - ColBlock Jcols = jmodel.jointCols(data.J); + ColBlock Jcols = jmodel.jointJacCols(data.J); jdata.U().noalias() = Ia * Jcols; jdata.StU().noalias() = Jcols.transpose() * jdata.U(); // Account for the rotor inertia contribution - jdata.StU().diagonal() += jmodel.jointVelocitySelector(model.armature); + jdata.StU().diagonal() += jmodel.jointVelocityFromNvSelector(model.armature); internal::PerformStYSInversion::run(jdata.StU(), jdata.Dinv()); @@ -437,13 +437,13 @@ namespace pinocchio typename Inertia::Matrix6 & Ia = data.oYaba[i]; typedef typename SizeDepType::template ColsReturn::Type ColBlock; - ColBlock Jcols = jmodel.jointCols(data.J); + ColBlock Jcols = jmodel.jointJacCols(data.J); jdata.U().noalias() = Ia * Jcols; jdata.StU().noalias() = Jcols.transpose() * jdata.U(); // Account for the rotor inertia contribution - jdata.StU().diagonal() += jmodel.jointVelocitySelector(model.armature); + jdata.StU().diagonal() += jmodel.jointVelocityFromNvSelector(model.armature); internal::PerformStYSInversion::run(jdata.StU(), jdata.Dinv()); @@ -588,7 +588,7 @@ namespace pinocchio { // When propagating 6D constraints scratch_pad_vector.noalias() = - data.extended_motion_propagator[i] * model.joints[i].jointCols(data.J); + data.extended_motion_propagator[i] * model.joints[i].jointJacCols(data.J); scratch_pad_vector2.noalias() = scratch_pad_vector * data.joints[i].Dinv().coeff(0, 0); data.spatial_inv_inertia[ad_i].noalias() += scratch_pad_vector2 * scratch_pad_vector.transpose(); @@ -598,7 +598,7 @@ namespace pinocchio // Propagating 3D constraints scratch_pad_vector.template topRows<3>().noalias() = data.extended_motion_propagator[i].template topRows<3>() - * model.joints[i].jointCols(data.J); + * model.joints[i].jointJacCols(data.J); scratch_pad_vector2.template topRows<3>().noalias() = scratch_pad_vector.template topRows<3>() * data.joints[i].Dinv().coeff(0, 0); data.spatial_inv_inertia[ad_i].template topLeftCorner<3, 3>().noalias() += @@ -609,7 +609,7 @@ namespace pinocchio else if (nv == 6) { scratch_pad1.noalias() = - data.extended_motion_propagator[i] * model.joints[i].jointCols(data.J); + data.extended_motion_propagator[i] * model.joints[i].jointJacCols(data.J); scratch_pad2.noalias() = scratch_pad1 * data.joints[i].Dinv(); data.spatial_inv_inertia[ad_i].noalias() += scratch_pad2 * scratch_pad1.transpose(); } @@ -617,7 +617,7 @@ namespace pinocchio { // Joints with more than 1 DoF scratch_pad1.leftCols(nv).noalias() = - data.extended_motion_propagator[i] * model.joints[i].jointCols(data.J); + data.extended_motion_propagator[i] * model.joints[i].jointJacCols(data.J); scratch_pad2.leftCols(nv).noalias() = scratch_pad1.leftCols(nv) * data.joints[i].Dinv(); data.spatial_inv_inertia[ad_i].noalias() += scratch_pad2.leftCols(nv) * scratch_pad1.leftCols(nv).transpose(); diff --git a/include/pinocchio/algorithm/energy.hxx b/include/pinocchio/algorithm/energy.hxx index a0bd50208a..b321be13ca 100644 --- a/include/pinocchio/algorithm/energy.hxx +++ b/include/pinocchio/algorithm/energy.hxx @@ -29,9 +29,9 @@ namespace pinocchio { const JointIndex & i = jmodel.id(); data.kinetic_energy += model.inertias[i].vtiv(data.v[i]); - data.kinetic_energy += - (jmodel.jointVelocitySelector(model.armature).array() * jdata.joint_v().array().square()) - .sum(); + data.kinetic_energy += (jmodel.jointVelocityFromDofSelector(model.armature).array() + * jdata.joint_v().array().square()) + .sum(); } }; diff --git a/include/pinocchio/algorithm/geometry.hxx b/include/pinocchio/algorithm/geometry.hxx index 89093d8ea7..232c57aa6b 100644 --- a/include/pinocchio/algorithm/geometry.hxx +++ b/include/pinocchio/algorithm/geometry.hxx @@ -45,7 +45,9 @@ namespace pinocchio { const Model::JointIndex joint_id = geom_model.geometryObjects[i].parentJoint; if (joint_id > 0) - geom_data.oMg[i] = (data.oMi[joint_id] * geom_model.geometryObjects[i].placement); + geom_data.oMg[i] = + (static_cast(data.oMi[joint_id]) + * geom_model.geometryObjects[i].placement); else geom_data.oMg[i] = geom_model.geometryObjects[i].placement; } diff --git a/include/pinocchio/algorithm/impulse-dynamics-derivatives.hxx b/include/pinocchio/algorithm/impulse-dynamics-derivatives.hxx index e94b59830b..18cbe4b9d3 100644 --- a/include/pinocchio/algorithm/impulse-dynamics-derivatives.hxx +++ b/include/pinocchio/algorithm/impulse-dynamics-derivatives.hxx @@ -66,7 +66,7 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock Jcols = jmodel.jointCols(data.J); + ColsBlock Jcols = jmodel.jointJacCols(data.J); typedef typename SizeDepType::template ColsReturn::Type ColsBlockOut1; @@ -78,13 +78,13 @@ namespace pinocchio // dvec/dv const int nv = jmodel.nv(); Eigen::Matrix v_spatial_partial_dv_cols(6, nv); - ColsBlockOut2 v_partial_dv_cols = jmodel.jointCols(v_partial_dv_); + ColsBlockOut2 v_partial_dv_cols = jmodel.jointVelCols(v_partial_dv_); motionSet::se3ActionInverse(oMlast, Jcols, v_spatial_partial_dv_cols); v_partial_dv_cols = v_spatial_partial_dv_cols.template middleRows<3>(Motion::LINEAR); // dvec/dq - ColsBlockOut1 v_partial_dq_cols = jmodel.jointCols(v_partial_dq_); + ColsBlockOut1 v_partial_dq_cols = jmodel.jointVelCols(v_partial_dq_); #define FOR_NV() for (Eigen::DenseIndex j = 0; j < nv; ++j) #define GET_LINEAR(vec6) vec6.template segment<3>(Motion::LINEAR) @@ -175,7 +175,7 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock Jcols = jmodel.jointCols(data.J); + ColsBlock Jcols = jmodel.jointJacCols(data.J); typedef typename SizeDepType::template ColsReturn::Type ColsBlockOut1; @@ -185,7 +185,7 @@ namespace pinocchio Matrix6xOut2 & v_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2, v_partial_dv); // dvec/dv - ColsBlockOut2 v_partial_dv_cols = jmodel.jointCols(v_partial_dv_); + ColsBlockOut2 v_partial_dv_cols = jmodel.jointVelCols(v_partial_dv_); switch (rf) { @@ -200,7 +200,7 @@ namespace pinocchio } // dvec/dq - ColsBlockOut1 v_partial_dq_cols = jmodel.jointCols(v_partial_dq_); + ColsBlockOut1 v_partial_dq_cols = jmodel.jointVelCols(v_partial_dq_); const Scalar factor = Scalar(1) + r_coeff; switch (rf) { diff --git a/include/pinocchio/algorithm/jacobian.hxx b/include/pinocchio/algorithm/jacobian.hxx index 8f91d2a57e..d86e73488e 100644 --- a/include/pinocchio/algorithm/jacobian.hxx +++ b/include/pinocchio/algorithm/jacobian.hxx @@ -57,7 +57,7 @@ namespace pinocchio data.oMi[i] = data.liMi[i]; Matrix6xLike & J_ = J.const_cast_derived(); - jmodel.jointCols(J_) = data.oMi[i].act(jdata.S()); + jmodel.jointJacCols(J_) = data.oMi[i].act(jdata.S()); } }; @@ -113,7 +113,7 @@ namespace pinocchio typedef typename Model::JointIndex JointIndex; const JointIndex & i = jmodel.id(); - jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S()); + jmodel.jointJacCols(data.J) = data.oMi[i].act(jdata.S()); } }; } // namespace impl @@ -149,6 +149,7 @@ namespace pinocchio PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.rows(), 6); Matrix6xLikeOut & Jout_ = Jout.const_cast_derived(); + Jout_.setZero(); typedef typename Matrix6xLikeIn::ConstColXpr ConstColXprIn; typedef const MotionRef MotionIn; @@ -184,12 +185,13 @@ namespace pinocchio assert(model.check(data) && "data is not consistent with model."); PINOCCHIO_CHECK_ARGUMENT_SIZE(Jin.rows(), 6); - PINOCCHIO_CHECK_ARGUMENT_SIZE(Jin.cols(), model.nv); + PINOCCHIO_CHECK_ARGUMENT_SIZE(Jin.cols(), model.nj); PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.rows(), 6); PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.cols(), model.nv); Matrix6xLikeOut & Jout_ = Jout.const_cast_derived(); + Jout_.setZero(); typedef typename Matrix6xLikeIn::ConstColXpr ConstColXprIn; typedef const MotionRef MotionIn; @@ -197,37 +199,55 @@ namespace pinocchio typedef typename Matrix6xLikeOut::ColXpr ColXprOut; typedef MotionRef MotionOut; - const int colRef = nv(model.joints[joint_id]) + idx_v(model.joints[joint_id]) - 1; + const int colRefIn = nj(model.joints[joint_id]) + idx_j(model.joints[joint_id]) - 1; switch (rf) { case WORLD: { - for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j]) + for (JointIndex j = joint_id; j > 0; j = model.parents[(size_t)j]) { - MotionIn v_in(Jin.col(j)); - MotionOut v_out(Jout_.col(j)); + for (int i = nj(model.joints[j]) - 1; i >= 0; i--) + { + const Eigen::DenseIndex col_in = idx_j(model.joints[j]) + i; + const Eigen::DenseIndex col_out = idx_v(model.joints[j]) + i; - v_out = v_in; + MotionIn v_in(Jin.col(col_in)); + MotionOut v_out(Jout_.col(col_out)); + + v_out += v_in; + } } break; } case LOCAL_WORLD_ALIGNED: { - for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j]) + for (JointIndex j = joint_id; j > 0; j = model.parents[(size_t)j]) { - MotionIn v_in(Jin.col(j)); - MotionOut v_out(Jout_.col(j)); + for (int i = nj(model.joints[j]) - 1; i >= 0; i--) + { + const Eigen::DenseIndex col_in = idx_j(model.joints[j]) + i; + const Eigen::DenseIndex col_out = idx_v(model.joints[j]) + i; + + MotionIn v_in(Jin.col(col_in)); + MotionOut v_out(Jout_.col(col_out)); - v_out = v_in; - v_out.linear() -= placement.translation().cross(v_in.angular()); + v_out += v_in; + v_out.linear() -= placement.translation().cross(v_in.angular()); + } } break; } case LOCAL: { - for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j]) + for (JointIndex j = joint_id; j > 0; j = model.parents[(size_t)j]) { - MotionIn v_in(Jin.col(j)); - MotionOut v_out(Jout_.col(j)); + for (int i = nj(model.joints[j]) - 1; i >= 0; i--) + { + const Eigen::DenseIndex col_in = idx_j(model.joints[j]) + i; + const Eigen::DenseIndex col_out = idx_v(model.joints[j]) + i; + + MotionIn v_in(Jin.col(col_in)); + MotionOut v_out(Jout_.col(col_out)); - v_out = placement.actInv(v_in); + v_out += placement.actInv(v_in); + } } break; } @@ -319,7 +339,7 @@ namespace pinocchio data.iMf[parent] = data.liMi[i] * data.iMf[i]; Matrix6xLike & J_ = J.const_cast_derived(); - jmodel.jointCols(J_) = data.iMf[i].actInv(jdata.S()); + jmodel.jointVelCols(J_) += data.iMf[i].actInv(jdata.S()); } }; @@ -344,6 +364,8 @@ namespace pinocchio typedef typename Model::JointIndex JointIndex; data.iMf[jointId].setIdentity(); + Matrix6xLike & J_ = J.const_cast_derived(); + J_.setZero(); typedef JointJacobianForwardStep< Scalar, Options, JointCollectionTpl, ConfigVectorType, Matrix6xLike> Pass; @@ -351,7 +373,7 @@ namespace pinocchio { Pass::run( model.joints[i], data.joints[i], - typename Pass::ArgsType(model, data, q.derived(), J.const_cast_derived())); + typename Pass::ArgsType(model, data, q.derived(), J_.const_cast_derived())); } } @@ -410,7 +432,7 @@ namespace pinocchio oMi = data.liMi[i]; } - jmodel.jointCols(data.J) = oMi.act(jdata.S()); + jmodel.jointJacCols(data.J) = oMi.act(jdata.S()); // Spatial velocity of joint i expressed in the global frame o data.ov[i] = oMi.act(vJ); @@ -418,8 +440,8 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock dJcols = jmodel.jointCols(data.dJ); - ColsBlock Jcols = jmodel.jointCols(data.J); + ColsBlock dJcols = jmodel.jointJacCols(data.dJ); + ColsBlock Jcols = jmodel.jointJacCols(data.J); motionSet::motionAction(data.ov[i], Jcols, dJcols); } @@ -481,6 +503,7 @@ namespace pinocchio && "jointId is larger than the number of joints contained in the model"); Matrix6xLike & dJ = dJ_.const_cast_derived(); + dJ.setZero(); ::pinocchio::details::translateJointJacobian(model, data, jointId, rf, data.dJ, dJ); // Add contribution for LOCAL and LOCAL_WORLD_ALIGNED @@ -489,38 +512,48 @@ namespace pinocchio case LOCAL: { const SE3 & oMjoint = data.oMi[jointId]; const Motion & v_joint = data.v[jointId]; - const int colRef = nv(model.joints[jointId]) + idx_v(model.joints[jointId]) - 1; - for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j]) + for (JointIndex j = jointId; j > 0; j = model.parents[(size_t)j]) { - typedef typename Data::Matrix6x::ConstColXpr ConstColXprIn; - typedef const MotionRef MotionIn; + for (int i = nj(model.joints[j]) - 1; i >= 0; i--) + { + const Eigen::DenseIndex col_in = idx_j(model.joints[j]) + i; + const Eigen::DenseIndex col_out = idx_v(model.joints[j]) + i; - typedef typename Matrix6xLike::ColXpr ColXprOut; - typedef MotionRef MotionOut; - MotionIn v_in(data.J.col(j)); - MotionOut v_out(dJ.col(j)); + typedef typename Data::Matrix6x::ConstColXpr ConstColXprIn; + typedef const MotionRef MotionIn; - v_out -= v_joint.cross(oMjoint.actInv(v_in)); + typedef typename Matrix6xLike::ColXpr ColXprOut; + typedef MotionRef MotionOut; + MotionIn v_in(data.J.col(col_in)); + MotionOut v_out(dJ.col(col_out)); + + v_out -= v_joint.cross(oMjoint.actInv(v_in)); + } } break; } case LOCAL_WORLD_ALIGNED: { const Motion & ov_joint = data.ov[jointId]; const SE3 & oMjoint = data.oMi[jointId]; - const int colRef = nv(model.joints[jointId]) + idx_v(model.joints[jointId]) - 1; - for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j]) + for (JointIndex j = jointId; j > 0; j = model.parents[(size_t)j]) { - typedef typename Data::Matrix6x::ConstColXpr ConstColXprIn; - typedef const MotionRef MotionIn; - - typedef typename Matrix6xLike::ColXpr ColXprOut; - typedef MotionRef MotionOut; - MotionIn v_in(data.J.col(j)); - MotionOut v_out(dJ.col(j)); - - v_out.linear() -= - Vector3(ov_joint.linear() + ov_joint.angular().cross(oMjoint.translation())) - .cross(v_in.angular()); + for (int i = nj(model.joints[j]) - 1; i >= 0; i--) + { + const Eigen::DenseIndex col_in = idx_j(model.joints[j]) + i; + const Eigen::DenseIndex col_out = idx_v(model.joints[j]) + i; + + typedef typename Data::Matrix6x::ConstColXpr ConstColXprIn; + typedef const MotionRef MotionIn; + + typedef typename Matrix6xLike::ColXpr ColXprOut; + typedef MotionRef MotionOut; + MotionIn v_in(data.J.col(col_in)); + MotionOut v_out(dJ.col(col_out)); + + v_out.linear() -= + Vector3(ov_joint.linear() + ov_joint.angular().cross(oMjoint.translation())) + .cross(v_in.angular()); + } } break; } diff --git a/include/pinocchio/algorithm/kinematics-derivatives.hxx b/include/pinocchio/algorithm/kinematics-derivatives.hxx index 0e0e171bbe..a44324b3ae 100644 --- a/include/pinocchio/algorithm/kinematics-derivatives.hxx +++ b/include/pinocchio/algorithm/kinematics-derivatives.hxx @@ -76,15 +76,15 @@ namespace pinocchio if (parent > 0) vi += data.liMi[i].actInv(data.v[parent]); - ai = jdata.S() * jmodel.jointVelocitySelector(a) + jdata.c() + (vi ^ jdata.v()); + ai = jdata.S() * jmodel.jointVelocityFromDofSelector(a) + jdata.c() + (vi ^ jdata.v()); if (parent > 0) ai += data.liMi[i].actInv(data.a[parent]); typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock dJcols = jmodel.jointCols(data.dJ); - ColsBlock Jcols = jmodel.jointCols(data.J); + ColsBlock dJcols = jmodel.jointJacCols(data.dJ); + ColsBlock Jcols = jmodel.jointJacCols(data.J); Jcols = oMi.act(jdata.S()); ov = oMi.act(vi); // Spatial velocity of joint i expressed in the global frame o @@ -181,7 +181,7 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn< typename Data::Matrix6x>::ConstType ColsBlock; - ColsBlock Jcols = jmodel.jointCols(data.J); + ColsBlock Jcols = jmodel.jointJacCols(data.J); typedef typename SizeDepType::template ColsReturn::Type ColsBlockOut1; @@ -191,7 +191,7 @@ namespace pinocchio Matrix6xOut2 & v_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2, v_partial_dv); // dvec/dv: this result is then needed by dvec/dq - ColsBlockOut2 v_partial_dv_cols = jmodel.jointCols(v_partial_dv_); + ColsBlockOut2 v_partial_dv_cols = jmodel.jointVelCols(v_partial_dv_); switch (rf) { case WORLD: @@ -208,7 +208,7 @@ namespace pinocchio } // dvec/dq - ColsBlockOut1 v_partial_dq_cols = jmodel.jointCols(v_partial_dq_); + ColsBlockOut1 v_partial_dq_cols = jmodel.jointVelCols(v_partial_dq_); switch (rf) { case WORLD: @@ -337,8 +337,8 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn< typename Data::Matrix6x>::ConstType ColsBlock; - ColsBlock dJcols = jmodel.jointCols(data.dJ); - ColsBlock Jcols = jmodel.jointCols(data.J); + ColsBlock dJcols = jmodel.jointJacCols(data.dJ); + ColsBlock Jcols = jmodel.jointJacCols(data.J); typedef typename SizeDepType::template ColsReturn::Type ColsBlockOut1; @@ -353,10 +353,10 @@ namespace pinocchio ColsBlockOut4; Matrix6xOut4 & a_partial_da_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4, a_partial_da); - ColsBlockOut1 v_partial_dq_cols = jmodel.jointCols(v_partial_dq_); - ColsBlockOut2 a_partial_dq_cols = jmodel.jointCols(a_partial_dq_); - ColsBlockOut3 a_partial_dv_cols = jmodel.jointCols(a_partial_dv_); - ColsBlockOut4 a_partial_da_cols = jmodel.jointCols(a_partial_da_); + ColsBlockOut1 v_partial_dq_cols = jmodel.jointVelCols(v_partial_dq_); + ColsBlockOut2 a_partial_dq_cols = jmodel.jointVelCols(a_partial_dq_); + ColsBlockOut3 a_partial_dv_cols = jmodel.jointVelCols(a_partial_dv_); + ColsBlockOut4 a_partial_da_cols = jmodel.jointVelCols(a_partial_da_); // dacc/da switch (rf) @@ -588,7 +588,7 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn< typename Data::Matrix6x>::ConstType ColsBlock; - ColsBlock Jcols = jmodel.jointCols(data.J); + ColsBlock Jcols = jmodel.jointJacCols(data.J); typedef typename SizeDepType::template ColsReturn::Type ColsBlockOut1; @@ -597,8 +597,8 @@ namespace pinocchio ColsBlockOut2; Matrix3xOut2 & v_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut2, v_partial_dv); - ColsBlockOut1 v_partial_dq_cols = jmodel.jointCols(v_partial_dq_); - ColsBlockOut2 v_partial_dv_cols = jmodel.jointCols(v_partial_dv_); + ColsBlockOut1 v_partial_dq_cols = jmodel.jointVelCols(v_partial_dq_); + ColsBlockOut2 v_partial_dv_cols = jmodel.jointVelCols(v_partial_dv_); const int nv = jmodel.nv(); Eigen::Matrix v_spatial_partial_dv_cols(6, nv); @@ -751,8 +751,8 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn< typename Data::Matrix6x>::ConstType ColsBlock; - ColsBlock dJcols = jmodel.jointCols(data.dJ); - ColsBlock Jcols = jmodel.jointCols(data.J); + ColsBlock dJcols = jmodel.jointJacCols(data.dJ); + ColsBlock Jcols = jmodel.jointJacCols(data.J); typedef typename SizeDepType::template ColsReturn::Type ColsBlockOut1; @@ -767,10 +767,10 @@ namespace pinocchio ColsBlockOut4; Matrix3xOut4 & a_partial_da_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut4, a_partial_da); - ColsBlockOut1 v_partial_dq_cols = jmodel.jointCols(v_partial_dq_); - ColsBlockOut2 a_partial_dq_cols = jmodel.jointCols(a_partial_dq_); - ColsBlockOut3 a_partial_dv_cols = jmodel.jointCols(a_partial_dv_); - ColsBlockOut4 a_partial_da_cols = jmodel.jointCols(a_partial_da_); + ColsBlockOut1 v_partial_dq_cols = jmodel.jointVelCols(v_partial_dq_); + ColsBlockOut2 a_partial_dq_cols = jmodel.jointVelCols(a_partial_dq_); + ColsBlockOut3 a_partial_dv_cols = jmodel.jointVelCols(a_partial_dv_); + ColsBlockOut4 a_partial_da_cols = jmodel.jointVelCols(a_partial_da_); const int nv = jmodel.nv(); Eigen::Matrix a_spatial_partial_da_cols(6, nv); diff --git a/include/pinocchio/algorithm/kinematics.hpp b/include/pinocchio/algorithm/kinematics.hpp index cc0bfe8ebe..bc2f856230 100644 --- a/include/pinocchio/algorithm/kinematics.hpp +++ b/include/pinocchio/algorithm/kinematics.hpp @@ -101,6 +101,30 @@ namespace pinocchio const Eigen::MatrixBase & v, const Eigen::MatrixBase & a); + /** + * @brief Returns the relative placement of two joints expressed in the desired reference + * frame. You must first call pinocchio::forwardKinematics to update placement values in data + * structure. + * + * @param[in] model The kinematic model + * @param[in] data Data associated to model + * @param[in] jointId Id of the reference joint + * @param[in] jointId Id of the target joint + * @param[in] convention Convention to use (compuputation is done using data.liMi if LOCAL, and + * data.oMi if WORLD). + * + * @return The relative placement of the target joint wrt to the refence joint, expressed in + * the desired reference frame. + * + */ + template class JointCollectionTpl> + SE3Tpl getRelativePlacement( + const ModelTpl & model, + const DataTpl & data, + const JointIndex jointIdRef, + const JointIndex jointIdTarget, + const Convention convention = Convention::LOCAL); + /** * @brief Returns the spatial velocity of the joint expressed in the desired reference frame. * You must first call pinocchio::forwardKinematics to update placement and velocity diff --git a/include/pinocchio/algorithm/kinematics.hxx b/include/pinocchio/algorithm/kinematics.hxx index fc7230cc6f..925d52ab80 100644 --- a/include/pinocchio/algorithm/kinematics.hxx +++ b/include/pinocchio/algorithm/kinematics.hxx @@ -5,6 +5,7 @@ #ifndef __pinocchio_kinematics_hxx__ #define __pinocchio_kinematics_hxx__ +#include "model.hpp" #include "pinocchio/multibody/visitor.hpp" #include "pinocchio/algorithm/check.hpp" @@ -233,7 +234,7 @@ namespace pinocchio data.oMi[i] = data.liMi[i]; data.a[i] = - jdata.S() * jmodel.jointVelocitySelector(a) + jdata.c() + (data.v[i] ^ jdata.v()); + jdata.S() * jmodel.jointVelocityFromDofSelector(a) + jdata.c() + (data.v[i] ^ jdata.v()); data.a[i] += data.liMi[i].actInv(data.a[parent]); } }; @@ -276,6 +277,57 @@ namespace pinocchio } } } // namespace impl + + template class JointCollectionTpl> + SE3Tpl getRelativePlacement( + const ModelTpl & model, + const DataTpl & data, + const JointIndex jointIdRef, + const JointIndex jointIdTarget, + const Convention convention) + { + assert(model.check(data) && "data is not consistent with model."); + assert(jointIdRef >= 0 && jointIdRef < (JointIndex)model.njoints && "invalid joint id"); + assert(jointIdTarget >= 0 && jointIdTarget < (JointIndex)model.njoints && "invalid joint id"); + switch (convention) + { + case Convention::LOCAL: { + SE3Tpl result; + const JointIndex child_id = std::max(jointIdRef, jointIdTarget); + const JointIndex parent_id = std::min(jointIdRef, jointIdTarget); + result.setIdentity(); + + // Traverse the kinematic chain from "tip" to "root" + size_t common_ancestor_parent, common_ancestor_child; + findCommonAncestor(model, parent_id, child_id, common_ancestor_parent, common_ancestor_child); + + // Traverse the kinematic chain backward to the common ancestor + for (size_t i = model.supports[child_id].size() - 1; i > common_ancestor_child; i--) + { + const JointIndex j = model.supports[child_id][(size_t)i]; + result = data.liMi[j].act(result); + } + // Then forward to the "parent" + for (size_t i = common_ancestor_parent + 1; i <= model.supports[parent_id].size() - 1; i++) + { + const JointIndex j = model.supports[parent_id][i]; + result = data.liMi[j].actInv(result); + } + + // Inverse the result if necessary + if (child_id == jointIdRef && jointIdRef != jointIdTarget) + { + result = result.inverse(); + } + return result; + } + case Convention::WORLD: + return data.oMi[jointIdRef].actInv(data.oMi[jointIdTarget]); + default: + throw std::invalid_argument("Bad convention."); + } + } + template class JointCollectionTpl> MotionTpl getVelocity( const ModelTpl & model, diff --git a/include/pinocchio/algorithm/kinematics.txx b/include/pinocchio/algorithm/kinematics.txx index 70bbf95e54..2e78f792fd 100644 --- a/include/pinocchio/algorithm/kinematics.txx +++ b/include/pinocchio/algorithm/kinematics.txx @@ -45,6 +45,16 @@ namespace pinocchio const Eigen::MatrixBase> &, const Eigen::MatrixBase> &); } // namespace impl + + extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI + SE3Tpl + getRelativePlacement( + const context::Model &, + const context::Data &, + const JointIndex, + const JointIndex, + const Convention); + extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI MotionTpl getVelocity( diff --git a/include/pinocchio/algorithm/model.hpp b/include/pinocchio/algorithm/model.hpp index 697e7a5ad2..a6f593578c 100644 --- a/include/pinocchio/algorithm/model.hpp +++ b/include/pinocchio/algorithm/model.hpp @@ -203,6 +203,28 @@ namespace pinocchio ModelTpl & reduced_model, std::vector & list_of_reduced_geom_models); + /** + * + * \brief Transform of a joint of the model into a mimic joint. Keep the type of the joint as it + * was previously. + * + * \param[in] model the input model to take joints from. + * \param[in] index_primary index of the joint to mimic + * \param[in] index_secondary index of the joint that will mimic + * \param[in] scaling Scaling of joint velocity and configuration + * \param[in] offset Offset of joint configuration + * \param[out] output_model Model with the joint mimic + * + */ + template class JointCollectionTpl> + void transformJointIntoMimic( + const ModelTpl & input_model, + const JointIndex & index_primary, + const JointIndex & index_secondary, + const Scalar & scaling, + const Scalar & offset, + ModelTpl & output_model); + /** * * \brief Computes the common ancestor between two joints belonging to the same kinematic tree. @@ -210,8 +232,8 @@ namespace pinocchio * \param[in] model the input model. * \param[in] joint1_id index of the first joint. * \param[in] joint2_id index of the second joint. - * \param[out] index_ancestor_in_support1 index of the ancestor within model.support[joint1_id]. - * \param[out] index_ancestor_in_support2 index of the ancestor within model.support[joint2_id]. + * \param[out] index_ancestor_in_support1 index of the ancestor within model.supports[joint1_id]. + * \param[out] index_ancestor_in_support2 index of the ancestor within model.supports[joint2_id]. * */ template class JointCollectionTpl> diff --git a/include/pinocchio/algorithm/model.hxx b/include/pinocchio/algorithm/model.hxx index 472f9e5009..d904c763a7 100644 --- a/include/pinocchio/algorithm/model.hxx +++ b/include/pinocchio/algorithm/model.hxx @@ -129,7 +129,7 @@ namespace pinocchio { go.parentFrame = parentFrame; } - go.placement = pframe.placement * pfMAB * go.placement; + go.placement = static_cast(pframe.placement * pfMAB) * go.placement; geomModel.addGeometryObject(go); } } @@ -153,6 +153,36 @@ namespace pinocchio GeometryModel &> ArgsType; + template + static typename std::enable_if< + !std::is_same>::value, + JointModel>::type + updateMimicIds( + const JointModel & jmodel, const Model & /*old_model*/, const Model & /*new_model*/) + { + JointModel res(jmodel); + return res; + } + + template + static typename std::enable_if< + std::is_same>::value, + JointModel>::type + updateMimicIds( + const JointModelMimicTpl & jmodel, + const Model & old_model, + const Model & new_model) + { + JointModel res(jmodel); + const JointIndex mimicked_old_id = res.jmodel().id(); + const std::string mimicked_name = old_model.names[mimicked_old_id]; + const JointIndex mimicked_new_id = new_model.getJointId(mimicked_name); + res.setMimicIndexes( + mimicked_new_id, new_model.joints[mimicked_new_id].idx_q(), + new_model.joints[mimicked_new_id].idx_v(), new_model.joints[mimicked_new_id].idx_j()); + return res; + } + template static void algo( const JointModelBase & jmodel_in, @@ -173,23 +203,31 @@ namespace pinocchio !model.existJointName(modelAB.names[joint_id_in]), "The two models have conflicting joint names."); + // For mimic joints, update the reference joint id + JointModel jmodel_inter = updateMimicIds(jmodel_in.derived(), modelAB, model); + JointIndex joint_id_out = model.addJoint( - parent_id, jmodel_in, pMi * modelAB.jointPlacements[joint_id_in], - modelAB.names[joint_id_in], jmodel_in.jointVelocitySelector(modelAB.effortLimit), - jmodel_in.jointVelocitySelector(modelAB.velocityLimit), - jmodel_in.jointConfigSelector(modelAB.lowerPositionLimit), - jmodel_in.jointConfigSelector(modelAB.upperPositionLimit), - jmodel_in.jointVelocitySelector(modelAB.friction), - jmodel_in.jointVelocitySelector(modelAB.damping)); + parent_id, + jmodel_inter, // Use the intermediate joint (jmodel_inter) with updates id, idx, ... + pMi * modelAB.jointPlacements[joint_id_in], modelAB.names[joint_id_in], + jmodel_in.jointVelocityFromNvSelector( + modelAB + .effortLimit), // Need to select the vector base on the origin idxs, so use jmodel_in. + jmodel_in.jointVelocityFromNvSelector(modelAB.velocityLimit), + jmodel_in.jointConfigFromNqSelector(modelAB.lowerPositionLimit), + jmodel_in.jointConfigFromNqSelector(modelAB.upperPositionLimit), + jmodel_in.jointVelocityFromNvSelector(modelAB.friction), + jmodel_in.jointVelocityFromNvSelector(modelAB.damping)); assert(joint_id_out < model.joints.size()); model.appendBodyToJoint(joint_id_out, modelAB.inertias[joint_id_in]); - const typename Model::JointModel & jmodel_out = model.joints[joint_id_out]; - jmodel_out.jointVelocitySelector(model.rotorInertia) = - jmodel_in.jointVelocitySelector(modelAB.rotorInertia); - jmodel_out.jointVelocitySelector(model.rotorGearRatio) = - jmodel_in.jointVelocitySelector(modelAB.rotorGearRatio); + typename Model::JointModel & jmodel_out = model.joints[joint_id_out]; + + jmodel_out.jointVelocityFromNvSelector(model.rotorInertia) = + jmodel_in.jointVelocityFromNvSelector(modelAB.rotorInertia); + jmodel_out.jointVelocityFromNvSelector(model.rotorGearRatio) = + jmodel_in.jointVelocityFromNvSelector(modelAB.rotorGearRatio); // Add all frames whose parent is this joint. for (FrameIndex fid = 1; fid < modelAB.frames.size(); ++fid) @@ -383,8 +421,8 @@ namespace pinocchio const typename Model::JointModel & joint_model = model.joints[joint_id]; const typename Model::JointModel & joint_modelA = modelA.joints[joint_idA]; - joint_model.jointConfigSelector(config_vector) = - joint_modelA.jointConfigSelector(config_vectorA); + joint_model.jointConfigFromNqSelector(config_vector) = + joint_modelA.jointConfigFromNqSelector(config_vectorA); } model.referenceConfigurations.insert(std::make_pair(config_name, config_vector)); @@ -412,8 +450,8 @@ namespace pinocchio const typename Model::JointModel & joint_model = model.joints[joint_id]; const typename Model::JointModel & joint_modelB = modelB.joints[joint_idB]; - joint_model.jointConfigSelector(config_vector) = - joint_modelB.jointConfigSelector(config_vectorB); + joint_model.jointConfigFromNqSelector(config_vector) = + joint_modelB.jointConfigFromNqSelector(config_vectorB); } } @@ -605,22 +643,22 @@ namespace pinocchio JointIndex reduced_joint_id = reduced_model.addJoint( reduced_parent_joint_index, joint_input_model, parent_frame_placement * input_model.jointPlacements[joint_id], joint_name, - joint_input_model.jointVelocitySelector(input_model.effortLimit), - joint_input_model.jointVelocitySelector(input_model.velocityLimit), - joint_input_model.jointConfigSelector(input_model.lowerPositionLimit), - joint_input_model.jointConfigSelector(input_model.upperPositionLimit), - joint_input_model.jointVelocitySelector(input_model.friction), - joint_input_model.jointVelocitySelector(input_model.damping)); + joint_input_model.jointVelocityFromNvSelector(input_model.effortLimit), + joint_input_model.jointVelocityFromNvSelector(input_model.velocityLimit), + joint_input_model.jointConfigFromNqSelector(input_model.lowerPositionLimit), + joint_input_model.jointConfigFromNqSelector(input_model.upperPositionLimit), + joint_input_model.jointVelocityFromNvSelector(input_model.friction), + joint_input_model.jointVelocityFromNvSelector(input_model.damping)); // Append inertia reduced_model.appendBodyToJoint( reduced_joint_id, input_model.inertias[joint_id], SE3::Identity()); // Copy other kinematics and dynamics properties const typename Model::JointModel & jmodel_out = reduced_model.joints[reduced_joint_id]; - jmodel_out.jointVelocitySelector(reduced_model.rotorInertia) = - joint_input_model.jointVelocitySelector(input_model.rotorInertia); - jmodel_out.jointVelocitySelector(reduced_model.rotorGearRatio) = - joint_input_model.jointVelocitySelector(input_model.rotorGearRatio); + jmodel_out.jointVelocityFromNvSelector(reduced_model.rotorInertia) = + joint_input_model.jointVelocityFromNvSelector(input_model.rotorInertia); + jmodel_out.jointVelocityFromNvSelector(reduced_model.rotorGearRatio) = + joint_input_model.jointVelocityFromNvSelector(input_model.rotorGearRatio); } } @@ -642,8 +680,8 @@ namespace pinocchio const JointModel & input_joint_model = input_model.joints[input_joint_id]; const JointModel & reduced_joint_model = reduced_model.joints[reduced_joint_id]; - reduced_joint_model.jointConfigSelector(reduced_config_vector) = - input_joint_model.jointConfigSelector(input_config_vector); + reduced_joint_model.jointConfigFromNqSelector(reduced_config_vector) = + input_joint_model.jointConfigFromNqSelector(input_config_vector); } reduced_model.referenceConfigurations.insert( @@ -751,7 +789,7 @@ namespace pinocchio const std::string & parent_joint_name = input_model.names[joint_id_in_input_model]; JointIndex reduced_joint_id = (JointIndex)-1; - typedef typename Model::SE3 SE3; + typedef typename GeometryObject::SE3 SE3; SE3 relative_placement = SE3::Identity(); if (reduced_model.existJointName(parent_joint_name)) { @@ -761,7 +799,7 @@ namespace pinocchio { const FrameIndex reduced_frame_id = reduced_model.getFrameId(parent_joint_name); reduced_joint_id = reduced_model.frames[reduced_frame_id].parentJoint; - relative_placement = reduced_model.frames[reduced_frame_id].placement; + relative_placement = static_cast(reduced_model.frames[reduced_frame_id].placement); } GeometryObject reduced_geom(geom); @@ -788,6 +826,121 @@ namespace pinocchio } } + template class JointCollectionTpl> + void transformJointIntoMimic( + const ModelTpl & input_model, + const JointIndex & index_primary, + const JointIndex & index_secondary, + const Scalar & scaling, + const Scalar & offset, + ModelTpl & output_model) + { + PINOCCHIO_CHECK_INPUT_ARGUMENT( + index_primary <= (size_t)input_model.njoints, + "index of primary is greater than the total of joints"); + PINOCCHIO_CHECK_INPUT_ARGUMENT( + index_secondary <= (size_t)input_model.njoints, + "index of primary is greater than the total of joints"); + PINOCCHIO_CHECK_INPUT_ARGUMENT( + index_primary < index_secondary, "index of primary is greater than secondary"); + + typedef ModelTpl Model; + typedef typename Model::JointModel JointModel; + + output_model = input_model; + + output_model.joints[index_secondary] = JointModelMimic( + input_model.joints.at(index_secondary), output_model.joints.at(index_primary), scaling, + offset); + + int old_nq = input_model.joints.at(index_secondary).nq(); + int old_nv = input_model.joints.at(index_secondary).nv(); + output_model.nq = input_model.nq - old_nq; + output_model.nv = input_model.nv - old_nv; + int nq = output_model.nq; + int nv = output_model.nv; + + // Resize limits + output_model.effortLimit.resize(nv); + output_model.velocityLimit.resize(nv); + output_model.lowerPositionLimit.resize(nq); + output_model.upperPositionLimit.resize(nq); + output_model.armature.resize(nv); + output_model.rotorInertia.resize(nv); + output_model.rotorGearRatio.resize(nv); + output_model.friction.resize(nv); + output_model.damping.resize(nv); + + // Move indexes and limits + for (JointIndex joint_id = 1; joint_id < (JointIndex)index_secondary; ++joint_id) + { + const JointModel & jmodel_input = input_model.joints[joint_id]; + const JointModel & jmodel_output = output_model.joints[joint_id]; + jmodel_output.jointVelocityFromNvSelector(output_model.effortLimit) = + jmodel_input.jointVelocityFromNvSelector(input_model.effortLimit); + jmodel_output.jointVelocityFromNvSelector(output_model.velocityLimit) = + jmodel_input.jointVelocityFromNvSelector(input_model.velocityLimit); + + jmodel_output.jointConfigFromNqSelector(output_model.lowerPositionLimit) = + jmodel_input.jointConfigFromNqSelector(input_model.lowerPositionLimit); + jmodel_output.jointConfigFromNqSelector(output_model.upperPositionLimit) = + jmodel_input.jointConfigFromNqSelector(input_model.upperPositionLimit); + + jmodel_output.jointVelocityFromNvSelector(output_model.armature) = + jmodel_input.jointVelocityFromNvSelector(input_model.armature); + jmodel_output.jointVelocityFromNvSelector(output_model.rotorInertia) = + jmodel_input.jointVelocityFromNvSelector(input_model.rotorInertia); + jmodel_output.jointVelocityFromNvSelector(output_model.rotorGearRatio) = + jmodel_input.jointVelocityFromNvSelector(input_model.rotorGearRatio); + jmodel_output.jointVelocityFromNvSelector(output_model.friction) = + jmodel_input.jointVelocityFromNvSelector(input_model.friction); + jmodel_output.jointVelocityFromNvSelector(output_model.damping) = + jmodel_input.jointVelocityFromNvSelector(input_model.damping); + } + + // Move indexes and limits + int idx_q = output_model.idx_qs[index_secondary]; + int idx_v = output_model.idx_vs[index_secondary]; + for (JointIndex joint_id = index_secondary; joint_id < (JointIndex)input_model.njoints; + ++joint_id) + { + const JointModel & jmodel_input = input_model.joints[joint_id]; + JointModel & jmodel_output = output_model.joints[joint_id]; + jmodel_output.setIndexes(jmodel_input.id(), idx_q, idx_v, jmodel_input.idx_j()); + + output_model.idx_qs[joint_id] = jmodel_output.idx_q(); + output_model.nqs[joint_id] = jmodel_output.nq(); + output_model.idx_vs[joint_id] = jmodel_output.idx_v(); + output_model.nvs[joint_id] = jmodel_output.nv(); + + idx_q += jmodel_output.nq(); + idx_v += jmodel_output.nv(); + if (joint_id != index_secondary) + { + jmodel_output.jointVelocityFromNvSelector(output_model.effortLimit) = + jmodel_input.jointVelocityFromNvSelector(input_model.effortLimit); + jmodel_output.jointVelocityFromNvSelector(output_model.velocityLimit) = + jmodel_input.jointVelocityFromNvSelector(input_model.velocityLimit); + + jmodel_output.jointConfigFromNqSelector(output_model.lowerPositionLimit) = + jmodel_input.jointConfigFromNqSelector(input_model.lowerPositionLimit); + jmodel_output.jointConfigFromNqSelector(output_model.upperPositionLimit) = + jmodel_input.jointConfigFromNqSelector(input_model.upperPositionLimit); + + jmodel_output.jointVelocityFromNvSelector(output_model.armature) = + jmodel_input.jointVelocityFromNvSelector(input_model.armature); + jmodel_output.jointVelocityFromNvSelector(output_model.rotorInertia) = + jmodel_input.jointVelocityFromNvSelector(input_model.rotorInertia); + jmodel_output.jointVelocityFromNvSelector(output_model.rotorGearRatio) = + jmodel_input.jointVelocityFromNvSelector(input_model.rotorGearRatio); + jmodel_output.jointVelocityFromNvSelector(output_model.friction) = + jmodel_input.jointVelocityFromNvSelector(input_model.friction); + jmodel_output.jointVelocityFromNvSelector(output_model.damping) = + jmodel_input.jointVelocityFromNvSelector(input_model.damping); + } + } + } + template class JointCollectionTpl> JointIndex findCommonAncestor( const ModelTpl & model, diff --git a/include/pinocchio/algorithm/model.txx b/include/pinocchio/algorithm/model.txx index b9a34f639b..03f9596ed0 100644 --- a/include/pinocchio/algorithm/model.txx +++ b/include/pinocchio/algorithm/model.txx @@ -81,6 +81,15 @@ namespace pinocchio context::Model &, std::vector> &); + extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI void + transformJointIntoMimic( + const context::Model &, + const JointIndex &, + const JointIndex &, + const context::Scalar &, + const context::Scalar &, + context::Model &); + extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI JointIndex findCommonAncestor( const context::Model &, JointIndex, JointIndex, size_t &, size_t &); diff --git a/include/pinocchio/algorithm/pv.hxx b/include/pinocchio/algorithm/pv.hxx index eacc35523a..9ffcd16b9a 100644 --- a/include/pinocchio/algorithm/pv.hxx +++ b/include/pinocchio/algorithm/pv.hxx @@ -183,16 +183,16 @@ namespace pinocchio bias_and_force.toVector() -= data.Yaba[i] * data.a_bias[i].toVector(); - jmodel.jointVelocitySelector(data.u) -= jdata.S().transpose() * data.f[i]; + jmodel.jointVelocityFromDofSelector(data.u) -= jdata.S().transpose() * data.f[i]; jmodel.calc_aba( - jdata.derived(), jmodel.jointVelocitySelector(model.armature), Ia, parent > 0); + jdata.derived(), jmodel.jointVelocityFromNvSelector(model.armature), Ia, parent > 0); Force & pa = data.f[i]; if (parent > 0) { - pa.toVector().noalias() += - Ia * data.a_bias[i].toVector() + jdata.UDinv() * jmodel.jointVelocitySelector(data.u); + pa.toVector().noalias() += Ia * data.a_bias[i].toVector() + + jdata.UDinv() * jmodel.jointVelocityFromDofSelector(data.u); data.Yaba[parent] += impl::internal::SE3actOn::run(data.liMi[i], Ia); data.f[parent] += data.liMi[i].act(pa); } @@ -228,16 +228,16 @@ namespace pinocchio bias_and_force.toVector() -= data.Yaba[i] * data.a_bias[i].toVector(); - jmodel.jointVelocitySelector(data.u) -= jdata.S().transpose() * data.f[i]; + jmodel.jointVelocityFromDofSelector(data.u) -= jdata.S().transpose() * data.f[i]; jmodel.calc_aba( - jdata.derived(), jmodel.jointVelocitySelector(model.armature), Ia, parent > 0); + jdata.derived(), jmodel.jointVelocityFromDofSelector(model.armature), Ia, parent > 0); Force & pa = data.f[i]; if (parent > 0) { - pa.toVector() += - Ia * data.a_bias[i].toVector() + jdata.UDinv() * jmodel.jointVelocitySelector(data.u); + pa.toVector() += Ia * data.a_bias[i].toVector() + + jdata.UDinv() * jmodel.jointVelocityFromDofSelector(data.u); data.Yaba[parent] += impl::internal::SE3actOn::run(data.liMi[i], Ia); data.f[parent] += data.liMi[i].act(pa); } @@ -257,13 +257,14 @@ namespace pinocchio for (int ind = 0; ind < data.constraints_supported_dim[i]; ind++) { // Abusing previously unused data.tau for a role unrelated to its name below - jmodel.jointVelocitySelector(data.tau).noalias() = jdata.Dinv() * data.KAS[i].col(ind); + jmodel.jointVelocityFromDofSelector(data.tau).noalias() = + jdata.Dinv() * data.KAS[i].col(ind); for (int ind2 = ind; ind2 < data.constraints_supported_dim[i]; ind2++) { data.LA[parent](data.par_cons_ind[i] + ind2, data.par_cons_ind[i] + ind) = data.LA[i](ind2, ind) - + (data.KAS[i].col(ind2).dot(jmodel.jointVelocitySelector(data.tau))); + + (data.KAS[i].col(ind2).dot(jmodel.jointVelocityFromDofSelector(data.tau))); } } @@ -271,11 +272,11 @@ namespace pinocchio if (data.constraints_supported_dim[i] > 0) { // Abusing previously unused data.tau variable for a role unrelated to its name below - jmodel.jointVelocitySelector(data.tau).noalias() = + jmodel.jointVelocityFromDofSelector(data.tau).noalias() = (jdata.Dinv() - * (jdata.S().transpose() * bias_and_force + jmodel.jointVelocitySelector(data.u))) + * (jdata.S().transpose() * bias_and_force + jmodel.jointVelocityFromDofSelector(data.u))) .eval(); - const Motion a_bf = jdata.S() * jmodel.jointVelocitySelector(data.tau); + const Motion a_bf = jdata.S() * jmodel.jointVelocityFromDofSelector(data.tau); const Motion a_bf_motion = a_bf + data.a_bias[i]; for (int ind = 0; ind < data.constraints_supported_dim[i]; ind++) { @@ -310,9 +311,10 @@ namespace pinocchio const JointIndex parent = model.parents[i]; // Abusing otherwise unused data.tau below. - jmodel.jointVelocitySelector(data.tau).noalias() = jdata.S().transpose() * data.f[i]; - jmodel.jointVelocitySelector(data.u) -= jmodel.jointVelocitySelector(data.tau); - data.f[i].toVector().noalias() -= jdata.UDinv() * jmodel.jointVelocitySelector(data.tau); + jmodel.jointVelocityFromDofSelector(data.tau).noalias() = jdata.S().transpose() * data.f[i]; + jmodel.jointVelocityFromNvSelector(data.u) -= jmodel.jointVelocityFromNvSelector(data.tau); + data.f[i].toVector().noalias() -= + jdata.UDinv() * jmodel.jointVelocityFromDofSelector(data.tau); if (parent > 0) { @@ -343,11 +345,11 @@ namespace pinocchio const JointIndex parent = model.parents[i]; data.a[i] = data.liMi[i].actInv(data.a[parent]) + data.a_bias[i]; - jmodel.jointVelocitySelector(data.ddq).noalias() = - jdata.Dinv() * (jmodel.jointVelocitySelector(data.u)) + jmodel.jointVelocityFromDofSelector(data.ddq).noalias() = + jdata.Dinv() * (jmodel.jointVelocityFromDofSelector(data.u)) - jdata.UDinv().transpose() * data.a[i].toVector(); - data.a[i] += jdata.S() * jmodel.jointVelocitySelector(data.ddq); + data.a[i] += jdata.S() * jmodel.jointVelocityFromDofSelector(data.ddq); } }; @@ -373,18 +375,18 @@ namespace pinocchio const JointIndex parent = model.parents[i]; data.a[i] = data.liMi[i].actInv(data.a[parent]) + data.a_bias[i]; - jmodel.jointVelocitySelector(data.ddq).noalias() = - jdata.Dinv() * (jmodel.jointVelocitySelector(data.u)) + jmodel.jointVelocityFromDofSelector(data.ddq).noalias() = + jdata.Dinv() * (jmodel.jointVelocityFromDofSelector(data.u)) - jdata.UDinv().transpose() * data.a[i].toVector(); data.lambdaA[i].noalias() = data.lambdaA[parent].segment(data.par_cons_ind[i], data.lambdaA[i].size()); for (int j = 0; j < data.constraints_supported_dim[i]; j++) { - jmodel.jointVelocitySelector(data.ddq).noalias() -= + jmodel.jointVelocityFromDofSelector(data.ddq).noalias() -= data.lambdaA[i][j] * jdata.Dinv() * (data.KAS[i].col(j)); } - data.a[i] += jdata.S() * jmodel.jointVelocitySelector(data.ddq); + data.a[i] += jdata.S() * jmodel.jointVelocityFromDofSelector(data.ddq); } }; diff --git a/include/pinocchio/algorithm/regressor.hxx b/include/pinocchio/algorithm/regressor.hxx index 88f7cffa51..5c85c42537 100644 --- a/include/pinocchio/algorithm/regressor.hxx +++ b/include/pinocchio/algorithm/regressor.hxx @@ -377,7 +377,7 @@ namespace pinocchio data.v[i] += data.liMi[i].actInv(data.v[parent]); data.a_gf[i] = jdata.c() + (data.v[i] ^ jdata.v()); - data.a_gf[i] += jdata.S() * jmodel.jointVelocitySelector(a); + data.a_gf[i] += jdata.S() * jmodel.jointVelocityFromDofSelector(a); data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]); } }; diff --git a/include/pinocchio/algorithm/rnea-derivatives.hxx b/include/pinocchio/algorithm/rnea-derivatives.hxx index c9f4812bc5..134049f213 100644 --- a/include/pinocchio/algorithm/rnea-derivatives.hxx +++ b/include/pinocchio/algorithm/rnea-derivatives.hxx @@ -37,6 +37,11 @@ namespace pinocchio Data & data, const Eigen::MatrixBase & q) { + assert( + (std::is_same>::value + == false) + && "Algorithm not supported for mimic joints"); + typedef typename Model::JointIndex JointIndex; typedef typename Data::Motion Motion; @@ -59,8 +64,8 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointCols(data.J); - ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); + ColsBlock J_cols = jmodel.jointJacCols(data.J); + ColsBlock dAdq_cols = jmodel.jointVelCols(data.dAdq); J_cols = data.oMi[i].act(jdata.S()); motionSet::motionAction(minus_gravity, J_cols, dAdq_cols); } @@ -93,6 +98,11 @@ namespace pinocchio typename Data::VectorXs & g, const Eigen::MatrixBase & gravity_partial_dq) { + assert( + (std::is_same>::value + == false) + && "Algorithm not supported for mimic joints"); + typedef typename Model::JointIndex JointIndex; typedef Eigen::Matrix< Scalar, JointModel::NV, 6, Options, JointModel::NV == Eigen::Dynamic ? 6 : JointModel::NV, @@ -108,10 +118,10 @@ namespace pinocchio typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointCols(data.J); - ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); - ColsBlock dFdq_cols = jmodel.jointCols(data.dFdq); - ColsBlock Ag_cols = jmodel.jointCols(data.Ag); + ColsBlock J_cols = jmodel.jointJacCols(data.J); + ColsBlock dAdq_cols = jmodel.jointVelCols(data.dAdq); + ColsBlock dFdq_cols = jmodel.jointVelCols(data.dFdq); + ColsBlock Ag_cols = jmodel.jointVelCols(data.Ag); motionSet::inertiaAction(data.oYcrb[i], dAdq_cols, dFdq_cols); @@ -128,7 +138,8 @@ namespace pinocchio gravity_partial_dq_.middleRows(jmodel.idx_v(), jmodel.nv()).col(j).noalias() = Ag_cols.transpose() * data.dAdq.col(j); - jmodel.jointVelocitySelector(g).noalias() = J_cols.transpose() * data.of[i].toVector(); + jmodel.jointVelocityFromDofSelector(g).noalias() = + J_cols.transpose() * data.of[i].toVector(); if (parent > 0) { data.oYcrb[parent] += data.oYcrb[i]; @@ -267,6 +278,11 @@ namespace pinocchio const Eigen::MatrixBase & v, const Eigen::MatrixBase & a) { + assert( + (std::is_same>::value + == false) + && "Algorithm not supported for mimic joints"); + typedef typename Model::JointIndex JointIndex; typedef typename Data::Motion Motion; @@ -291,7 +307,7 @@ namespace pinocchio data.oMi[i] = data.liMi[i]; data.a[i] = - jdata.S() * jmodel.jointVelocitySelector(a) + jdata.c() + (data.v[i] ^ jdata.v()); + jdata.S() * jmodel.jointVelocityFromDofSelector(a) + jdata.c() + (data.v[i] ^ jdata.v()); if (parent > 0) { data.a[i] += data.liMi[i].actInv(data.a[parent]); @@ -308,11 +324,11 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointCols(data.J); - ColsBlock dJ_cols = jmodel.jointCols(data.dJ); - ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); - ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); - ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); + ColsBlock J_cols = jmodel.jointJacCols(data.J); + ColsBlock dJ_cols = jmodel.jointJacCols(data.dJ); + ColsBlock dVdq_cols = jmodel.jointVelCols(data.dVdq); + ColsBlock dAdq_cols = jmodel.jointVelCols(data.dAdq); + ColsBlock dAdv_cols = jmodel.jointVelCols(data.dAdv); J_cols = data.oMi[i].act(jdata.S()); motionSet::motionAction(ov, J_cols, dJ_cols); @@ -381,6 +397,11 @@ namespace pinocchio const Eigen::MatrixBase & rnea_partial_dv, const Eigen::MatrixBase & rnea_partial_da) { + assert( + (std::is_same>::value + == false) + && "Algorithm not supported for mimic joints"); + typedef typename Model::JointIndex JointIndex; typedef typename Data::Matrix6x Matrix6x; @@ -390,21 +411,21 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; Matrix6x & dYtJ = data.Fcrb[0]; - ColsBlock J_cols = jmodel.jointCols(data.J); - ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); - ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); - ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); - ColsBlock dFdq_cols = jmodel.jointCols(data.dFdq); - ColsBlock dFdv_cols = jmodel.jointCols(data.dFdv); - ColsBlock dFda_cols = jmodel.jointCols(data.dFda); // Also equals to Ag_cols - ColsBlock dYtJ_cols = jmodel.jointCols(dYtJ); + ColsBlock J_cols = jmodel.jointJacCols(data.J); + ColsBlock dVdq_cols = jmodel.jointVelCols(data.dVdq); + ColsBlock dAdq_cols = jmodel.jointVelCols(data.dAdq); + ColsBlock dAdv_cols = jmodel.jointVelCols(data.dAdv); + ColsBlock dFdq_cols = jmodel.jointVelCols(data.dFdq); + ColsBlock dFdv_cols = jmodel.jointVelCols(data.dFdv); + ColsBlock dFda_cols = jmodel.jointVelCols(data.dFda); // Also equals to Ag_cols + ColsBlock dYtJ_cols = jmodel.jointVelCols(dYtJ); MatrixType1 & rnea_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType1, rnea_partial_dq); MatrixType2 & rnea_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType2, rnea_partial_dv); MatrixType3 & rnea_partial_da_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType3, rnea_partial_da); // tau - jmodel.jointVelocitySelector(data.tau).noalias() = + jmodel.jointVelocityFromDofSelector(data.tau).noalias() = J_cols.transpose() * data.of[i].toVector(); const Eigen::DenseIndex nv_subtree = data.nvSubtree[i]; diff --git a/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx b/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx index b5c1fe412b..99ac2dfccf 100644 --- a/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx +++ b/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx @@ -47,6 +47,11 @@ namespace pinocchio const Eigen::MatrixBase & v, const Eigen::MatrixBase & a) { + assert( + (std::is_same>::value + == false) + && "Algorithm not supported for mimic joints"); + typedef typename Model::JointIndex JointIndex; typedef typename Data::Motion Motion; typedef typename Data::Inertia Inertia; @@ -77,11 +82,12 @@ namespace pinocchio typename SizeDepType::template ColsReturn::Type ColsBlock; ColsBlock J_cols = - jmodel.jointCols(data.J); // data.J has all the phi (in world frame) stacked in columns - ColsBlock psid_cols = jmodel.jointCols(data.psid); // psid_cols is the psi_dot in world frame + jmodel.jointJacCols(data.J); // data.J has all the phi (in world frame) stacked in columns + ColsBlock psid_cols = + jmodel.jointVelCols(data.psid); // psid_cols is the psi_dot in world frame ColsBlock psidd_cols = - jmodel.jointCols(data.psidd); // psidd_cols is the psi_dotdot in world frame - ColsBlock dJ_cols = jmodel.jointCols(data.dJ); // This here is phi_dot in world frame + jmodel.jointVelCols(data.psidd); // psidd_cols is the psi_dotdot in world frame + ColsBlock dJ_cols = jmodel.jointJacCols(data.dJ); // This here is phi_dot in world frame J_cols.noalias() = data.oMi[i].act(jdata.S()); // J_cols is just the phi in world frame for a joint @@ -92,7 +98,8 @@ namespace pinocchio ov, psid_cols, psidd_cols); // This ov here is v(p(i)) , psi_dotdot calcs ov += vJ; - oa += (ov ^ vJ) + data.oMi[i].act(jdata.S() * jmodel.jointVelocitySelector(a) + jdata.c()); + oa += + (ov ^ vJ) + data.oMi[i].act(jdata.S() * jmodel.jointVelocityFromDofSelector(a) + jdata.c()); motionSet::motionAction(ov, J_cols, dJ_cols); // This here is phi_dot, here ov used is v(p(i)) // + vJ Composite rigid body inertia Inertia & oY = data.oYcrb[i]; @@ -157,6 +164,10 @@ namespace pinocchio const Tensor3 & dtau_dqdv, const Tensor3 & dtau_dadq) { + assert( + (std::is_same>::value + == false) + && "Algorithm not supported for mimic joints"); typedef typename Data::Motion Motion; typedef typename Data::Force Force; typedef typename Data::Inertia Inertia; diff --git a/include/pinocchio/algorithm/rnea.hxx b/include/pinocchio/algorithm/rnea.hxx index f9ba3c7266..a5511da0e8 100644 --- a/include/pinocchio/algorithm/rnea.hxx +++ b/include/pinocchio/algorithm/rnea.hxx @@ -65,7 +65,7 @@ namespace pinocchio data.v[i] += data.liMi[i].actInv(data.v[parent]); data.a_gf[i] = jdata.c() + (data.v[i] ^ jdata.v()); - data.a_gf[i] += jdata.S() * jmodel.jointVelocitySelector(a); + data.a_gf[i] += jdata.S() * jmodel.jointVelocityFromDofSelector(a); data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]); // // data.f[i] = model.inertias[i]*data.a_gf[i];// + model.inertias[i].vxiv(data.v[i]); @@ -99,8 +99,7 @@ namespace pinocchio const JointIndex i = jmodel.id(); const JointIndex parent = model.parents[i]; - - jmodel.jointVelocitySelector(data.tau) = jdata.S().transpose() * data.f[i]; + jmodel.jointVelocityFromDofSelector(data.tau) += jdata.S().transpose() * data.f[i]; if (parent > 0) data.f[parent] += data.liMi[i].act(data.f[i]); @@ -131,6 +130,7 @@ namespace pinocchio typedef ModelTpl Model; typedef typename Model::JointIndex JointIndex; + data.tau.setZero(); data.v[0].setZero(); data.a_gf[0] = -model.gravity; @@ -184,6 +184,7 @@ namespace pinocchio typedef ModelTpl Model; typedef typename Model::JointIndex JointIndex; + data.tau.setZero(); data.v[0].setZero(); data.a_gf[0] = -model.gravity; @@ -279,7 +280,7 @@ namespace pinocchio const JointIndex & i = jmodel.id(); const JointIndex & parent = model.parents[i]; - jmodel.jointVelocitySelector(data.nle) = jdata.S().transpose() * data.f[i]; + jmodel.jointVelocityFromDofSelector(data.nle) += jdata.S().transpose() * data.f[i]; if (parent > 0) data.f[parent] += data.liMi[i].act(data.f[i]); } @@ -306,6 +307,7 @@ namespace pinocchio typedef ModelTpl Model; typedef typename Model::JointIndex JointIndex; + data.nle.setZero(); data.v[0].setZero(); data.a_gf[0] = -model.gravity; @@ -387,7 +389,7 @@ namespace pinocchio const JointIndex & i = jmodel.id(); const JointIndex & parent = model.parents[i]; - jmodel.jointVelocitySelector(g) = jdata.S().transpose() * data.f[i]; + jmodel.jointVelocityFromDofSelector(g) += jdata.S().transpose() * data.f[i]; if (parent > 0) data.f[(size_t)parent] += data.liMi[i].act(data.f[i]); } @@ -411,6 +413,7 @@ namespace pinocchio typedef ModelTpl Model; typedef typename Model::JointIndex JointIndex; + data.g.setZero(); data.a_gf[0] = -model.gravity; typedef ComputeGeneralizedGravityForwardStep< @@ -422,6 +425,7 @@ namespace pinocchio model.joints[i], data.joints[i], typename Pass1::ArgsType(model, data, q.derived())); } + data.g.setZero(); typedef ComputeGeneralizedGravityBackwardStep Pass2; for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) { @@ -464,6 +468,7 @@ namespace pinocchio data.f[i] -= fext[i]; } + data.tau.setZero(); typedef ComputeGeneralizedGravityBackwardStep Pass2; for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) { @@ -530,11 +535,11 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointCols(data.J); + ColsBlock J_cols = jmodel.jointJacCols(data.J); J_cols = data.oMi[i].act(jdata.S()); // collection of S expressed at the world frame // computes vxS expressed at the world frame - ColsBlock dJ_cols = jmodel.jointCols(data.dJ); + ColsBlock dJ_cols = jmodel.jointJacCols(data.dJ); motionSet::motionAction(data.ov[i], J_cols, dJ_cols); data.B[i] = data.oYcrb[i].variation(Scalar(0.5) * data.ov[i]); @@ -568,6 +573,12 @@ namespace pinocchio template static void algo(const JointModelBase & jmodel, const Model & model, Data & data) { + + assert( + (std::is_same>::value + == false) + && "Algorithm not supported for mimic joints"); + typedef typename Model::JointIndex JointIndex; typedef Eigen::Matrix< Scalar, JointModel::NV, 6, Options, JointModel::NV == Eigen::Dynamic ? 6 : JointModel::NV, @@ -582,12 +593,12 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock dJ_cols = jmodel.jointCols(data.dJ); - ColsBlock J_cols = jmodel.jointCols(data.J); - ColsBlock Ag_cols = jmodel.jointCols(data.Ag); + ColsBlock dJ_cols = jmodel.jointJacCols(data.dJ); + ColsBlock J_cols = jmodel.jointJacCols(data.J); + ColsBlock Ag_cols = jmodel.jointVelCols(data.Ag); - motionSet::inertiaAction(data.oYcrb[i], dJ_cols, jmodel.jointCols(data.dFdv)); - jmodel.jointCols(data.dFdv).noalias() += data.B[i] * J_cols; + motionSet::inertiaAction(data.oYcrb[i], dJ_cols, jmodel.jointVelCols(data.dFdv)); + jmodel.jointVelCols(data.dFdv).noalias() += data.B[i] * J_cols; data.C.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]).noalias() = J_cols.transpose() * data.dFdv.middleCols(jmodel.idx_v(), data.nvSubtree[i]); @@ -663,6 +674,12 @@ namespace pinocchio template static void algo(const JointModelBase & jmodel, const Model & model, Data & data) { + + assert( + (std::is_same>::value + == false) + && "Algorithm not supported for mimic joints"); + typedef typename Model::JointIndex JointIndex; typedef Eigen::Matrix< Scalar, JointModel::NV, 6, Options, JointModel::NV == Eigen::Dynamic ? 6 : JointModel::NV, @@ -677,11 +694,11 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock dJ_cols = jmodel.jointCols(data.dJ); - ColsBlock J_cols = jmodel.jointCols(data.J); - ColsBlock Ag_cols = jmodel.jointCols(data.Ag); + ColsBlock dJ_cols = jmodel.jointJacCols(data.dJ); + ColsBlock J_cols = jmodel.jointJacCols(data.J); + ColsBlock Ag_cols = jmodel.jointVelCols(data.Ag); typename Data::Matrix6x & dFdv = data.Fcrb[0]; - ColsBlock dFdv_cols = jmodel.jointCols(dFdv); + ColsBlock dFdv_cols = jmodel.jointVelCols(dFdv); motionSet::inertiaAction(data.oYcrb[i], dJ_cols, dFdv_cols); dFdv_cols.noalias() += data.B[i] * J_cols; diff --git a/include/pinocchio/bindings/python/context/generic.hpp b/include/pinocchio/bindings/python/context/generic.hpp index 9f3eb37616..1eb7a151d3 100644 --- a/include/pinocchio/bindings/python/context/generic.hpp +++ b/include/pinocchio/bindings/python/context/generic.hpp @@ -64,6 +64,7 @@ namespace pinocchio typedef DataTpl Data; typedef JointCollectionDefaultTpl JointCollectionDefault; + typedef JointCollectionMimicableTpl JointCollectionMimicable; // Joints typedef JointModelTpl JointModel; @@ -138,6 +139,11 @@ namespace pinocchio typedef JointModelCompositeTpl JointModelComposite; typedef JointDataCompositeTpl JointDataComposite; + typedef JointModelMimicTpl JointModelMimic; + typedef JointDataMimicTpl JointDataMimic; + + typedef JointModelTpl JointModelMimicable; + // Algorithm typedef ProximalSettingsTpl ProximalSettings; typedef ContactCholeskyDecompositionTpl ContactCholeskyDecomposition; diff --git a/include/pinocchio/bindings/python/multibody/joint/joints-models.hpp b/include/pinocchio/bindings/python/multibody/joint/joints-models.hpp index c6253ade15..77cca4d02d 100644 --- a/include/pinocchio/bindings/python/multibody/joint/joints-models.hpp +++ b/include/pinocchio/bindings/python/multibody/joint/joints-models.hpp @@ -10,6 +10,7 @@ #include "pinocchio/multibody/joint/joint-collection.hpp" #include "pinocchio/multibody/joint/joint-composite.hpp" #include "pinocchio/multibody/joint/joint-generic.hpp" +#include "pinocchio/multibody/joint/joint-mimic.hpp" #include @@ -357,9 +358,64 @@ namespace pinocchio .def(bp::self == bp::self) .def(bp::self != bp::self) #endif - ; } + + // Specialization for JointModelMimic + struct JointModelMimicConstructorVisitor + : public boost::static_visitor + { + const context::Scalar & m_scaling; + const context::Scalar & m_offset; + + JointModelMimicConstructorVisitor( + const context::Scalar & scaling, const context::Scalar & offset) + : m_scaling(scaling) + , m_offset(offset) + { + } + + template + context::JointModelMimic * operator()(const JointModelDerived & jmodel) const + { + + return new context::JointModelMimic(jmodel, m_scaling, m_offset); + } + + }; // struct JointModelMimicConstructorVisitor + + static context::JointModelMimic * init_proxy( + const context::JointModel & jmodel, + const context::Scalar & scaling, + const context::Scalar & offset) + { + return boost::apply_visitor(JointModelMimicConstructorVisitor(scaling, offset), jmodel); + } + + static context::Scalar get_scaling(context::JointModelMimic & jmodel) + { + return jmodel.scaling(); + } + + static context::Scalar get_offset(context::JointModelMimic & jmodel) + { + return jmodel.offset(); + } + + template<> + bp::class_ & + expose_joint_model(bp::class_ & cl) + { + return cl + .def( + "__init__", + bp::make_constructor( + init_proxy, bp::default_call_policies(), bp::args("joint_model", "scaling", "offset")), + "Init JointModelMimic from an existing joint with scaling and offset.") + .add_property("scaling", &get_scaling) + .add_property("offset", &get_offset); + } + } // namespace python } // namespace pinocchio diff --git a/include/pinocchio/bindings/python/multibody/model.hpp b/include/pinocchio/bindings/python/multibody/model.hpp index 091f5beb27..e3fac073c7 100644 --- a/include/pinocchio/bindings/python/multibody/model.hpp +++ b/include/pinocchio/bindings/python/multibody/model.hpp @@ -71,6 +71,7 @@ namespace pinocchio // Class Members .add_property("nq", &Model::nq, "Dimension of the configuration vector representation.") .add_property("nv", &Model::nv, "Dimension of the velocity vector space.") + .add_property("nj", &Model::nv, "Dimension of the jacobian matrix space.") .add_property("njoints", &Model::njoints, "Number of joints.") .add_property("nbodies", &Model::nbodies, "Number of bodies.") .add_property("nframes", &Model::nframes, "Number of frames.") @@ -89,6 +90,9 @@ namespace pinocchio "idx_vs", &Model::idx_vs, "Starting index of the *i*th joint in the tangent configuration space.") .add_property("nvs", &Model::nvs, "Dimension of the *i*th joint tangent subspace.") + .add_property( + "idx_js", &Model::idx_js, "Starting index of the *i*th joint in the jacobian space.") + .add_property("njs", &Model::njs, "Dimension of the *i*th joint jacobian subspace.") .add_property( "parents", &Model::parents, "Vector of parent joint indexes. The parent of joint *i*, denoted *li*, " diff --git a/include/pinocchio/multibody/data.hxx b/include/pinocchio/multibody/data.hxx index c0aed6741c..644f596306 100644 --- a/include/pinocchio/multibody/data.hxx +++ b/include/pinocchio/multibody/data.hxx @@ -79,9 +79,9 @@ namespace pinocchio , parents_fromRow((std::size_t)model.nv, -1) , supports_fromRow((std::size_t)model.nv) , nvSubtree_fromRow((std::size_t)model.nv, -1) - , J(Matrix6x::Zero(6, model.nv)) - , dJ(Matrix6x::Zero(6, model.nv)) - , ddJ(Matrix6x::Zero(6, model.nv)) + , J(Matrix6x::Zero(6, model.nj)) + , dJ(Matrix6x::Zero(6, model.nj)) + , ddJ(Matrix6x::Zero(6, model.nj)) , psid(Matrix6x::Zero(6, model.nv)) , psidd(Matrix6x::Zero(6, model.nv)) , dVdq(Matrix6x::Zero(6, model.nv)) @@ -200,11 +200,21 @@ namespace pinocchio if (lastChild[(Index)i] == -1) lastChild[(Index)i] = i; const Index & parent = model.parents[(Index)i]; + lastChild[parent] = std::max(lastChild[(Index)i], lastChild[parent]); + int nv_; + + if (boost::get>( + &model.joints[(Index)lastChild[(Index)i]])) + nv_ = boost::get>( + model.joints[(Index)lastChild[(Index)i]]) + .jmodel() + .nv(); + else + nv_ = nv(model.joints[(Index)lastChild[(Index)i]]); - nvSubtree[(Index)i] = model.joints[(Index)lastChild[(Index)i]].idx_v() - + model.joints[(Index)lastChild[(Index)i]].nv() - - model.joints[(Index)i].idx_v(); + nvSubtree[(Index)i] = + model.joints[(Index)lastChild[(Index)i]].idx_v() + nv_ - model.joints[(Index)i].idx_v(); } } @@ -216,6 +226,8 @@ namespace pinocchio for (Index joint = 1; joint < (Index)(model.njoints); joint++) { + if (boost::get>(&model.joints[joint])) + continue; // Mimic joints should not override mimicked joint fromRow values const Index & parent = model.parents[joint]; const int nvj = model.joints[joint].nv(); const int idx_vj = model.joints[joint].idx_v(); diff --git a/include/pinocchio/multibody/geometry-object.hpp b/include/pinocchio/multibody/geometry-object.hpp index 03776d8e90..004fe6ee90 100644 --- a/include/pinocchio/multibody/geometry-object.hpp +++ b/include/pinocchio/multibody/geometry-object.hpp @@ -77,10 +77,10 @@ namespace pinocchio template<> struct traits { - typedef context::Scalar Scalar; + typedef double Scalar; enum { - Options = context::Options + Options = 0 }; }; diff --git a/include/pinocchio/multibody/geometry.hpp b/include/pinocchio/multibody/geometry.hpp index 4458548d3e..f2a0ecdd1e 100644 --- a/include/pinocchio/multibody/geometry.hpp +++ b/include/pinocchio/multibody/geometry.hpp @@ -44,7 +44,11 @@ namespace pinocchio template<> struct traits { - typedef context::Scalar Scalar; + typedef double Scalar; + enum + { + Options = 0 + }; }; struct GeometryModel @@ -53,10 +57,10 @@ namespace pinocchio { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - typedef context::Scalar Scalar; + typedef typename traits::Scalar Scalar; enum { - Options = context::Options + Options = traits::Options }; typedef SE3Tpl SE3; @@ -227,7 +231,11 @@ namespace pinocchio template<> struct traits { - typedef context::Scalar Scalar; + typedef double Scalar; + enum + { + Options = 0 + }; }; struct GeometryData @@ -236,10 +244,10 @@ namespace pinocchio { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - typedef context::Scalar Scalar; + typedef typename traits::Scalar Scalar; enum { - Options = context::Options + Options = traits::Options }; typedef SE3Tpl SE3; diff --git a/include/pinocchio/multibody/joint-motion-subspace-generic.hpp b/include/pinocchio/multibody/joint-motion-subspace-generic.hpp index 7ef1026b50..9906d404a8 100644 --- a/include/pinocchio/multibody/joint-motion-subspace-generic.hpp +++ b/include/pinocchio/multibody/joint-motion-subspace-generic.hpp @@ -9,8 +9,8 @@ namespace pinocchio { - template - struct traits> + template + struct traits> { typedef _Scalar Scalar; enum @@ -22,9 +22,9 @@ namespace pinocchio }; typedef MotionTpl JointMotion; - typedef Eigen::Matrix JointForce; - typedef Eigen::Matrix DenseBase; - typedef Eigen::Matrix ReducedSquaredMatrix; + typedef Eigen::Matrix JointForce; + typedef Eigen::Matrix DenseBase; + typedef Eigen::Matrix ReducedSquaredMatrix; typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(DenseBase) ConstMatrixReturnType; typedef typename PINOCCHIO_EIGEN_REF_TYPE(DenseBase) MatrixReturnType; @@ -32,21 +32,38 @@ namespace pinocchio typedef ReducedSquaredMatrix StDiagonalMatrixSOperationReturnType; }; // traits JointMotionSubspaceTpl - template - struct SE3GroupAction> + template + struct SE3GroupAction> { - typedef Eigen::Matrix ReturnType; + typedef Eigen::Matrix ReturnType; }; - template - struct MotionAlgebraAction, MotionDerived> + template + struct MotionAlgebraAction, MotionDerived> { - typedef Eigen::Matrix ReturnType; + typedef Eigen::Matrix ReturnType; }; - template + template + struct ConstraintForceOp, ForceDerived> + { + typedef + typename traits>::DenseBase DenseBase; + typedef Eigen::Matrix ReturnType; + }; + + template + struct ConstraintForceSetOp, ForceSet> + { + typedef + typename traits>::DenseBase DenseBase; + typedef + typename MatrixMatrixProduct, ForceSet>::type ReturnType; + }; + + template struct JointMotionSubspaceTpl - : public JointMotionSubspaceBase> + : public JointMotionSubspaceBase> { EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -60,6 +77,8 @@ namespace pinocchio NV = _Dim }; + constexpr static int MaxNV = NV < 0 ? _MaxDim : NV; + using Base::nv; template @@ -84,7 +103,18 @@ namespace pinocchio : S(6, dim) { EIGEN_STATIC_ASSERT( - _Dim == Eigen::Dynamic, YOU_CALLED_A_FIXED_SIZE_METHOD_ON_A_DYNAMIC_SIZE_MATRIX_OR_VECTOR) + _Dim == Eigen::Dynamic, YOU_CALLED_A_FIXED_SIZE_METHOD_ON_A_DYNAMIC_SIZE_MATRIX_OR_VECTOR); + assert(_MaxDim < 0 || dim <= _MaxDim); + } + + // It is only valid for dynamics size + template + explicit JointMotionSubspaceTpl(const JointMotionSubspaceTpl subspace) + : S(subspace.matrix()) + { + EIGEN_STATIC_ASSERT( + _Dim == Eigen::Dynamic, YOU_CALLED_A_FIXED_SIZE_METHOD_ON_A_DYNAMIC_SIZE_MATRIX_OR_VECTOR); + assert(_MaxDim < 0 || subspace.matrix().cols() <= _MaxDim); } static JointMotionSubspaceTpl Zero(const int dim) @@ -106,16 +136,18 @@ namespace pinocchio { } - template - JointForce operator*(const ForceDense & f) const + template + typename ConstraintForceOp::ReturnType + operator*(const ForceDense & f) const { return (ref.S.transpose() * f.toVector()).eval(); } - template - typename Eigen::Matrix operator*(const Eigen::MatrixBase & F) + template + typename ConstraintForceSetOp::ReturnType + operator*(const Eigen::MatrixBase & F) { - return (ref.S.transpose() * F).eval(); + return ref.S.transpose() * F.derived(); } }; @@ -139,7 +171,7 @@ namespace pinocchio } template - friend typename JointMotionSubspaceTpl<_Dim, _Scalar, _Options>::DenseBase + friend typename JointMotionSubspaceTpl<_Dim, _Scalar, _Options, _MaxDim>::DenseBase operator*(const InertiaTpl & Y, const JointMotionSubspaceTpl & S) { typedef typename JointMotionSubspaceTpl::DenseBase ReturnType; @@ -149,10 +181,10 @@ namespace pinocchio } template - friend Eigen::Matrix<_Scalar, 6, _Dim> + friend Eigen::Matrix<_Scalar, 6, _Dim, _Options, 6, _MaxDim> operator*(const Eigen::Matrix & Ymatrix, const JointMotionSubspaceTpl & S) { - typedef Eigen::Matrix<_Scalar, 6, _Dim> ReturnType; + typedef Eigen::Matrix<_Scalar, 6, _Dim, _Options, 6, _MaxDim> ReturnType; return ReturnType(Ymatrix * S.matrix()); } @@ -194,10 +226,10 @@ namespace pinocchio namespace details { - template - struct StDiagonalMatrixSOperation> + template + struct StDiagonalMatrixSOperation> { - typedef JointMotionSubspaceTpl Constraint; + typedef JointMotionSubspaceTpl Constraint; typedef typename traits::StDiagonalMatrixSOperationReturnType ReturnType; static ReturnType run(const JointMotionSubspaceBase & constraint) diff --git a/include/pinocchio/multibody/joint-motion-subspace.hpp b/include/pinocchio/multibody/joint-motion-subspace.hpp index f5129138a5..ece1448736 100644 --- a/include/pinocchio/multibody/joint-motion-subspace.hpp +++ b/include/pinocchio/multibody/joint-motion-subspace.hpp @@ -10,7 +10,7 @@ namespace pinocchio { - template + template struct JointMotionSubspaceTpl; typedef JointMotionSubspaceTpl<1, context::Scalar, context::Options> JointMotionSubspace1d; diff --git a/include/pinocchio/multibody/joint/fwd.hpp b/include/pinocchio/multibody/joint/fwd.hpp index 05f83e5185..a4d29db3ef 100644 --- a/include/pinocchio/multibody/joint/fwd.hpp +++ b/include/pinocchio/multibody/joint/fwd.hpp @@ -134,6 +134,10 @@ namespace pinocchio struct JointCollectionDefaultTpl; typedef JointCollectionDefaultTpl JointCollectionDefault; + template + struct JointCollectionMimicableTpl; + typedef JointCollectionMimicableTpl JointCollectionMimicable; + template< typename Scalar, int Options = context::Options, @@ -148,6 +152,20 @@ namespace pinocchio struct JointDataCompositeTpl; typedef JointDataCompositeTpl JointDataComposite; + template< + typename Scalar, + int Options = context::Options, + template class JointCollectionTpl = JointCollectionDefaultTpl> + struct JointModelMimicTpl; + typedef JointModelMimicTpl JointModelMimic; + + template< + typename Scalar, + int Options = context::Options, + template class JointCollectionTpl = JointCollectionDefaultTpl> + struct JointDataMimicTpl; + typedef JointDataMimicTpl JointDataMimic; + template< typename Scalar, int Options = context::Options, diff --git a/include/pinocchio/multibody/joint/joint-basic-visitors.hpp b/include/pinocchio/multibody/joint/joint-basic-visitors.hpp index 4aa7a32358..a70833836b 100644 --- a/include/pinocchio/multibody/joint/joint-basic-visitors.hpp +++ b/include/pinocchio/multibody/joint/joint-basic-visitors.hpp @@ -144,6 +144,17 @@ namespace pinocchio template class JointCollectionTpl> inline int nq(const JointModelTpl & jmodel); + /** + * @brief Visit a JointModelTpl through JointNjVisitor to get the dimension of + * the joint configuration space + * + * @param[in] jmodel The JointModelVariant + * + * @return The dimension of joint jacobian space + */ + template class JointCollectionTpl> + inline int nj(const JointModelTpl & jmodel); + /** * @brief Visit a JointModelTpl through JointConfigurationLimitVisitor * to get the configurations limits @@ -192,6 +203,18 @@ namespace pinocchio template class JointCollectionTpl> inline int idx_v(const JointModelTpl & jmodel); + /** + * @brief Visit a JointModelTpl through JointIdjVVisitor to get the index in the full model + * tangent space corresponding to the first joint jacobian space degree + * + * @param[in] jmodel The JointModelVariant + * + * @return The index in the full model tangent space corresponding to the first + * joint jacobian space degree + */ + template class JointCollectionTpl> + inline int idx_j(const JointModelTpl & jmodel); + /** * @brief Visit a JointModelTpl through JointIdVisitor to get the index of the joint in the * kinematic chain @@ -213,12 +236,18 @@ namespace pinocchio * degree of freedom * @param[in] v The index in the full model tangent space corresponding to the first joint * tangent space degree + * @param[in] j The index in the full model tangent space corresponding to the first joint + * jacobian space degree * * @return The index of the joint in the kinematic chain */ template class JointCollectionTpl> inline void setIndexes( - JointModelTpl & jmodel, JointIndex id, int q, int v); + JointModelTpl & jmodel, + JointIndex id, + int q, + int v, + int j); /** * @brief Visit a JointModelTpl through JointShortnameVisitor to get the shortname of the @@ -423,6 +452,46 @@ namespace pinocchio const JointDataTpl & jmodel_generic, const JointDataBase & jmodel); + /** + * @brief Transfer a value from one variant to another (given that the new variant contains the + * type of the value). For instance transfer a joint model (or data) from a generic joint model + * (or data) to another generic joint model (or data) with slightly different joint collections. + * + * @param[in] variant The value to transfer. + * + * @return The same value in the new variant. + */ + template + VariantDst transferToVariant(const VariantSrc & value); + + /** + * @brief Apply the correct affine transform, on a joint configuration, depending on the joint + * type. + * + * @tparam Scalar Type of scaling and offset scalars. + * @tparam Options + * @tparam JointCollectionTpl Collection of Joint types + * @tparam ConfigVectorIn Type of the input joint configuration vector. + * @tparam ConfigVectorOut Type of the ouptut joint configuration vector. + * @param jmodel Joint variant to determine the correct affine transform to use. + * @param qIn Input configuration vector + * @param scaling scaling factor + * @param offset Offset value + * @param qOut Ouptut joint configuration vector + */ + template< + typename Scalar, + int Options, + template class JointCollectionTpl, + typename ConfigVectorIn, + typename ConfigVectorOut> + void configVectorAffineTransform( + const JointModelTpl & jmodel, + const Eigen::MatrixBase & qIn, + const Scalar & scaling, + const Scalar & offset, + const Eigen::MatrixBase & qOut); + } // namespace pinocchio /* --- Details -------------------------------------------------------------------- */ diff --git a/include/pinocchio/multibody/joint/joint-basic-visitors.hxx b/include/pinocchio/multibody/joint/joint-basic-visitors.hxx index 548b1926c0..3fef515c54 100644 --- a/include/pinocchio/multibody/joint/joint-basic-visitors.hxx +++ b/include/pinocchio/multibody/joint/joint-basic-visitors.hxx @@ -193,6 +193,31 @@ namespace pinocchio PINOCCHIO_EIGEN_CONST_CAST(Matrix6Type, I), update_I)); } + template + struct JointConfigFromDofSelectorVisitor + : fusion:: + JointUnaryVisitorBase, ReturnType> + { + typedef boost::fusion::vector ArgsType; + + template + static ReturnType algo(const JointModelBase & jmodel, InputType a) + { + // Converting a VectorBlock of anysize (static or dynamic) to another vector block of anysize + // (static or dynamic) since there is no copy constructor. + auto vectorBlock = jmodel.jointConfigFromDofSelector(a); + + // VectorBlock does not implemet such getter, hack the Eigen::Block base class to retreive + // such values. + const Index start = vectorBlock.startRow() + + vectorBlock.startCol(); // The other dimension is always 0 (for vectors) + const Index size = + vectorBlock.rows() * vectorBlock.cols(); // The other dimension is always 1 (for vectors) + + return ReturnType(vectorBlock.nestedExpression(), start, size); + } + }; + /** * @brief JointNvVisitor visitor */ @@ -241,6 +266,30 @@ namespace pinocchio return JointNqVisitor::run(jmodel); } + /** + * @brief JointNjVisitor visitor + */ + struct JointNjVisitor : boost::static_visitor + { + template + int operator()(const JointModelBase & jmodel) const + { + return jmodel.nj(); + } + + template class JointCollectionTpl> + static int run(const JointModelTpl & jmodel) + { + return boost::apply_visitor(JointNjVisitor(), jmodel); + } + }; + + template class JointCollectionTpl> + inline int nj(const JointModelTpl & jmodel) + { + return JointNjVisitor::run(jmodel); + } + /** * @brief JointConfigurationLimitVisitor visitor */ @@ -341,6 +390,30 @@ namespace pinocchio return JointIdxVVisitor::run(jmodel); } + /** + * @brief JointIdxjVisitor visitor + */ + struct JointIdxJVisitor : boost::static_visitor + { + template + int operator()(const JointModelBase & jmodel) const + { + return jmodel.idx_j(); + } + + template class JointCollectionTpl> + static int run(const JointModelTpl & jmodel) + { + return boost::apply_visitor(JointIdxJVisitor(), jmodel); + } + }; + + template class JointCollectionTpl> + inline int idx_j(const JointModelTpl & jmodel) + { + return JointIdxJVisitor::run(jmodel); + } + /** * @brief JointIdVisitor visitor */ @@ -373,33 +446,39 @@ namespace pinocchio JointIndex id; int q; int v; + int j; - JointSetIndexesVisitor(JointIndex id, int q, int v) + JointSetIndexesVisitor(JointIndex id, int q, int v, int j) : id(id) , q(q) , v(v) + , j(j) { } template void operator()(JointModelBase & jmodel) const { - jmodel.setIndexes(id, q, v); + jmodel.setIndexes(id, q, v, j); } template class JointCollectionTpl> - static void - run(JointModelTpl & jmodel, JointIndex id, int q, int v) + static void run( + JointModelTpl & jmodel, + JointIndex id, + int q, + int v, + int j) { - return boost::apply_visitor(JointSetIndexesVisitor(id, q, v), jmodel); + return boost::apply_visitor(JointSetIndexesVisitor(id, q, v, j), jmodel); } }; template class JointCollectionTpl> inline void setIndexes( - JointModelTpl & jmodel, JointIndex id, int q, int v) + JointModelTpl & jmodel, JointIndex id, int q, int v, int j) { - return JointSetIndexesVisitor::run(jmodel, id, q, v); + return JointSetIndexesVisitor::run(jmodel, id, q, v, j); } /** @@ -864,6 +943,87 @@ namespace pinocchio return Algo::run(jdata_generic, typename Algo::ArgsType(boost::ref(jdata.derived()))); } + // Meta-function to check is_mimicable_t trait + template + struct is_mimicable + { + static constexpr bool value = traits::is_mimicable_t::value; + }; + + template + struct CheckMimicVisitor : public boost::static_visitor + { + template + typename boost::enable_if_c::value, JointModel>::type + operator()(const T & value) const + { + return value; + } + + template + typename boost::disable_if_c::value, JointModel>::type + operator()(const T & value) const + { + assert(false && "Type not supported in new variant"); + return value; + } + }; + + template + JointModel checkMimic(const JointModel & value) + { + return boost::apply_visitor(CheckMimicVisitor(), value); + } + + template + struct ConfigVectorAffineTransformVisitor : public boost::static_visitor + { + public: + const Eigen::MatrixBase & qIn; + const Scalar & scaling; + const Scalar & offset; + const Eigen::MatrixBase & qOut; + + ConfigVectorAffineTransformVisitor( + const Eigen::MatrixBase & qIn, + const Scalar & scaling, + const Scalar & offset, + const Eigen::MatrixBase & qOut) + : qIn(qIn) + , scaling(scaling) + , offset(offset) + , qOut(qOut) + { + } + + template + void operator()(const JointModel & /*jmodel*/) const + { + typedef typename ConfigVectorAffineTransform::Type + AffineTransform; + AffineTransform::run(qIn, scaling, offset, qOut); + } + }; + + template< + typename Scalar, + int Options, + template class JointCollectionTpl, + typename ConfigVectorIn, + typename ConfigVectorOut> + void configVectorAffineTransform( + const JointModelTpl & jmodel, + const Eigen::MatrixBase & qIn, + const Scalar & scaling, + const Scalar & offset, + const Eigen::MatrixBase & qOut) + { + boost::apply_visitor( + ConfigVectorAffineTransformVisitor( + qIn, scaling, offset, qOut), + jmodel); + } + /// @endcond } // namespace pinocchio diff --git a/include/pinocchio/multibody/joint/joint-collection.hpp b/include/pinocchio/multibody/joint/joint-collection.hpp index f8158bb197..540d8fb3fb 100644 --- a/include/pinocchio/multibody/joint/joint-collection.hpp +++ b/include/pinocchio/multibody/joint/joint-collection.hpp @@ -28,10 +28,6 @@ namespace pinocchio typedef JointModelRevoluteTpl JointModelRY; typedef JointModelRevoluteTpl JointModelRZ; - typedef JointModelMimic JointModelMimicRX; - typedef JointModelMimic JointModelMimicRY; - typedef JointModelMimic JointModelMimicRZ; - // Joint Revolute Unaligned typedef JointModelRevoluteUnalignedTpl JointModelRevoluteUnaligned; @@ -71,6 +67,10 @@ namespace pinocchio typedef JointModelCompositeTpl JointModelComposite; + // Joint Mimic + typedef JointModelMimicTpl + JointModelMimic; + // Joint Helical typedef JointModelHelicalTpl JointModelHx; typedef JointModelHelicalTpl JointModelHy; @@ -87,9 +87,6 @@ namespace pinocchio JointModelRX, JointModelRY, JointModelRZ, - JointModelMimicRX, - JointModelMimicRY, - JointModelMimicRZ, JointModelFreeFlyer, JointModelPlanar, JointModelRevoluteUnaligned, @@ -109,7 +106,8 @@ namespace pinocchio JointModelHz, JointModelHelicalUnaligned, JointModelUniversal, - boost::recursive_wrapper> + boost::recursive_wrapper, + boost::recursive_wrapper> JointModelVariant; // Joint Revolute @@ -117,10 +115,6 @@ namespace pinocchio typedef JointDataRevoluteTpl JointDataRY; typedef JointDataRevoluteTpl JointDataRZ; - typedef JointDataMimic JointDataMimicRX; - typedef JointDataMimic JointDataMimicRY; - typedef JointDataMimic JointDataMimicRZ; - // Joint Revolute Unaligned typedef JointDataRevoluteUnalignedTpl JointDataRevoluteUnaligned; @@ -160,6 +154,10 @@ namespace pinocchio typedef JointDataCompositeTpl JointDataComposite; + // Joint Mimic + typedef JointDataMimicTpl + JointDataMimic; + // Joint Helical typedef JointDataHelicalTpl JointDataHx; typedef JointDataHelicalTpl JointDataHy; @@ -176,9 +174,6 @@ namespace pinocchio JointDataRX, JointDataRY, JointDataRZ, - JointDataMimicRX, - JointDataMimicRY, - JointDataMimicRZ, JointDataFreeFlyer, JointDataPlanar, JointDataRevoluteUnaligned, @@ -198,7 +193,8 @@ namespace pinocchio JointDataHz, JointDataHelicalUnaligned, JointDataUniversal, - boost::recursive_wrapper> + boost::recursive_wrapper, + boost::recursive_wrapper> JointDataVariant; }; diff --git a/include/pinocchio/multibody/joint/joint-common-operations.hpp b/include/pinocchio/multibody/joint/joint-common-operations.hpp index 4401914414..9267bb4141 100644 --- a/include/pinocchio/multibody/joint/joint-common-operations.hpp +++ b/include/pinocchio/multibody/joint/joint-common-operations.hpp @@ -7,8 +7,10 @@ #include "pinocchio/macros.hpp" #include "pinocchio/math/matrix.hpp" +#include "pinocchio/math/fwd.hpp" #include +#include namespace pinocchio { @@ -51,25 +53,68 @@ namespace pinocchio { template static void run( - const Eigen::MatrixBase & q, + const Eigen::MatrixBase & qIn, const Scalar & scaling, const Scalar & offset, - const Eigen::MatrixBase & dest) + const Eigen::MatrixBase & qOut) { - assert(q.size() == dest.size()); - PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, dest).noalias() = - scaling * q + ConfigVectorOut::Constant(dest.size(), offset); + assert(qIn.size() == qOut.size()); + PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, qOut).noalias() = + scaling * qIn + ConfigVectorOut::Constant(qOut.size(), offset); + } + }; + + struct UnboundedRevoluteAffineTransform + { + template + static void run( + const Eigen::MatrixBase & qIn, + const Scalar & scaling, + const Scalar & offset, + const Eigen::MatrixBase & qOut) + { + assert(qIn.size() == 2); + assert(qOut.size() == 2); + + const typename ConfigVectorIn::Scalar & ca = qIn(0); + const typename ConfigVectorIn::Scalar & sa = qIn(1); + + const typename ConfigVectorIn::Scalar & theta = math::atan2(sa, ca); + const typename ConfigVectorIn::Scalar & theta_transform = scaling * theta + offset; + + ConfigVectorOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, qOut); + SINCOS(theta_transform, &dest_.coeffRef(1), &dest_.coeffRef(0)); + } + }; + + struct NoAffineTransform + { + template + static void run( + const Eigen::MatrixBase & qIn, + const Scalar & scaling, + const Scalar & offset, + const Eigen::MatrixBase & qOut) + { + assert( + check_expression_if_real( + math::fabs(scaling - 1.0) < Eigen::NumTraits::dummy_precision()) + && check_expression_if_real( + math::fabs(offset) < Eigen::NumTraits::dummy_precision()) + && "No ConfigVectorAffineTransform specialized for this joint type"); + + PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, qOut).noalias() = qIn; } }; /// /// \brief Assign the correct configuration vector space affine transformation according to the - /// joint type. + /// joint type. Must be specialized for every joint type. /// template struct ConfigVectorAffineTransform { - typedef LinearAffineTransform Type; + typedef NoAffineTransform Type; }; } // namespace pinocchio diff --git a/include/pinocchio/multibody/joint/joint-composite.hpp b/include/pinocchio/multibody/joint/joint-composite.hpp index 9216238396..b94d43a1f6 100644 --- a/include/pinocchio/multibody/joint/joint-composite.hpp +++ b/include/pinocchio/multibody/joint/joint-composite.hpp @@ -28,7 +28,8 @@ namespace pinocchio { Options = _Options, NQ = Eigen::Dynamic, - NV = Eigen::Dynamic + NV = Eigen::Dynamic, + NJ = Eigen::Dynamic }; typedef JointCollectionTpl JointCollection; @@ -47,6 +48,8 @@ namespace pinocchio typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + typedef boost::mpl::false_ is_mimicable_t; + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; @@ -193,8 +196,10 @@ namespace pinocchio typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModelVariant) JointModelVector; using Base::id; + using Base::idx_j; using Base::idx_q; using Base::idx_v; + using Base::nj; using Base::nq; using Base::nv; using Base::setIndexes; @@ -205,6 +210,7 @@ namespace pinocchio , jointPlacements() , m_nq(0) , m_nv(0) + , m_nj(0) , njoints(0) { } @@ -215,14 +221,17 @@ namespace pinocchio , jointPlacements() , m_nq(0) , m_nv(0) + , m_nj(0) , njoints(0) { joints.reserve(size); jointPlacements.reserve(size); m_idx_q.reserve(size); m_idx_v.reserve(size); + m_idx_j.reserve(size); m_nqs.reserve(size); m_nvs.reserve(size); + m_njs.reserve(size); } /// @@ -238,10 +247,13 @@ namespace pinocchio , jointPlacements(1, placement) , m_nq(jmodel.nq()) , m_nv(jmodel.nv()) + , m_nj(jmodel.nj()) , m_idx_q(1, 0) , m_nqs(1, jmodel.nq()) , m_idx_v(1, 0) , m_nvs(1, jmodel.nv()) + , m_idx_j(1, 0) + , m_njs(1, jmodel.nj()) , njoints(1) { } @@ -257,10 +269,13 @@ namespace pinocchio , jointPlacements(other.jointPlacements) , m_nq(other.m_nq) , m_nv(other.m_nv) + , m_nj(other.m_nj) , m_idx_q(other.m_idx_q) , m_nqs(other.m_nqs) , m_idx_v(other.m_idx_v) , m_nvs(other.m_nvs) + , m_idx_j(other.m_idx_j) + , m_njs(other.m_njs) , njoints(other.njoints) { } @@ -282,6 +297,7 @@ namespace pinocchio m_nq += jmodel.nq(); m_nv += jmodel.nv(); + m_nj += jmodel.nj(); updateJointIndexes(); njoints++; @@ -367,13 +383,17 @@ namespace pinocchio { return m_nq; } + int nj_impl() const + { + return m_nj; + } /** * @brief Update the indexes of subjoints in the stack */ - void setIndexes_impl(JointIndex id, int q, int v) + void setIndexes_impl(JointIndex id, int q, int v, int j) { - Base::setIndexes_impl(id, q, v); + Base::setIndexes_impl(id, q, v, j); updateJointIndexes(); } @@ -391,10 +411,13 @@ namespace pinocchio Base::operator=(other); m_nq = other.m_nq; m_nv = other.m_nv; + m_nj = other.m_nj; m_idx_q = other.m_idx_q; m_idx_v = other.m_idx_v; + m_idx_j = other.m_idx_j; m_nqs = other.m_nqs; m_nvs = other.m_nvs; + m_njs = other.m_njs; joints = other.joints; jointPlacements = other.jointPlacements; njoints = other.njoints; @@ -407,10 +430,13 @@ namespace pinocchio { return Base::isEqual(other) && internal::comparison_eq(nq(), other.nq()) && internal::comparison_eq(nv(), other.nv()) + && internal::comparison_eq(nj(), other.nj()) && internal::comparison_eq(m_idx_q, other.m_idx_q) && internal::comparison_eq(m_idx_v, other.m_idx_v) + && internal::comparison_eq(m_idx_j, other.m_idx_j) && internal::comparison_eq(m_nqs, other.m_nqs) && internal::comparison_eq(m_nvs, other.m_nvs) + && internal::comparison_eq(m_njs, other.m_njs) && internal::comparison_eq(joints, other.joints) && internal::comparison_eq(jointPlacements, other.jointPlacements) && internal::comparison_eq(njoints, other.njoints); @@ -422,13 +448,16 @@ namespace pinocchio { typedef JointModelCompositeTpl ReturnType; ReturnType res((size_t)njoints); - res.setIndexes(id(), idx_q(), idx_v()); + res.setIndexes(id(), idx_q(), idx_v(), idx_j()); res.m_nq = m_nq; res.m_nv = m_nv; + res.m_nj = m_nj; res.m_idx_q = m_idx_q; res.m_idx_v = m_idx_v; + res.m_idx_j = m_idx_j; res.m_nqs = m_nqs; res.m_nvs = m_nvs; + res.m_njs = m_njs; res.njoints = njoints; res.joints.resize(joints.size()); @@ -450,79 +479,157 @@ namespace pinocchio template typename SizeDepType::template SegmentReturn::ConstType - jointConfigSelector(const Eigen::MatrixBase & a) const + jointConfigFromDofSelector(const Eigen::MatrixBase & a) const + { + return a.segment(Base::i_q, nq()); + } + template + typename SizeDepType::template SegmentReturn::Type + jointConfigFromDofSelector(Eigen::MatrixBase & a) const + { + return a.segment(Base::i_q, nq()); + } + + template + typename SizeDepType::template SegmentReturn::ConstType + jointConfigFromNqSelector(const Eigen::MatrixBase & a) const { return a.segment(Base::i_q, nq()); } template typename SizeDepType::template SegmentReturn::Type - jointConfigSelector(Eigen::MatrixBase & a) const + jointConfigFromNqSelector(Eigen::MatrixBase & a) const { return a.segment(Base::i_q, nq()); } template typename SizeDepType::template SegmentReturn::ConstType - jointVelocitySelector(const Eigen::MatrixBase & a) const + jointVelocityFromDofSelector(const Eigen::MatrixBase & a) const + { + return a.segment(Base::i_v, nv()); + } + template + typename SizeDepType::template SegmentReturn::Type + jointVelocityFromDofSelector(Eigen::MatrixBase & a) const + { + return a.segment(Base::i_v, nv()); + } + + template + typename SizeDepType::template SegmentReturn::ConstType + jointVelocityFromNvSelector(const Eigen::MatrixBase & a) const { return a.segment(Base::i_v, nv()); } template typename SizeDepType::template SegmentReturn::Type - jointVelocitySelector(Eigen::MatrixBase & a) const + jointVelocityFromNvSelector(Eigen::MatrixBase & a) const { return a.segment(Base::i_v, nv()); } template typename SizeDepType::template ColsReturn::ConstType - jointCols(const Eigen::MatrixBase & A) const + jointVelCols(const Eigen::MatrixBase & A) const { return A.middleCols(Base::i_v, nv()); } template - typename SizeDepType::template ColsReturn::Type jointCols(Eigen::MatrixBase & A) const + typename SizeDepType::template ColsReturn::ConstType + jointJacCols(const Eigen::MatrixBase & A) const + { + return A.middleCols(Base::i_j, nj()); + } + template + typename SizeDepType::template ColsReturn::Type + jointVelCols(Eigen::MatrixBase & A) const { return A.middleCols(Base::i_v, nv()); } + template + typename SizeDepType::template ColsReturn::Type + jointJacCols(Eigen::MatrixBase & A) const + { + return A.middleCols(Base::i_j, nj()); + } template typename SizeDepType::template SegmentReturn::ConstType - jointConfigSelector_impl(const Eigen::MatrixBase & a) const + jointConfigFromNqSelector_impl(const Eigen::MatrixBase & a) const { return a.segment(Base::i_q, nq()); } template typename SizeDepType::template SegmentReturn::Type - jointConfigSelector_impl(Eigen::MatrixBase & a) const + jointConfigFromNqSelector_impl(Eigen::MatrixBase & a) const { return a.segment(Base::i_q, nq()); } + + template + typename SizeDepType::template SegmentReturn::ConstType + jointConfigFromDofSelector_impl(const Eigen::MatrixBase & a) const + { + return a.segment(Base::i_q, nq()); + } + template + typename SizeDepType::template SegmentReturn::Type + jointConfigFromDofSelector_impl(Eigen::MatrixBase & a) const + { + return a.segment(Base::i_q, nq()); + } + template typename SizeDepType::template SegmentReturn::ConstType - jointVelocitySelector_impl(const Eigen::MatrixBase & a) const + jointVelocityFromDofSelector_impl(const Eigen::MatrixBase & a) const { return a.segment(Base::i_v, nv()); } template typename SizeDepType::template SegmentReturn::Type - jointVelocitySelector_impl(Eigen::MatrixBase & a) const + jointVelocityFromDofSelector_impl(Eigen::MatrixBase & a) const + { + return a.segment(Base::i_v, nv()); + } + + template + typename SizeDepType::template SegmentReturn::ConstType + jointVelocityFromNvSelector_impl(const Eigen::MatrixBase & a) const + { + return a.segment(Base::i_v, nv()); + } + template + typename SizeDepType::template SegmentReturn::Type + jointVelocityFromNvSelector_impl(Eigen::MatrixBase & a) const { return a.segment(Base::i_v, nv()); } template typename SizeDepType::template ColsReturn::ConstType - jointCols_impl(const Eigen::MatrixBase & A) const + jointVelCols_impl(const Eigen::MatrixBase & A) const { return A.middleCols(Base::i_v, nv()); } template + typename SizeDepType::template ColsReturn::ConstType + jointJacCols_impl(const Eigen::MatrixBase & A) const + { + return A.middleCols(Base::i_j, nj()); + } + template typename SizeDepType::template ColsReturn::Type - jointCols_impl(Eigen::MatrixBase & A) const + jointVelCols_impl(Eigen::MatrixBase & A) const { return A.middleCols(Base::i_v, nv()); } + template + typename SizeDepType::template ColsReturn::Type + jointJacCols_impl(Eigen::MatrixBase & A) const + { + return A.middleCols(Base::i_j, nj()); + } protected: friend struct Serialize; @@ -536,11 +643,14 @@ namespace pinocchio { int idx_q = this->idx_q(); int idx_v = this->idx_v(); + int idx_j = this->idx_j(); m_idx_q.resize(joints.size()); m_idx_v.resize(joints.size()); + m_idx_j.resize(joints.size()); m_nqs.resize(joints.size()); m_nvs.resize(joints.size()); + m_njs.resize(joints.size()); for (size_t i = 0; i < joints.size(); ++i) { @@ -548,16 +658,19 @@ namespace pinocchio m_idx_q[i] = idx_q; m_idx_v[i] = idx_v; - ::pinocchio::setIndexes(joint, i, idx_q, idx_v); + m_idx_j[i] = idx_j; + ::pinocchio::setIndexes(joint, i, idx_q, idx_v, idx_j); m_nqs[i] = ::pinocchio::nq(joint); m_nvs[i] = ::pinocchio::nv(joint); + m_njs[i] = ::pinocchio::nj(joint); idx_q += m_nqs[i]; idx_v += m_nvs[i]; + idx_j += m_njs[i]; } } /// \brief Dimensions of the config and tangent space of the composite joint. - int m_nq, m_nv; + int m_nq, m_nv, m_nj; /// Keep information of both the dimension and the position of the joints in the composition. @@ -569,6 +682,10 @@ namespace pinocchio std::vector m_idx_v; /// \brief Dimension of the segment in the tangent vector std::vector m_nvs; + /// \brief Index in the jacobian matrix + std::vector m_idx_j; + /// \brief Dimension of the segment in the jacobian matrix + std::vector m_njs; public: /// \brief Number of joints contained in the JointModelComposite diff --git a/include/pinocchio/multibody/joint/joint-free-flyer.hpp b/include/pinocchio/multibody/joint/joint-free-flyer.hpp index 7274494c3b..5d3ca7b47b 100644 --- a/include/pinocchio/multibody/joint/joint-free-flyer.hpp +++ b/include/pinocchio/multibody/joint/joint-free-flyer.hpp @@ -168,7 +168,8 @@ namespace pinocchio enum { NQ = 7, - NV = 6 + NV = 6, + NJ = 6 }; typedef _Scalar Scalar; enum @@ -190,6 +191,8 @@ namespace pinocchio typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + typedef boost::mpl::true_ is_mimicable_t; + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; @@ -287,6 +290,7 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; + using Base::idx_j; using Base::idx_q; using Base::idx_v; using Base::setIndexes; @@ -402,7 +406,7 @@ namespace pinocchio { typedef JointModelFreeFlyerTpl ReturnType; ReturnType res; - res.setIndexes(id(), idx_q(), idx_v()); + res.setIndexes(id(), idx_q(), idx_v(), idx_j()); return res; } diff --git a/include/pinocchio/multibody/joint/joint-generic.hpp b/include/pinocchio/multibody/joint/joint-generic.hpp index 478c1278b8..c4d14bbf92 100644 --- a/include/pinocchio/multibody/joint/joint-generic.hpp +++ b/include/pinocchio/multibody/joint/joint-generic.hpp @@ -29,7 +29,8 @@ namespace pinocchio { Options = _Options, NQ = Eigen::Dynamic, // Dynamic because unknown at compile time - NV = Eigen::Dynamic + NV = Eigen::Dynamic, + NJ = Eigen::Dynamic }; typedef _Scalar Scalar; @@ -69,6 +70,8 @@ namespace pinocchio typedef ConfigVector_t ConfigVectorTypeRef; typedef TangentVector_t TangentVectorTypeConstRef; typedef TangentVector_t TangentVectorTypeRef; + + typedef boost::mpl::false_ is_mimicable_t; }; template class JointCollectionTpl> @@ -379,6 +382,31 @@ namespace pinocchio *this, data, armature.derived(), PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I), update_I); } + /* Acces to dedicated segment in robot config space. */ + // Const access + template + typename SizeDepType::template SegmentReturn::ConstType + jointConfigFromDofSelector_impl(const Eigen::MatrixBase & a) const + { + typedef const Eigen::MatrixBase & InputType; + typedef typename SizeDepType::template SegmentReturn::ConstType ReturnType; + typedef JointConfigFromDofSelectorVisitor Visitor; + typename Visitor::ArgsType arg(a); + return Visitor::run(*this, arg); + } + + // Non-const access + template + typename SizeDepType::template SegmentReturn::Type + jointConfigFromDofSelector_impl(Eigen::MatrixBase & a) const + { + typedef Eigen::MatrixBase & InputType; + typedef typename SizeDepType::template SegmentReturn::Type ReturnType; + typedef JointConfigFromDofSelectorVisitor Visitor; + typename Visitor::ArgsType arg(a); + return Visitor::run(*this, arg); + } + std::string shortname() const { return ::pinocchio::shortname(*this); @@ -396,6 +424,10 @@ namespace pinocchio { return ::pinocchio::nv(*this); } + int nj_impl() const + { + return ::pinocchio::nj(*this); + } int idx_q_impl() const { @@ -405,15 +437,19 @@ namespace pinocchio { return ::pinocchio::idx_v(*this); } + int idx_j_impl() const + { + return ::pinocchio::idx_j(*this); + } JointIndex id_impl() const { return ::pinocchio::id(*this); } - void setIndexes(JointIndex id, int nq, int nv) + void setIndexes(JointIndex id, int nq, int nv, int nj) { - ::pinocchio::setIndexes(*this, id, nq, nv); + ::pinocchio::setIndexes(*this, id, nq, nv, nj); } /// \returns An expression of *this with the Scalar type casted to NewScalar. diff --git a/include/pinocchio/multibody/joint/joint-helical-unaligned.hpp b/include/pinocchio/multibody/joint/joint-helical-unaligned.hpp index 790f9ec01b..55d2989e89 100644 --- a/include/pinocchio/multibody/joint/joint-helical-unaligned.hpp +++ b/include/pinocchio/multibody/joint/joint-helical-unaligned.hpp @@ -524,7 +524,8 @@ namespace pinocchio enum { NQ = 1, - NV = 1 + NV = 1, + NJ = 1 }; typedef _Scalar Scalar; enum @@ -546,6 +547,8 @@ namespace pinocchio typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + typedef boost::mpl::true_ is_mimicable_t; + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; @@ -636,6 +639,7 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; + using Base::idx_j; using Base::idx_q; using Base::idx_v; using Base::setIndexes; @@ -758,7 +762,7 @@ namespace pinocchio { typedef JointModelHelicalUnalignedTpl ReturnType; ReturnType res(axis.template cast(), ScalarCast::cast(m_pitch)); - res.setIndexes(id(), idx_q(), idx_v()); + res.setIndexes(id(), idx_q(), idx_v(), idx_j()); return res; } diff --git a/include/pinocchio/multibody/joint/joint-helical.hpp b/include/pinocchio/multibody/joint/joint-helical.hpp index dc92b2fb44..94ea970661 100644 --- a/include/pinocchio/multibody/joint/joint-helical.hpp +++ b/include/pinocchio/multibody/joint/joint-helical.hpp @@ -735,7 +735,8 @@ namespace pinocchio enum { NQ = 1, - NV = 1 + NV = 1, + NJ = 1 }; typedef _Scalar Scalar; enum @@ -757,6 +758,8 @@ namespace pinocchio typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + typedef boost::mpl::true_ is_mimicable_t; + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; @@ -835,6 +838,7 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; + using Base::idx_j; using Base::idx_q; using Base::idx_v; using Base::setIndexes; @@ -947,7 +951,7 @@ namespace pinocchio { typedef JointModelHelicalTpl ReturnType; ReturnType res(ScalarCast::cast(m_pitch)); - res.setIndexes(id(), idx_q(), idx_v()); + res.setIndexes(id(), idx_q(), idx_v(), idx_j()); return res; } diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 4725bc968e..3ad4f17f6c 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -5,117 +5,134 @@ #ifndef __pinocchio_multibody_joint_mimic_hpp__ #define __pinocchio_multibody_joint_mimic_hpp__ +#include "pinocchio/multibody/joint/fwd.hpp" +#include "pinocchio/multibody/joint/joint-collection.hpp" #include "pinocchio/macros.hpp" #include "pinocchio/multibody/joint/joint-base.hpp" +#include "pinocchio/multibody/joint/joint-basic-visitors.hpp" +#include +#include +#include +#include +#include +#include namespace pinocchio { + template + struct ScaledJointMotionSubspaceTpl; - template - struct ScaledJointMotionSubspace; - - template - struct traits> + template + struct traits> { - typedef typename traits::Scalar Scalar; enum { - Options = traits::Options + MaxDim = _MaxDim }; + typedef JointMotionSubspaceTpl + RefJointMotionSubspace; + typedef typename traits::Scalar Scalar; enum { - LINEAR = traits::LINEAR, - ANGULAR = traits::ANGULAR + Options = traits::Options }; - - typedef typename traits::JointMotion JointMotion; - typedef typename traits::JointForce JointForce; - typedef typename traits::DenseBase DenseBase; - typedef typename traits::ReducedSquaredMatrix ReducedSquaredMatrix; - - typedef typename traits::MatrixReturnType MatrixReturnType; - typedef typename traits::ConstMatrixReturnType ConstMatrixReturnType; - typedef ReducedSquaredMatrix StDiagonalMatrixSOperationReturnType; - }; // traits ScaledJointMotionSubspace - - template - struct SE3GroupAction> + enum + { + LINEAR = traits::LINEAR, + ANGULAR = traits::ANGULAR + }; + typedef typename traits::JointMotion JointMotion; + typedef typename traits::JointForce JointForce; + typedef typename traits::DenseBase DenseBase; + typedef typename traits::MatrixReturnType MatrixReturnType; + typedef typename traits::ConstMatrixReturnType ConstMatrixReturnType; + }; // traits ScaledJointMotionSubspaceTpl + + template + struct SE3GroupAction> { - typedef typename SE3GroupAction::ReturnType ReturnType; + typedef typename SE3GroupAction>::RefJointMotionSubspace>::ReturnType + ReturnType; }; - template - struct MotionAlgebraAction, MotionDerived> + template + struct MotionAlgebraAction< + ScaledJointMotionSubspaceTpl<_Scalar, _Options, _MaxDim>, + MotionDerived> { - typedef typename MotionAlgebraAction::ReturnType ReturnType; + typedef typename MotionAlgebraAction< + typename traits< + ScaledJointMotionSubspaceTpl<_Scalar, _Options, _MaxDim>>::RefJointMotionSubspace, + MotionDerived>::ReturnType ReturnType; }; - template - struct ConstraintForceOp, ForceDerived> + template + struct ConstraintForceOp, ForceDerived> { - typedef typename Constraint::Scalar Scalar; - typedef typename ConstraintForceOp::ReturnType OriginalReturnType; - - typedef typename ScalarMatrixProduct::type IdealReturnType; - typedef Eigen::Matrix< - Scalar, - IdealReturnType::RowsAtCompileTime, - IdealReturnType::ColsAtCompileTime, - Constraint::Options> - ReturnType; + typedef + typename ScaledJointMotionSubspaceTpl<_Scalar, _Options, _MaxDim>::RefJointMotionSubspace + RefJointMotionSubspace; + typedef + typename ConstraintForceOp::ReturnType RefReturnType; + typedef typename ScalarMatrixProduct<_Scalar, RefReturnType>::type ReturnType; }; - template - struct ConstraintForceSetOp, ForceSet> + template + struct ConstraintForceSetOp, ForceSet> { - typedef typename Constraint::Scalar Scalar; - typedef typename ConstraintForceSetOp::ReturnType OriginalReturnType; - typedef typename ScalarMatrixProduct::type IdealReturnType; - typedef Eigen::Matrix< - Scalar, - Constraint::NV, - ForceSet::ColsAtCompileTime, - Constraint::Options | Eigen::RowMajor> - ReturnType; + typedef + typename ScaledJointMotionSubspaceTpl<_Scalar, _Options, _MaxDim>::RefJointMotionSubspace + RefJointMotionSubspace; + typedef + typename ConstraintForceSetOp::ReturnType RefReturnType; + typedef typename ScalarMatrixProduct<_Scalar, RefReturnType>::type ReturnType; }; - template - struct ScaledJointMotionSubspace : JointMotionSubspaceBase> + template + struct ScaledJointMotionSubspaceTpl + : JointMotionSubspaceBase> { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ScaledJointMotionSubspace) + PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ScaledJointMotionSubspaceTpl) enum { - NV = Constraint::NV + NV = Eigen::Dynamic, + MaxDim = _MaxDim }; - typedef JointMotionSubspaceBase Base; + typedef JointMotionSubspaceBase Base; using Base::nv; - typedef typename SE3GroupAction::ReturnType SE3ActionReturnType; + typedef typename traits>:: + RefJointMotionSubspace RefJointMotionSubspace; + typedef typename SE3GroupAction::ReturnType SE3ActionReturnType; - ScaledJointMotionSubspace() + ScaledJointMotionSubspaceTpl() + : ScaledJointMotionSubspaceTpl(1.0) { } - explicit ScaledJointMotionSubspace(const Scalar & scaling_factor) + explicit ScaledJointMotionSubspaceTpl(const Scalar & scaling_factor) : m_scaling_factor(scaling_factor) + , m_constraint(0) { } - ScaledJointMotionSubspace(const Constraint & constraint, const Scalar & scaling_factor) + template + ScaledJointMotionSubspaceTpl(const ConstraintTpl & constraint, const Scalar & scaling_factor) : m_constraint(constraint) , m_scaling_factor(scaling_factor) { } - ScaledJointMotionSubspace(const ScaledJointMotionSubspace & other) + ScaledJointMotionSubspaceTpl(const ScaledJointMotionSubspaceTpl & other) : m_constraint(other.m_constraint) , m_scaling_factor(other.m_scaling_factor) { } - ScaledJointMotionSubspace & operator=(const ScaledJointMotionSubspace & other) + ScaledJointMotionSubspaceTpl & operator=(const ScaledJointMotionSubspaceTpl & other) { m_constraint = other.m_constraint; m_scaling_factor = other.m_scaling_factor; @@ -125,23 +142,22 @@ namespace pinocchio template JointMotion __mult__(const Eigen::MatrixBase & v) const { + assert(v.size() == nv()); JointMotion jm = m_constraint * v; - return jm * m_scaling_factor; + return m_scaling_factor * jm; } template SE3ActionReturnType se3Action(const SE3Tpl & m) const { - SE3ActionReturnType res = m_constraint.se3Action(m); - return m_scaling_factor * res; + return m_scaling_factor * m_constraint.se3Action(m); } template SE3ActionReturnType se3ActionInverse(const SE3Tpl & m) const { - SE3ActionReturnType res = m_constraint.se3ActionInverse(m); - return m_scaling_factor * res; + return m_scaling_factor * m_constraint.se3ActionInverse(m); } int nv_impl() const @@ -149,33 +165,27 @@ namespace pinocchio return m_constraint.nv(); } - struct TransposeConst : JointMotionSubspaceTransposeBase + struct TransposeConst { - const ScaledJointMotionSubspace & ref; - TransposeConst(const ScaledJointMotionSubspace & ref) + const ScaledJointMotionSubspaceTpl & ref; + explicit TransposeConst(const ScaledJointMotionSubspaceTpl & ref) : ref(ref) { } template - typename ConstraintForceOp::ReturnType - operator*(const ForceDense & f) const + // typename ConstraintForceOp::ReturnType + JointForce operator*(const ForceDense & f) const { - // TODO: I don't know why, but we should a dense a return type, otherwise it fails at the - // evaluation level; - typedef - typename ConstraintForceOp::ReturnType ReturnType; - return ReturnType(ref.m_scaling_factor * (ref.m_constraint.transpose() * f)); + return ref.m_scaling_factor * (ref.m_constraint.transpose() * f); } - /// [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) + /// [CRBA] MatrixBase operator* (RefConstraint::Transpose S, ForceSet::Block) template - typename ConstraintForceSetOp::ReturnType + typename ConstraintForceSetOp::ReturnType operator*(const Eigen::MatrixBase & F) const { - typedef - typename ConstraintForceSetOp::ReturnType ReturnType; - return ReturnType(ref.m_scaling_factor * (ref.m_constraint.transpose() * F)); + return ref.m_scaling_factor * (ref.m_constraint.transpose() * F); } }; // struct TransposeConst @@ -185,20 +195,23 @@ namespace pinocchio return TransposeConst(*this); } - DenseBase matrix_impl() const + const DenseBase & matrix_impl() const + { + S = m_scaling_factor * m_constraint.matrix_impl(); + return S; + } + + DenseBase & matrix_impl() { - DenseBase S = m_scaling_factor * m_constraint.matrix(); + S = m_scaling_factor * m_constraint.matrix_impl(); return S; } template - typename MotionAlgebraAction::ReturnType + typename MotionAlgebraAction::ReturnType motionAction(const MotionDense & m) const { - typedef typename MotionAlgebraAction::ReturnType - ReturnType; - ReturnType res = m_scaling_factor * m_constraint.motionAction(m); - return res; + return m_scaling_factor * m_constraint.motionAction(m); } inline const Scalar & scaling() const @@ -210,63 +223,44 @@ namespace pinocchio return m_scaling_factor; } - inline const Constraint & constraint() const + inline const RefJointMotionSubspace & constraint() const { - return m_constraint; + return m_constraint.derived(); } - inline Constraint & constraint() + inline RefJointMotionSubspace & constraint() { - return m_constraint; + return m_constraint.derived(); } - bool isEqual(const ScaledJointMotionSubspace & other) const + bool isEqual(const ScaledJointMotionSubspaceTpl & other) const { - return internal::comparison_eq(m_constraint, other.m_constraint) - && internal::comparison_eq(m_scaling_factor, other.m_scaling_factor); + return m_constraint == other.m_constraint && m_scaling_factor == other.m_scaling_factor; } protected: - Constraint m_constraint; + RefJointMotionSubspace m_constraint; Scalar m_scaling_factor; - }; // struct ScaledJointMotionSubspace - - namespace details - { - template - struct StDiagonalMatrixSOperation> - { - typedef ScaledJointMotionSubspace Constraint; - typedef typename traits::StDiagonalMatrixSOperationReturnType ReturnType; + mutable DenseBase S; + }; // struct ScaledJointMotionSubspaceTpl - static ReturnType run(const JointMotionSubspaceBase & constraint) - { - const Constraint & constraint_ = constraint.derived(); - return (constraint_.constraint().transpose() * constraint_.constraint()) - * (constraint_.scaling() * constraint_.scaling()); - } - }; - } // namespace details - - template - struct MultiplicationOp, ScaledJointMotionSubspace<_Constraint>> + template + struct MultiplicationOp, ScaledJointMotionSubspaceTpl> { typedef InertiaTpl Inertia; - typedef ScaledJointMotionSubspace<_Constraint> Constraint; + typedef ScaledJointMotionSubspaceTpl Constraint; typedef typename Constraint::Scalar Scalar; - typedef typename MultiplicationOp::ReturnType OriginalReturnType; - // typedef typename ScalarMatrixProduct::type ReturnType; - typedef OriginalReturnType ReturnType; + typedef Eigen::Matrix ReturnType; }; /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */ namespace impl { - template - struct LhsMultiplicationOp, ScaledJointMotionSubspace<_Constraint>> + template + struct LhsMultiplicationOp, ScaledJointMotionSubspaceTpl> { typedef InertiaTpl Inertia; - typedef ScaledJointMotionSubspace<_Constraint> Constraint; + typedef ScaledJointMotionSubspaceTpl Constraint; typedef typename MultiplicationOp::ReturnType ReturnType; static inline ReturnType run(const Inertia & Y, const Constraint & scaled_constraint) @@ -276,20 +270,20 @@ namespace pinocchio }; } // namespace impl - template - struct MultiplicationOp, ScaledJointMotionSubspace<_Constraint>> + template + struct MultiplicationOp, ScaledJointMotionSubspaceTpl> { - typedef typename MultiplicationOp::ReturnType OriginalReturnType; - typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(OriginalReturnType) ReturnType; + typedef ScaledJointMotionSubspaceTpl MotionSubspace; + typedef Eigen::Matrix ReturnType; }; /* [ABA] operator* (Inertia Y,Constraint S) */ namespace impl { - template - struct LhsMultiplicationOp, ScaledJointMotionSubspace<_Constraint>> + template + struct LhsMultiplicationOp, ScaledJointMotionSubspaceTpl> { - typedef ScaledJointMotionSubspace<_Constraint> Constraint; + typedef ScaledJointMotionSubspaceTpl Constraint; typedef typename MultiplicationOp, Constraint>::ReturnType ReturnType; @@ -301,141 +295,144 @@ namespace pinocchio }; } // namespace impl - template - struct JointMimic; - template - struct JointModelMimic; - template - struct JointDataMimic; + template class JointCollectionTpl> + struct JointMimicTpl; + template class JointCollectionTpl> + struct JointModelMimicTpl; + template class JointCollectionTpl> + struct JointDataMimicTpl; - template - struct traits> + template class JointCollectionTpl> + struct traits> { + typedef _Scalar Scalar; + enum { - NQ = traits::NV, - NV = traits::NQ - }; - typedef typename traits::Scalar Scalar; - enum - { - Options = traits::Options + Options = _Options, + NQ = Eigen::Dynamic, + NV = Eigen::Dynamic, + NJ = Eigen::Dynamic, + MaxNJ = 6 }; - typedef typename traits::JointDataDerived JointDataBase; - typedef typename traits::JointModelDerived JointModelBase; - - typedef JointDataMimic JointDataDerived; - typedef JointModelMimic JointModelDerived; + typedef JointCollectionTpl JointCollection; + typedef JointDataMimicTpl JointDataDerived; + typedef JointModelMimicTpl JointModelDerived; + typedef ScaledJointMotionSubspaceTpl Constraint_t; + typedef SE3Tpl Transformation_t; - typedef ScaledJointMotionSubspace::Constraint_t> Constraint_t; - typedef typename traits::Transformation_t Transformation_t; - typedef typename traits::Motion_t Motion_t; - typedef typename traits::Bias_t Bias_t; + typedef MotionTpl Motion_t; + typedef MotionTpl Bias_t; // [ABA] - typedef typename traits::U_t U_t; - typedef typename traits::D_t D_t; - typedef typename traits::UD_t UD_t; + typedef Eigen::Matrix U_t; + typedef Eigen::Matrix D_t; + typedef Eigen::Matrix UD_t; - typedef typename traits::ConfigVector_t ConfigVector_t; - typedef typename traits::TangentVector_t TangentVector_t; + typedef Eigen::Matrix ConfigVector_t; + typedef Eigen::Matrix TangentVector_t; + + typedef boost::mpl::false_ is_mimicable_t; PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; - template - struct traits> + template class JointCollectionTpl> + struct traits> { - typedef JointMimic::JointDerived> JointDerived; - typedef typename traits::Scalar Scalar; + typedef JointMimicTpl<_Scalar, Options, JointCollectionTpl> JointDerived; + typedef _Scalar Scalar; }; - template - struct traits> + template class JointCollectionTpl> + struct traits> { - typedef JointMimic::JointDerived> JointDerived; - typedef typename traits::Scalar Scalar; + typedef JointMimicTpl<_Scalar, Options, JointCollectionTpl> JointDerived; + typedef _Scalar Scalar; }; - template - struct JointDataMimic : public JointDataBase> + template class JointCollectionTpl> + struct JointDataMimicTpl + : public JointDataBase> { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - typedef typename traits::JointDerived JointDerived; - typedef JointDataBase> Base; - + typedef JointDataBase Base; + typedef JointMimicTpl<_Scalar, _Options, JointCollectionTpl> JointDerived; PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); - JointDataMimic() + typedef JointDataTpl<_Scalar, _Options, JointCollectionTpl> RefJointData; + typedef typename RefJointData::JointDataVariant RefJointDataVariant; + + JointDataMimicTpl() : m_scaling((Scalar)0) - , joint_q(ConfigVector_t::Zero()) - , joint_v(TangentVector_t::Zero()) , S((Scalar)0) { + joint_q.resize(0, 1); + joint_q_transformed.resize(0, 1); + joint_v.resize(0, 1); + joint_v_transformed.resize(0, 1); } - JointDataMimic(const JointDataMimic & other) - { - *this = other; - } + // JointDataMimicTpl(const JointDataMimicTpl & other) + // { *this = other; } - JointDataMimic(const JointDataBase & jdata, const Scalar & scaling) - : m_jdata_ref(jdata.derived()) + // JointDataMimicTpl(const RefJointDataVariant & jdata, + // const Scalar & scaling, + // const int & nq, + // const int & nv) + // : m_jdata_ref(jdata) + // , m_scaling(scaling) + // , S(m_jdata_ref.S(),scaling) + // { + + // } + + JointDataMimicTpl( + const RefJointData & jdata, const Scalar & scaling, const int & nq, const int & nv) + : m_jdata_ref(checkMimic(jdata.derived())) , m_scaling(scaling) - , S(m_jdata_ref.S, scaling) + , S(m_jdata_ref.S(), scaling) { + joint_q.resize(nq, 1); + joint_q_transformed.resize(nq, 1); + joint_v.resize(nv, 1); + joint_v_transformed.resize(nv, 1); } - JointDataMimic & operator=(const JointDataMimic & other) + JointDataMimicTpl & operator=(const JointDataMimicTpl & other) { m_jdata_ref = other.m_jdata_ref; m_scaling = other.m_scaling; joint_q = other.joint_q; + joint_q_transformed = other.joint_q_transformed; joint_v = other.joint_v; - S = Constraint_t(m_jdata_ref.S, other.m_scaling); + joint_v_transformed = other.joint_v_transformed; + S = Constraint_t(m_jdata_ref.S(), other.m_scaling); return *this; } using Base::isEqual; - bool isEqual(const JointDataMimic & other) const + bool isEqual(const JointDataMimicTpl & other) const { - return Base::isEqual(other) && internal::comparison_eq(m_jdata_ref, other.m_jdata_ref) - && internal::comparison_eq(m_scaling, other.m_scaling) - && internal::comparison_eq(joint_q, other.joint_q) - && internal::comparison_eq(joint_v, other.joint_v); + return Base::isEqual(other) && m_jdata_ref == other.m_jdata_ref + && m_scaling == other.m_scaling && joint_q == other.joint_q + && joint_q_transformed == other.joint_q_transformed && joint_v == other.joint_v + && joint_v_transformed == other.joint_v_transformed; } static std::string classname() { - return std::string("JointDataMimic<") + JointData::classname() + std::string(">"); + return std::string("JointDataMimic"); } std::string shortname() const { - return std::string("JointDataMimic<") + m_jdata_ref.shortname() + std::string(">"); - } - - // Accessors - ConfigVectorTypeConstRef joint_q_accessor() const - { - return joint_q; - } - ConfigVectorTypeRef joint_q_accessor() - { - return joint_q; - } - - TangentVectorTypeConstRef joint_v_accessor() const - { - return joint_v; - } - TangentVectorTypeRef joint_v_accessor() - { - return joint_v; + return classname(); } + // // Accessors ConstraintTypeConstRef S_accessor() const { return S; @@ -447,75 +444,102 @@ namespace pinocchio TansformTypeConstRef M_accessor() const { - return m_jdata_ref.M; + M_ = m_jdata_ref.M(); + return M_; } TansformTypeRef M_accessor() { - return m_jdata_ref.M; + // assert(false && "Changes to non const ref on mimic joints won't be taken into account. Use + // const ref"); + M_ = m_jdata_ref.M(); + return M_; } MotionTypeConstRef v_accessor() const { - return m_jdata_ref.v; + v_ = m_jdata_ref.v(); + return v_; } MotionTypeRef v_accessor() { - return m_jdata_ref.v; + // assert(false && "Changes to non const ref on mimic joints won't be taken into account. Use + // const ref"); + v_ = m_jdata_ref.v(); + return v_; } BiasTypeConstRef c_accessor() const { - return m_jdata_ref.c; + c_ = m_jdata_ref.c(); + return c_; } BiasTypeRef c_accessor() { - return m_jdata_ref.c; + // assert(false && "Changes to non const ref on mimic joints won't be taken into account. Use + // const ref"); + c_ = m_jdata_ref.c(); + return c_; } UTypeConstRef U_accessor() const { - return m_jdata_ref.U; + U_ = m_jdata_ref.U(); + return U_; } UTypeRef U_accessor() { - return m_jdata_ref.U; + // assert(false && "Changes to non const ref on mimic joints won't be taken into account. Use + // const ref"); + U_ = m_jdata_ref.U(); + return U_; } DTypeConstRef Dinv_accessor() const { - return m_jdata_ref.Dinv; + Dinv_ = m_jdata_ref.Dinv(); + return Dinv_; } DTypeRef Dinv_accessor() { - return m_jdata_ref.Dinv; + // assert(false && "Changes to non const ref on mimic joints won't be taken into account. Use + // const ref"); + Dinv_ = m_jdata_ref.Dinv(); + return Dinv_; } UDTypeConstRef UDinv_accessor() const { - return m_jdata_ref.UDinv; + UDinv_ = m_jdata_ref.UDinv(); + return UDinv_; } UDTypeRef UDinv_accessor() { - return m_jdata_ref.UDinv; + // assert(false && "Changes to non const ref on mimic joints won't be taken into account. Use + // const ref"); + UDinv_ = m_jdata_ref.UDinv(); + return UDinv_; } DTypeConstRef StU_accessor() const { - return m_jdata_ref.StU; + StU_ = m_jdata_ref.StU(); + return StU_; } DTypeRef StU_accessor() { - return m_jdata_ref.StU; + // assert(false && "Changes to non const ref on mimic joints won't be taken into account. Use + // const ref"); + StU_ = m_jdata_ref.StU(); + return StU_; } - template - friend struct JointModelMimic; + friend struct JointModelMimicTpl<_Scalar, _Options, JointCollectionTpl>; - const JointData & jdata() const + const RefJointData & jdata() const { return m_jdata_ref; } - JointData & jdata() + RefJointData & jdata() { return m_jdata_ref; } @@ -529,73 +553,140 @@ namespace pinocchio return m_scaling; } - ConfigVector_t & jointConfiguration() + ConfigVector_t & joint_q_accessor() { return joint_q; } - const ConfigVector_t & jointConfiguration() const + const ConfigVector_t & joint_q_accessor() const { return joint_q; } - TangentVector_t & jointVelocity() + ConfigVector_t & q_transformed() + { + return joint_q_transformed; + } + const ConfigVector_t & q_transformed() const + { + return joint_q_transformed; + } + + TangentVector_t & joint_v_accessor() { return joint_v; } - const TangentVector_t & jointVelocity() const + const TangentVector_t & joint_v_accessor() const { return joint_v; } + TangentVector_t & v_transformed() + { + return joint_v_transformed; + } + const TangentVector_t & v_transformed() const + { + return joint_v_transformed; + } + protected: - JointData m_jdata_ref; + RefJointData m_jdata_ref; Scalar m_scaling; - /// \brief Transform configuration vector + /// \brief original configuration vector ConfigVector_t joint_q; - /// \brief Transform velocity vector. + /// \brief Transformed configuration vector + ConfigVector_t joint_q_transformed; + /// \brief original velocity vector TangentVector_t joint_v; + /// \brief Transform velocity vector. + TangentVector_t joint_v_transformed; public: // data Constraint_t S; - }; // struct JointDataMimic - - template - struct CastType> + protected: + /// \brief Buffer variable for accessors to return references + mutable Transformation_t M_; + mutable Motion_t v_; + mutable Bias_t c_; + mutable U_t U_; + mutable D_t Dinv_; + mutable UD_t UDinv_; + mutable D_t StU_; + + }; // struct JointDataMimicTpl + + template< + typename NewScalar, + typename Scalar, + int Options, + template class JointCollectionTpl> + struct CastType> { - typedef typename CastType::type JointModelNewType; - typedef JointModelMimic type; + typedef JointModelMimicTpl type; }; - template - struct JointModelMimic : public JointModelBase> + template class JointCollectionTpl> + struct JointModelMimicTpl + : public JointModelBase> { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - typedef typename traits::JointDerived JointDerived; - + typedef JointModelBase Base; + typedef JointMimicTpl<_Scalar, _Options, JointCollectionTpl> JointDerived; PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); + enum + { + MaxNJ = traits::MaxNJ + }; + + typedef JointCollectionTpl JointCollection; + typedef JointModelTpl JointModel; + typedef typename JointModel::JointModelVariant JointModelVariant; + + typedef SE3Tpl SE3; + typedef MotionTpl Motion; + typedef InertiaTpl Inertia; - typedef JointModelBase Base; using Base::id; + using Base::idx_j; using Base::idx_q; using Base::idx_v; + using Base::nj; using Base::nq; using Base::nv; using Base::setIndexes; - JointModelMimic() + JointModelMimicTpl() { } - JointModelMimic( + template + JointModelMimicTpl( const JointModelBase & jmodel, const Scalar & scaling, const Scalar & offset) - : m_jmodel_ref(jmodel.derived()) + : JointModelMimicTpl(jmodel, jmodel, scaling, offset) + { + } + + template + JointModelMimicTpl( + const JointModelBase & jmodel_mimicking, + const JointModelBase & jmodel_mimicked, + const Scalar & scaling, + const Scalar & offset) + : m_jmodel_ref(checkMimic((JointModel)jmodel_mimicking.derived())) , m_scaling(scaling) , m_offset(offset) { + assert(jmodel_mimicking.nq() == jmodel_mimicked.nq()); + assert(jmodel_mimicking.nv() == jmodel_mimicked.nv()); + assert(jmodel_mimicking.nj() == jmodel_mimicked.nj()); + + setMimicIndexes( + jmodel_mimicked.id(), jmodel_mimicked.idx_q(), jmodel_mimicked.idx_v(), + jmodel_mimicked.idx_j()); } Base & base() @@ -615,26 +706,36 @@ namespace pinocchio { return 0; } - - inline int idx_q_impl() const + inline int nj_impl() const { - return m_jmodel_ref.idx_q(); + return m_jmodel_ref.nj(); } - inline int idx_v_impl() const + + void setIndexes_impl(JointIndex id, int /*q*/, int /*v*/, int j) { - return m_jmodel_ref.idx_v(); + Base::i_id = id; + // When setting the indexes q and v should remain on the mimicked joint + // Base::i_q = q; + // Base::i_v = v; + Base::i_j = j; } - void setIndexes_impl(JointIndex id, int /*q*/, int /*v*/) + // Specific way for mimic joints to set the mimicked q,v indexes. + // Used for manipulating tree (e.g. appendModel) + void setMimicIndexes(JointIndex id, int q, int v, int j) { - Base::i_id = id; // Only the id of the joint in the model is different. - Base::i_q = m_jmodel_ref.idx_q(); - Base::i_v = m_jmodel_ref.idx_v(); + // Set idx_q, idx_v to zero so that only sub segment of q,v can be passed to ref joint + m_jmodel_ref.setIndexes(id, 0, 0, j); + // idx_q, idx_v kept separately + Base::i_q = q; + Base::i_v = v; } JointDataDerived createData() const { - return JointDataDerived(m_jmodel_ref.createData(), scaling()); + + return JointDataDerived( + m_jmodel_ref.createData(), scaling(), m_jmodel_ref.nq(), m_jmodel_ref.nv()); } const std::vector hasConfigurationLimit() const @@ -651,20 +752,10 @@ namespace pinocchio EIGEN_DONT_INLINE void calc(JointDataDerived & jdata, const typename Eigen::MatrixBase & qs) const { - typedef typename ConfigVectorAffineTransform::Type AffineTransform; - - AffineTransform::run(qs.head(m_jmodel_ref.nq()), m_scaling, m_offset, jdata.joint_q); - m_jmodel_ref.calc(jdata.m_jdata_ref, jdata.joint_q); - } - - template - EIGEN_DONT_INLINE void calc( - JointDataDerived & jdata, - const Blank blank, - const typename Eigen::MatrixBase & vs) const - { - jdata.joint_v = m_scaling * vs.head(m_jmodel_ref.nv()); - m_jmodel_ref.calc(jdata.m_jdata_ref, blank, jdata.joint_v); + jdata.joint_q = qs.segment(idx_q(), m_jmodel_ref.nq()); + configVectorAffineTransform( + m_jmodel_ref, jdata.joint_q, m_scaling, m_offset, jdata.joint_q_transformed); + m_jmodel_ref.calc(jdata.m_jdata_ref, jdata.joint_q_transformed); } template @@ -673,11 +764,12 @@ namespace pinocchio const typename Eigen::MatrixBase & qs, const typename Eigen::MatrixBase & vs) const { - typedef typename ConfigVectorAffineTransform::Type AffineTransform; - - AffineTransform::run(qs.head(m_jmodel_ref.nq()), m_scaling, m_offset, jdata.joint_q); - jdata.joint_v = m_scaling * vs.head(m_jmodel_ref.nv()); - m_jmodel_ref.calc(jdata.m_jdata_ref, jdata.joint_q, jdata.joint_v); + jdata.joint_q = qs.segment(idx_q(), m_jmodel_ref.nq()); + jdata.joint_v = vs.segment(idx_v(), m_jmodel_ref.nv()); + configVectorAffineTransform( + m_jmodel_ref, jdata.joint_q, m_scaling, m_offset, jdata.joint_q_transformed); + jdata.joint_v_transformed = m_scaling * jdata.joint_v; + m_jmodel_ref.calc(jdata.m_jdata_ref, jdata.joint_q_transformed, jdata.joint_v_transformed); } template @@ -688,31 +780,34 @@ namespace pinocchio const bool update_I) const { // TODO: fixme + assert( + false + && "Joint Mimic is not supported for aba yet. Remove it from your model if you want to use " + "this function"); m_jmodel_ref.calc_aba( data.m_jdata_ref, armature, PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I), update_I); } static std::string classname() { - return std::string("JointModelMimic<") + JointModel::classname() + std::string(">"); - ; + return std::string("JointModelMimic"); } std::string shortname() const { - return std::string("JointModelMimic<") + m_jmodel_ref.shortname() + std::string(">"); + return classname(); } /// \returns An expression of *this with the Scalar type casted to NewScalar. template - typename CastType::type cast() const + typename CastType::type cast() const { - typedef typename CastType::type ReturnType; + typedef typename CastType::type ReturnType; ReturnType res( - m_jmodel_ref.template cast(), pinocchio::cast(m_scaling), - pinocchio::cast(m_offset)); - res.setIndexes(id(), idx_q(), idx_v()); + m_jmodel_ref.template cast(), ScalarCast::cast(m_scaling), + ScalarCast::cast(m_offset)); + res.setIndexes(id(), idx_q(), idx_v(), idx_j()); return res; } @@ -753,94 +848,106 @@ namespace pinocchio // Const access template typename SizeDepType::template SegmentReturn::ConstType - jointConfigSelector_impl(const Eigen::MatrixBase & a) const + jointConfigFromDofSelector_impl(const Eigen::MatrixBase & a) const { - return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_q(), m_jmodel_ref.nq()); + return SizeDepType::segment(a.derived(), idx_q(), m_jmodel_ref.nq()); } // Non-const access template typename SizeDepType::template SegmentReturn::Type - jointConfigSelector_impl(Eigen::MatrixBase & a) const - { - return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_q(), m_jmodel_ref.nq()); - } - - /* Acces to dedicated segment in robot config velocity space. */ - // Const access - template - typename SizeDepType::template SegmentReturn::ConstType - jointVelocitySelector_impl(const Eigen::MatrixBase & a) const + jointConfigFromDofSelector_impl(Eigen::MatrixBase & a) const { - return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); - } - - // Non-const access - template - typename SizeDepType::template SegmentReturn::Type - jointVelocitySelector_impl(Eigen::MatrixBase & a) const - { - return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); + return SizeDepType::segment(a.derived(), idx_q(), m_jmodel_ref.nq()); } /* Acces to dedicated columns in a ForceSet or MotionSet matrix.*/ // Const access template typename SizeDepType::template ColsReturn::ConstType - jointCols_impl(const Eigen::MatrixBase & A) const + jointVelCols_impl(const Eigen::MatrixBase & A) const { - return SizeDepType::middleCols(A.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); + return SizeDepType::middleCols(A.derived(), idx_v(), m_jmodel_ref.nv()); } // Non-const access template typename SizeDepType::template ColsReturn::Type - jointCols_impl(Eigen::MatrixBase & A) const + jointVelCols_impl(Eigen::MatrixBase & A) const { - return SizeDepType::middleCols(A.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); + return SizeDepType::middleCols(A.derived(), idx_v(), m_jmodel_ref.nv()); } /* Acces to dedicated rows in a matrix.*/ // Const access template typename SizeDepType::template RowsReturn::ConstType - jointRows_impl(const Eigen::MatrixBase & A) const + joinVelRows_impl(const Eigen::MatrixBase & A) const { - return SizeDepType::middleRows(A.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); + return SizeDepType::middleRows(A.derived(), idx_v(), m_jmodel_ref.nv()); } // Non-const access template typename SizeDepType::template RowsReturn::Type - jointRows_impl(Eigen::MatrixBase & A) const + jointVelRows_impl(Eigen::MatrixBase & A) const { - return SizeDepType::middleRows(A.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); + return SizeDepType::middleRows(A.derived(), idx_v(), m_jmodel_ref.nv()); } - /// \brief Returns a block of dimension nv()xnv() located at position idx_v(),idx_v() in the - /// matrix Mat - // Const access + // /// \brief Returns a block of dimension nv()xnv() located at position idx_v(),idx_v() in the + // matrix Mat + // // Const access template typename SizeDepType::template BlockReturn::ConstType - jointBlock_impl(const Eigen::MatrixBase & Mat) const + jointVelBlock_impl(const Eigen::MatrixBase & Mat) const { return SizeDepType::block( - Mat.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv(), - m_jmodel_ref.nv()); + Mat.derived(), idx_v(), idx_v(), m_jmodel_ref.nv(), m_jmodel_ref.nv()); } // Non-const access template typename SizeDepType::template BlockReturn::Type - jointBlock_impl(Eigen::MatrixBase & Mat) const + jointVelBlock_impl(Eigen::MatrixBase & Mat) const { return SizeDepType::block( - Mat.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv(), - m_jmodel_ref.nv()); + Mat.derived(), idx_v(), idx_v(), m_jmodel_ref.nv(), m_jmodel_ref.nv()); } - }; // struct JointModelMimic + }; // struct JointModelMimicTpl } // namespace pinocchio +#include + +namespace boost +{ + template class JointCollectionTpl> + struct has_nothrow_constructor< + ::pinocchio::JointModelMimicTpl> + : public integral_constant + { + }; + + template class JointCollectionTpl> + struct has_nothrow_copy<::pinocchio::JointModelMimicTpl> + : public integral_constant + { + }; + + template class JointCollectionTpl> + struct has_nothrow_constructor< + ::pinocchio::JointDataMimicTpl> + : public integral_constant + { + }; + + template class JointCollectionTpl> + struct has_nothrow_copy<::pinocchio::JointDataMimicTpl> + : public integral_constant + { + }; +} // namespace boost + #endif // ifndef __pinocchio_multibody_joint_mimic_hpp__ diff --git a/include/pinocchio/multibody/joint/joint-model-base.hpp b/include/pinocchio/multibody/joint/joint-model-base.hpp index da7164dc96..e527f6a99c 100644 --- a/include/pinocchio/multibody/joint/joint-model-base.hpp +++ b/include/pinocchio/multibody/joint/joint-model-base.hpp @@ -25,11 +25,13 @@ typedef TYPENAME traits::U_t U_t; \ typedef TYPENAME traits::D_t D_t; \ typedef TYPENAME traits::UD_t UD_t; \ + typedef TYPENAME traits::is_mimicable_t is_mimicable_t; \ enum \ { \ Options = traits::Options, \ NQ = traits::NQ, \ - NV = traits::NV \ + NV = traits::NV, \ + NJ = traits::NJ \ }; \ typedef TYPENAME traits::ConfigVector_t ConfigVector_t; \ typedef TYPENAME traits::TangentVector_t TangentVector_t @@ -59,7 +61,8 @@ #define PINOCCHIO_JOINT_USE_INDEXES(Joint) \ typedef JointModelBase Base; \ using Base::idx_q; \ - using Base::idx_v + using Base::idx_v; \ + using Base::idx_j #define PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelTpl) \ template \ @@ -146,6 +149,10 @@ namespace pinocchio { return derived().nq_impl(); } + int nj() const + { + return derived().nj_impl(); + } // Default _impl methods are reimplemented by dynamic-size joints. int nv_impl() const @@ -156,6 +163,10 @@ namespace pinocchio { return NQ; } + int nj_impl() const + { + return NJ; + } int idx_q() const { @@ -165,6 +176,10 @@ namespace pinocchio { return derived().idx_v_impl(); } + int idx_j() const + { + return derived().idx_j_impl(); + } JointIndex id() const { return derived().id_impl(); @@ -178,20 +193,25 @@ namespace pinocchio { return i_v; } + int idx_j_impl() const + { + return i_j; + } JointIndex id_impl() const { return i_id; } - void setIndexes(JointIndex id, int q, int v) + void setIndexes(JointIndex id, int q, int v, int j) { - derived().setIndexes_impl(id, q, v); + derived().setIndexes_impl(id, q, v, j); } - void setIndexes_impl(JointIndex id, int q, int v) + void setIndexes_impl(JointIndex id, int q, int v, int j) { i_id = id, i_q = q; i_v = v; + i_j = j; } void disp(std::ostream & os) const @@ -201,8 +221,10 @@ namespace pinocchio << " index: " << id() << endl << " index q: " << idx_q() << endl << " index v: " << idx_v() << endl + << " index j: " << idx_j() << endl << " nq: " << nq() << endl - << " nv: " << nv() << endl; + << " nv: " << nv() << endl + << " nj: " << nj() << endl; } friend std::ostream & operator<<(std::ostream & os, const JointModelBase & joint) @@ -254,21 +276,52 @@ namespace pinocchio { return internal::comparison_eq(other.id(), id()) && internal::comparison_eq(other.idx_q(), idx_q()) - && internal::comparison_eq(other.idx_v(), idx_v()); + && internal::comparison_eq(other.idx_v(), idx_v()) + && internal::comparison_eq(other.idx_j(), idx_j()); } /* Acces to dedicated segment in robot config space. */ // Const access template typename SizeDepType::template SegmentReturn::ConstType - jointConfigSelector(const Eigen::MatrixBase & a) const + jointConfigFromDofSelector(const Eigen::MatrixBase & a) const + { + return derived().jointConfigFromDofSelector_impl(a); + } + + template + typename SizeDepType::template SegmentReturn::ConstType + jointConfigFromDofSelector_impl(const Eigen::MatrixBase & a) const + { + return SizeDepType::segment(a.derived(), idx_q(), nq()); + } + + // Non-const access + template + typename SizeDepType::template SegmentReturn::Type + jointConfigFromDofSelector(Eigen::MatrixBase & a) const + { + return derived().jointConfigFromDofSelector_impl(a); + } + + template + typename SizeDepType::template SegmentReturn::Type + jointConfigFromDofSelector_impl(Eigen::MatrixBase & a) const + { + return SizeDepType::segment(a, idx_q(), nq()); + } + + // Const access + template + typename SizeDepType::template SegmentReturn::ConstType + jointConfigFromNqSelector(const Eigen::MatrixBase & a) const { - return derived().jointConfigSelector_impl(a); + return derived().jointConfigFromNqSelector_impl(a); } template typename SizeDepType::template SegmentReturn::ConstType - jointConfigSelector_impl(const Eigen::MatrixBase & a) const + jointConfigFromNqSelector_impl(const Eigen::MatrixBase & a) const { return SizeDepType::segment(a.derived(), idx_q(), nq()); } @@ -276,14 +329,14 @@ namespace pinocchio // Non-const access template typename SizeDepType::template SegmentReturn::Type - jointConfigSelector(Eigen::MatrixBase & a) const + jointConfigFromNqSelector(Eigen::MatrixBase & a) const { - return derived().jointConfigSelector_impl(a); + return derived().jointConfigFromNqSelector_impl(a); } template typename SizeDepType::template SegmentReturn::Type - jointConfigSelector_impl(Eigen::MatrixBase & a) const + jointConfigFromNqSelector_impl(Eigen::MatrixBase & a) const { return SizeDepType::segment(a, idx_q(), nq()); } @@ -292,14 +345,44 @@ namespace pinocchio // Const access template typename SizeDepType::template SegmentReturn::ConstType - jointVelocitySelector(const Eigen::MatrixBase & a) const + jointVelocityFromDofSelector(const Eigen::MatrixBase & a) const + { + return derived().jointVelocityFromDofSelector_impl(a.derived()); + } + + template + typename SizeDepType::template SegmentReturn::ConstType + jointVelocityFromDofSelector_impl(const Eigen::MatrixBase & a) const + { + return SizeDepType::segment(a.derived(), idx_v(), nj()); + } + + // Non-const access + template + typename SizeDepType::template SegmentReturn::Type + jointVelocityFromDofSelector(Eigen::MatrixBase & a) const + { + return derived().jointVelocityFromDofSelector_impl(a.derived()); + } + + template + typename SizeDepType::template SegmentReturn::Type + jointVelocityFromDofSelector_impl(Eigen::MatrixBase & a) const + { + return SizeDepType::segment(a.derived(), idx_v(), nj()); + } + + // Const access + template + typename SizeDepType::template SegmentReturn::ConstType + jointVelocityFromNvSelector(const Eigen::MatrixBase & a) const { - return derived().jointVelocitySelector_impl(a.derived()); + return derived().jointVelocityFromNvSelector_impl(a.derived()); } template typename SizeDepType::template SegmentReturn::ConstType - jointVelocitySelector_impl(const Eigen::MatrixBase & a) const + jointVelocityFromNvSelector_impl(const Eigen::MatrixBase & a) const { return SizeDepType::segment(a.derived(), idx_v(), nv()); } @@ -307,14 +390,14 @@ namespace pinocchio // Non-const access template typename SizeDepType::template SegmentReturn::Type - jointVelocitySelector(Eigen::MatrixBase & a) const + jointVelocityFromNvSelector(Eigen::MatrixBase & a) const { - return derived().jointVelocitySelector_impl(a.derived()); + return derived().jointVelocityFromNvSelector_impl(a.derived()); } template typename SizeDepType::template SegmentReturn::Type - jointVelocitySelector_impl(Eigen::MatrixBase & a) const + jointVelocityFromNvSelector_impl(Eigen::MatrixBase & a) const { return SizeDepType::segment(a.derived(), idx_v(), nv()); } @@ -323,94 +406,181 @@ namespace pinocchio // Const access template typename SizeDepType::template ColsReturn::ConstType - jointCols(const Eigen::MatrixBase & A) const + jointVelCols(const Eigen::MatrixBase & A) const { - return derived().jointCols_impl(A.derived()); + return derived().jointVelCols_impl(A.derived()); + } + + template + typename SizeDepType::template ColsReturn::ConstType + jointJacCols(const Eigen::MatrixBase & A) const + { + return derived().jointJacCols_impl(A.derived()); } template typename SizeDepType::template ColsReturn::ConstType - jointCols_impl(const Eigen::MatrixBase & A) const + jointVelCols_impl(const Eigen::MatrixBase & A) const { return SizeDepType::middleCols(A.derived(), idx_v(), nv()); } + template + typename SizeDepType::template ColsReturn::ConstType + jointJacCols_impl(const Eigen::MatrixBase & A) const + { + return SizeDepType::middleCols(A.derived(), idx_j(), nj()); + } + // Non-const access + // TODO rename Jac/Vel into Full/Red (for full system and reduced system ?) + template + typename SizeDepType::template ColsReturn::Type + jointVelCols(Eigen::MatrixBase & A) const + { + return derived().jointVelCols_impl(A.derived()); + } + template - typename SizeDepType::template ColsReturn::Type jointCols(Eigen::MatrixBase & A) const + typename SizeDepType::template ColsReturn::Type + jointJacCols(Eigen::MatrixBase & A) const { - return derived().jointCols_impl(A.derived()); + return derived().jointJacCols_impl(A.derived()); } template typename SizeDepType::template ColsReturn::Type - jointCols_impl(Eigen::MatrixBase & A) const + jointVelCols_impl(Eigen::MatrixBase & A) const { return SizeDepType::middleCols(A.derived(), idx_v(), nv()); } + template + typename SizeDepType::template ColsReturn::Type + jointJacCols_impl(Eigen::MatrixBase & A) const + { + return SizeDepType::middleCols(A.derived(), idx_j(), nj()); + } + /* Acces to dedicated rows in a matrix.*/ // Const access template typename SizeDepType::template RowsReturn::ConstType - jointRows(const Eigen::MatrixBase & A) const + jointVelRows(const Eigen::MatrixBase & A) const { - return derived().jointRows_impl(A.derived()); + return derived().jointVelRows_impl(A.derived()); + } + + template + typename SizeDepType::template RowsReturn::ConstType + jointJacRows(const Eigen::MatrixBase & A) const + { + return derived().jointJacRows_impl(A.derived()); } template typename SizeDepType::template RowsReturn::ConstType - jointRows_impl(const Eigen::MatrixBase & A) const + jointVelRows_impl(const Eigen::MatrixBase & A) const { return SizeDepType::middleRows(A.derived(), idx_v(), nv()); } + template + typename SizeDepType::template RowsReturn::ConstType + jointJacRows_impl(const Eigen::MatrixBase & A) const + { + return SizeDepType::middleRows(A.derived(), idx_j(), nj()); + } + // Non-const access template - typename SizeDepType::template RowsReturn::Type jointRows(Eigen::MatrixBase & A) const + typename SizeDepType::template RowsReturn::Type + jointVelRows(Eigen::MatrixBase & A) const { - return derived().jointRows_impl(A.derived()); + return derived().jointVelRows_impl(A.derived()); + } + + template + typename SizeDepType::template RowsReturn::Type + jointJacRows(Eigen::MatrixBase & A) const + { + return derived().jointJacRows_impl(A.derived()); } template typename SizeDepType::template RowsReturn::Type - jointRows_impl(Eigen::MatrixBase & A) const + jointVelRows_impl(Eigen::MatrixBase & A) const { return SizeDepType::middleRows(A.derived(), idx_v(), nv()); } + template + typename SizeDepType::template RowsReturn::Type + jointJacRows_impl(Eigen::MatrixBase & A) const + { + return SizeDepType::middleRows(A.derived(), idx_j(), nj()); + } + /// \brief Returns a block of dimension nv()xnv() located at position idx_v(),idx_v() in the /// matrix Mat // Const access template typename SizeDepType::template BlockReturn::ConstType - jointBlock(const Eigen::MatrixBase & Mat) const + jointVelBlock(const Eigen::MatrixBase & Mat) const + { + return derived().jointVelBlock_impl(Mat.derived()); + } + + template + typename SizeDepType::template BlockReturn::ConstType + jointJacBlock(const Eigen::MatrixBase & Mat) const { - return derived().jointBlock_impl(Mat.derived()); + return derived().jointJacBlock_impl(Mat.derived()); } template typename SizeDepType::template BlockReturn::ConstType - jointBlock_impl(const Eigen::MatrixBase & Mat) const + jointVelBlock_impl(const Eigen::MatrixBase & Mat) const { return SizeDepType::block(Mat.derived(), idx_v(), idx_v(), nv(), nv()); } + template + typename SizeDepType::template BlockReturn::ConstType + jointJacBlock_impl(const Eigen::MatrixBase & Mat) const + { + return SizeDepType::block(Mat.derived(), idx_j(), idx_j(), nj(), nj()); + } + // Non-const access template typename SizeDepType::template BlockReturn::Type - jointBlock(Eigen::MatrixBase & Mat) const + jointVelBlock(Eigen::MatrixBase & Mat) const { - return derived().jointBlock_impl(Mat.derived()); + return derived().jointVelBlock_impl(Mat.derived()); + } + + template + typename SizeDepType::template BlockReturn::Type + jointJacBlock(Eigen::MatrixBase & Mat) const + { + return derived().jointJacBlock_impl(Mat.derived()); } template typename SizeDepType::template BlockReturn::Type - jointBlock_impl(Eigen::MatrixBase & Mat) const + jointVelBlock_impl(Eigen::MatrixBase & Mat) const { return SizeDepType::block(Mat.derived(), idx_v(), idx_v(), nv(), nv()); } + template + typename SizeDepType::template BlockReturn::Type + jointJacBlock_impl(Eigen::MatrixBase & Mat) const + { + return SizeDepType::block(Mat.derived(), idx_j(), idx_j(), nj(), nj()); + } + protected: /// Default constructor: protected. /// @@ -419,6 +589,7 @@ namespace pinocchio : i_id(std::numeric_limits::max()) , i_q(-1) , i_v(-1) + , i_j(-1) { } @@ -440,6 +611,7 @@ namespace pinocchio i_id = clone.i_id; i_q = clone.i_q; i_v = clone.i_v; + i_j = clone.i_j; return *this; } @@ -447,6 +619,7 @@ namespace pinocchio JointIndex i_id; // ID of the joint in the multibody list. int i_q; // Index of the joint configuration in the joint configuration vector. int i_v; // Index of the joint velocity in the joint velocity vector. + int i_j; // Index of the joint jacobian in the joint jacobian matrix. }; // struct JointModelBase diff --git a/include/pinocchio/multibody/joint/joint-planar.hpp b/include/pinocchio/multibody/joint/joint-planar.hpp index 57e529454f..96e7c56f3c 100644 --- a/include/pinocchio/multibody/joint/joint-planar.hpp +++ b/include/pinocchio/multibody/joint/joint-planar.hpp @@ -471,7 +471,8 @@ namespace pinocchio enum { NQ = 4, - NV = 3 + NV = 3, + NJ = 3 }; enum { @@ -493,6 +494,8 @@ namespace pinocchio typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + typedef boost::mpl::false_ is_mimicable_t; + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; @@ -564,6 +567,7 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; + using Base::idx_j; using Base::idx_q; using Base::idx_v; using Base::setIndexes; @@ -672,12 +676,17 @@ namespace pinocchio { typedef JointModelPlanarTpl ReturnType; ReturnType res; - res.setIndexes(id(), idx_q(), idx_v()); + res.setIndexes(id(), idx_q(), idx_v(), idx_j()); return res; } }; // struct JointModelPlanarTpl + template + struct ConfigVectorAffineTransform> + { + typedef LinearAffineTransform Type; + }; } // namespace pinocchio #include diff --git a/include/pinocchio/multibody/joint/joint-prismatic-unaligned.hpp b/include/pinocchio/multibody/joint/joint-prismatic-unaligned.hpp index dd3f45e13e..614abb07b1 100644 --- a/include/pinocchio/multibody/joint/joint-prismatic-unaligned.hpp +++ b/include/pinocchio/multibody/joint/joint-prismatic-unaligned.hpp @@ -478,7 +478,8 @@ namespace pinocchio enum { NQ = 1, - NV = 1 + NV = 1, + NJ = 1 }; typedef _Scalar Scalar; enum @@ -500,6 +501,8 @@ namespace pinocchio typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + typedef boost::mpl::true_ is_mimicable_t; + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; @@ -589,6 +592,7 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; + using Base::idx_j; using Base::idx_q; using Base::idx_v; using Base::setIndexes; @@ -694,7 +698,7 @@ namespace pinocchio { typedef JointModelPrismaticUnalignedTpl ReturnType; ReturnType res(axis.template cast()); - res.setIndexes(id(), idx_q(), idx_v()); + res.setIndexes(id(), idx_q(), idx_v(), idx_j()); return res; } @@ -706,6 +710,11 @@ namespace pinocchio Vector3 axis; }; // struct JointModelPrismaticUnalignedTpl + template + struct ConfigVectorAffineTransform> + { + typedef LinearAffineTransform Type; + }; } // namespace pinocchio #include diff --git a/include/pinocchio/multibody/joint/joint-prismatic.hpp b/include/pinocchio/multibody/joint/joint-prismatic.hpp index 2a8233cbfc..08c33a63c8 100644 --- a/include/pinocchio/multibody/joint/joint-prismatic.hpp +++ b/include/pinocchio/multibody/joint/joint-prismatic.hpp @@ -570,7 +570,8 @@ namespace pinocchio enum { NQ = 1, - NV = 1 + NV = 1, + NJ = 1 }; typedef _Scalar Scalar; enum @@ -592,6 +593,8 @@ namespace pinocchio typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + typedef boost::mpl::true_ is_mimicable_t; + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; @@ -671,6 +674,7 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; + using Base::idx_j; using Base::idx_q; using Base::idx_v; using Base::setIndexes; @@ -766,7 +770,7 @@ namespace pinocchio { typedef JointModelPrismaticTpl ReturnType; ReturnType res; - res.setIndexes(id(), idx_q(), idx_v()); + res.setIndexes(id(), idx_q(), idx_v(), idx_j()); return res; } @@ -784,6 +788,11 @@ namespace pinocchio typedef JointDataPrismaticTpl JointDataPZ; typedef JointModelPrismaticTpl JointModelPZ; + template + struct ConfigVectorAffineTransform> + { + typedef LinearAffineTransform Type; + }; } // namespace pinocchio #include diff --git a/include/pinocchio/multibody/joint/joint-revolute-unaligned.hpp b/include/pinocchio/multibody/joint/joint-revolute-unaligned.hpp index a5401bd4e1..eb039f9a3e 100644 --- a/include/pinocchio/multibody/joint/joint-revolute-unaligned.hpp +++ b/include/pinocchio/multibody/joint/joint-revolute-unaligned.hpp @@ -503,7 +503,8 @@ namespace pinocchio enum { NQ = 1, - NV = 1 + NV = 1, + NJ = 1 }; typedef _Scalar Scalar; enum @@ -525,6 +526,8 @@ namespace pinocchio typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + typedef boost::mpl::true_ is_mimicable_t; + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; @@ -615,6 +618,7 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; + using Base::idx_j; using Base::idx_q; using Base::idx_v; using Base::setIndexes; @@ -718,7 +722,7 @@ namespace pinocchio { typedef JointModelRevoluteUnalignedTpl ReturnType; ReturnType res(axis.template cast()); - res.setIndexes(id(), idx_q(), idx_v()); + res.setIndexes(id(), idx_q(), idx_v(), idx_j()); return res; } @@ -730,6 +734,11 @@ namespace pinocchio Vector3 axis; }; // struct JointModelRevoluteUnalignedTpl + template + struct ConfigVectorAffineTransform> + { + typedef LinearAffineTransform Type; + }; } // namespace pinocchio #include diff --git a/include/pinocchio/multibody/joint/joint-revolute-unbounded-unaligned.hpp b/include/pinocchio/multibody/joint/joint-revolute-unbounded-unaligned.hpp index 31c5836d00..8211eaa935 100644 --- a/include/pinocchio/multibody/joint/joint-revolute-unbounded-unaligned.hpp +++ b/include/pinocchio/multibody/joint/joint-revolute-unbounded-unaligned.hpp @@ -24,7 +24,8 @@ namespace pinocchio enum { NQ = 2, - NV = 1 + NV = 1, + NJ = 1 }; typedef _Scalar Scalar; enum @@ -48,6 +49,8 @@ namespace pinocchio typedef Eigen::Matrix D_t; typedef Eigen::Matrix UD_t; + typedef boost::mpl::true_ is_mimicable_t; + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; @@ -139,6 +142,7 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; + using Base::idx_j; using Base::idx_q; using Base::idx_v; using Base::setIndexes; @@ -246,7 +250,7 @@ namespace pinocchio { typedef JointModelRevoluteUnboundedUnalignedTpl ReturnType; ReturnType res(axis.template cast()); - res.setIndexes(id(), idx_q(), idx_v()); + res.setIndexes(id(), idx_q(), idx_v(), idx_j()); return res; } @@ -257,6 +261,11 @@ namespace pinocchio Vector3 axis; }; // struct JointModelRevoluteUnboundedUnalignedTpl + template + struct ConfigVectorAffineTransform> + { + typedef UnboundedRevoluteAffineTransform Type; + }; } // namespace pinocchio #include diff --git a/include/pinocchio/multibody/joint/joint-revolute-unbounded.hpp b/include/pinocchio/multibody/joint/joint-revolute-unbounded.hpp index fb635e4319..619ae467c2 100644 --- a/include/pinocchio/multibody/joint/joint-revolute-unbounded.hpp +++ b/include/pinocchio/multibody/joint/joint-revolute-unbounded.hpp @@ -23,7 +23,8 @@ namespace pinocchio enum { NQ = 2, - NV = 1 + NV = 1, + NJ = 1 }; typedef _Scalar Scalar; enum @@ -45,6 +46,8 @@ namespace pinocchio typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + typedef boost::mpl::true_ is_mimicable_t; + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; @@ -125,6 +128,7 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; + using Base::idx_j; using Base::idx_q; using Base::idx_v; using Base::setIndexes; @@ -224,41 +228,12 @@ namespace pinocchio { typedef JointModelRevoluteUnboundedTpl ReturnType; ReturnType res; - res.setIndexes(id(), idx_q(), idx_v()); + res.setIndexes(id(), idx_q(), idx_v(), idx_j()); return res; } }; // struct JointModelRevoluteUnboundedTpl - struct UnboundedRevoluteAffineTransform - { - template - static void run( - const Eigen::MatrixBase & q, - const Scalar & scaling, - const Scalar & offset, - const Eigen::MatrixBase & dest) - { - EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(ConfigVectorIn, 2); - EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(ConfigVectorOut, 2); - - const typename ConfigVectorIn::Scalar & ca = q(0); - const typename ConfigVectorIn::Scalar & sa = q(1); - - const typename ConfigVectorIn::Scalar & theta = math::atan2(sa, ca); - const typename ConfigVectorIn::Scalar & theta_transform = scaling * theta + offset; - - ConfigVectorOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, dest); - SINCOS(theta_transform, &dest_.coeffRef(1), &dest_.coeffRef(0)); - } - }; - - template - struct ConfigVectorAffineTransform> - { - typedef UnboundedRevoluteAffineTransform Type; - }; - typedef JointRevoluteUnboundedTpl JointRUBX; typedef JointDataRevoluteUnboundedTpl JointDataRUBX; typedef JointModelRevoluteUnboundedTpl JointModelRUBX; @@ -271,6 +246,11 @@ namespace pinocchio typedef JointDataRevoluteUnboundedTpl JointDataRUBZ; typedef JointModelRevoluteUnboundedTpl JointModelRUBZ; + template + struct ConfigVectorAffineTransform> + { + typedef UnboundedRevoluteAffineTransform Type; + }; } // namespace pinocchio #include diff --git a/include/pinocchio/multibody/joint/joint-revolute.hpp b/include/pinocchio/multibody/joint/joint-revolute.hpp index 9eaeeebe62..7be27ad828 100644 --- a/include/pinocchio/multibody/joint/joint-revolute.hpp +++ b/include/pinocchio/multibody/joint/joint-revolute.hpp @@ -659,7 +659,8 @@ namespace pinocchio enum { NQ = 1, - NV = 1 + NV = 1, + NJ = 1 }; typedef _Scalar Scalar; enum @@ -681,6 +682,8 @@ namespace pinocchio typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + typedef boost::mpl::true_ is_mimicable_t; + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; @@ -759,6 +762,7 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; + using Base::idx_j; using Base::idx_q; using Base::idx_v; using Base::setIndexes; @@ -862,7 +866,7 @@ namespace pinocchio { typedef JointModelRevoluteTpl ReturnType; ReturnType res; - res.setIndexes(id(), idx_q(), idx_v()); + res.setIndexes(id(), idx_q(), idx_v(), idx_j()); return res; } @@ -880,6 +884,11 @@ namespace pinocchio typedef JointDataRevoluteTpl JointDataRZ; typedef JointModelRevoluteTpl JointModelRZ; + template + struct ConfigVectorAffineTransform> + { + typedef LinearAffineTransform Type; + }; } // namespace pinocchio #include diff --git a/include/pinocchio/multibody/joint/joint-spherical-ZYX.hpp b/include/pinocchio/multibody/joint/joint-spherical-ZYX.hpp index 46378a8f62..63bf442979 100644 --- a/include/pinocchio/multibody/joint/joint-spherical-ZYX.hpp +++ b/include/pinocchio/multibody/joint/joint-spherical-ZYX.hpp @@ -279,7 +279,8 @@ namespace pinocchio enum { NQ = 3, - NV = 3 + NV = 3, + NJ = 3 }; typedef _Scalar Scalar; enum @@ -301,6 +302,8 @@ namespace pinocchio typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + typedef boost::mpl::false_ is_mimicable_t; + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; @@ -377,6 +380,7 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; + using Base::idx_j; using Base::idx_q; using Base::idx_v; using Base::setIndexes; @@ -500,7 +504,7 @@ namespace pinocchio { typedef JointModelSphericalZYXTpl ReturnType; ReturnType res; - res.setIndexes(id(), idx_q(), idx_v()); + res.setIndexes(id(), idx_q(), idx_v(), idx_j()); return res; } diff --git a/include/pinocchio/multibody/joint/joint-spherical.hpp b/include/pinocchio/multibody/joint/joint-spherical.hpp index 800696fa97..9ce0600f8b 100644 --- a/include/pinocchio/multibody/joint/joint-spherical.hpp +++ b/include/pinocchio/multibody/joint/joint-spherical.hpp @@ -382,7 +382,8 @@ namespace pinocchio enum { NQ = 4, - NV = 3 + NV = 3, + NJ = 3 }; typedef _Scalar Scalar; enum @@ -404,6 +405,8 @@ namespace pinocchio typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + typedef boost::mpl::false_ is_mimicable_t; + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; @@ -478,6 +481,7 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; + using Base::idx_j; using Base::idx_q; using Base::idx_v; using Base::setIndexes; @@ -600,7 +604,7 @@ namespace pinocchio { typedef JointModelSphericalTpl ReturnType; ReturnType res; - res.setIndexes(id(), idx_q(), idx_v()); + res.setIndexes(id(), idx_q(), idx_v(), idx_j()); return res; } diff --git a/include/pinocchio/multibody/joint/joint-translation.hpp b/include/pinocchio/multibody/joint/joint-translation.hpp index bf2b113f66..f4702ec097 100644 --- a/include/pinocchio/multibody/joint/joint-translation.hpp +++ b/include/pinocchio/multibody/joint/joint-translation.hpp @@ -479,7 +479,8 @@ namespace pinocchio enum { NQ = 3, - NV = 3 + NV = 3, + NJ = 3 }; typedef _Scalar Scalar; enum @@ -501,6 +502,8 @@ namespace pinocchio typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + typedef boost::mpl::false_ is_mimicable_t; + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; // traits JointTranslationTpl @@ -575,6 +578,7 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; + using Base::idx_j; using Base::idx_q; using Base::idx_v; using Base::setIndexes; @@ -597,7 +601,7 @@ namespace pinocchio template void calc(JointDataDerived & data, const typename Eigen::MatrixBase & qs) const { - data.joint_q = this->jointConfigSelector(qs); + data.joint_q = this->jointConfigFromDofSelector(qs); data.M.translation() = data.joint_q; } @@ -606,7 +610,7 @@ namespace pinocchio calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase & vs) const { - data.joint_v = this->jointVelocitySelector(vs); + data.joint_v = this->jointVelocityFromDofSelector(vs); data.v.linear() = data.joint_v; } @@ -618,7 +622,7 @@ namespace pinocchio { calc(data, qs.derived()); - data.joint_v = this->jointVelocitySelector(vs); + data.joint_v = this->jointVelocityFromDofSelector(vs); data.v.linear() = data.joint_v; } @@ -657,12 +661,17 @@ namespace pinocchio { typedef JointModelTranslationTpl ReturnType; ReturnType res; - res.setIndexes(id(), idx_q(), idx_v()); + res.setIndexes(id(), idx_q(), idx_v(), idx_j()); return res; } }; // struct JointModelTranslationTpl + template + struct ConfigVectorAffineTransform> + { + typedef LinearAffineTransform Type; + }; } // namespace pinocchio #include diff --git a/include/pinocchio/multibody/joint/joint-universal.hpp b/include/pinocchio/multibody/joint/joint-universal.hpp index d22e435e74..54e797a1ab 100644 --- a/include/pinocchio/multibody/joint/joint-universal.hpp +++ b/include/pinocchio/multibody/joint/joint-universal.hpp @@ -313,7 +313,8 @@ namespace pinocchio enum { NQ = 2, - NV = 2 + NV = 2, + NJ = 2 }; typedef _Scalar Scalar; enum @@ -335,6 +336,8 @@ namespace pinocchio typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + typedef boost::mpl::false_ is_mimicable_t; + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; @@ -411,6 +414,7 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; + using Base::idx_j; using Base::idx_q; using Base::idx_v; using Base::setIndexes; @@ -600,7 +604,7 @@ namespace pinocchio { typedef JointModelUniversalTpl ReturnType; ReturnType res(axis1.template cast(), axis2.template cast()); - res.setIndexes(id(), idx_q(), idx_v()); + res.setIndexes(id(), idx_q(), idx_v(), idx_j()); return res; } diff --git a/include/pinocchio/multibody/liegroup/liegroup-algo.hxx b/include/pinocchio/multibody/liegroup/liegroup-algo.hxx index 374d5aaea7..d5dfd2b6d2 100644 --- a/include/pinocchio/multibody/liegroup/liegroup-algo.hxx +++ b/include/pinocchio/multibody/liegroup/liegroup-algo.hxx @@ -197,8 +197,9 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.integrate( - jmodel.jointConfigSelector(q.derived()), jmodel.jointVelocitySelector(v.derived()), - jmodel.jointConfigSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, result))); + jmodel.jointConfigFromNqSelector(q.derived()), + jmodel.jointVelocityFromNvSelector(v.derived()), + jmodel.jointConfigFromNqSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, result))); } }; @@ -243,8 +244,9 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.dIntegrate( - jmodel.jointConfigSelector(q.derived()), jmodel.jointVelocitySelector(v.derived()), - jmodel.jointBlock(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType, mat)), arg, op); + jmodel.jointConfigFromNqSelector(q.derived()), + jmodel.jointVelocityFromNvSelector(v.derived()), + jmodel.jointJacBlock(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType, mat)), arg, op); } }; @@ -298,9 +300,9 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.dIntegrateTransport( - jmodel.jointConfigSelector(q.derived()), jmodel.jointVelocitySelector(v.derived()), - jmodel.jointRows(mat_in.derived()), - jmodel.jointRows(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixOutType, mat_out)), arg); + jmodel.jointConfigFromNqSelector(q.derived()), + jmodel.jointVelocityFromNvSelector(v.derived()), jmodel.jointJacRows(mat_in.derived()), + jmodel.jointJacRows(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixOutType, mat_out)), arg); } }; @@ -347,8 +349,9 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.dIntegrateTransport( - jmodel.jointConfigSelector(q.derived()), jmodel.jointVelocitySelector(v.derived()), - jmodel.jointRows(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType, mat)), arg); + jmodel.jointConfigFromNqSelector(q.derived()), + jmodel.jointVelocityFromNvSelector(v.derived()), + jmodel.jointJacRows(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType, mat)), arg); } }; @@ -391,8 +394,9 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.dDifference( - jmodel.jointConfigSelector(q0.derived()), jmodel.jointConfigSelector(q1.derived()), - jmodel.jointBlock(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix, mat)), arg); + jmodel.jointConfigFromNqSelector(q0.derived()), + jmodel.jointConfigFromNqSelector(q1.derived()), + jmodel.jointJacBlock(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix, mat)), arg); } }; @@ -437,8 +441,9 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.interpolate( - jmodel.jointConfigSelector(q0.derived()), jmodel.jointConfigSelector(q1.derived()), u, - jmodel.jointConfigSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, result))); + jmodel.jointConfigFromNqSelector(q0.derived()), + jmodel.jointConfigFromNqSelector(q1.derived()), u, + jmodel.jointConfigFromNqSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, result))); } }; @@ -477,8 +482,9 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.difference( - jmodel.jointConfigSelector(q0.derived()), jmodel.jointConfigSelector(q1.derived()), - jmodel.jointVelocitySelector(PINOCCHIO_EIGEN_CONST_CAST(TangentVectorOut, result))); + jmodel.jointConfigFromNqSelector(q0.derived()), + jmodel.jointConfigFromNqSelector(q1.derived()), + jmodel.jointVelocityFromNvSelector(PINOCCHIO_EIGEN_CONST_CAST(TangentVectorOut, result))); } }; @@ -521,7 +527,8 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; DistanceVectorOut & distances_ = PINOCCHIO_EIGEN_CONST_CAST(DistanceVectorOut, distances); distances_[(Eigen::DenseIndex)i] += lgo.squaredDistance( - jmodel.jointConfigSelector(q0.derived()), jmodel.jointConfigSelector(q1.derived())); + jmodel.jointConfigFromNqSelector(q0.derived()), + jmodel.jointConfigFromNqSelector(q1.derived())); } }; @@ -554,7 +561,8 @@ namespace pinocchio typedef typename Visitor::LieGroupMap LieGroupMap; typename LieGroupMap::template operation::type lgo; squaredDistance += lgo.squaredDistance( - jmodel.jointConfigSelector(q0.derived()), jmodel.jointConfigSelector(q1.derived())); + jmodel.jointConfigFromNqSelector(q0.derived()), + jmodel.jointConfigFromNqSelector(q1.derived())); } }; @@ -593,9 +601,9 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.randomConfiguration( - jmodel.jointConfigSelector(lowerLimits.derived()), - jmodel.jointConfigSelector(upperLimits.derived()), - jmodel.jointConfigSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, q))); + jmodel.jointConfigFromNqSelector(lowerLimits.derived()), + jmodel.jointConfigFromNqSelector(upperLimits.derived()), + jmodel.jointConfigFromNqSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, q))); } }; @@ -623,7 +631,8 @@ namespace pinocchio typedef typename Visitor::LieGroupMap LieGroupMap; typename LieGroupMap::template operation::type lgo; - lgo.normalize(jmodel.jointConfigSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorType, qout))); + lgo.normalize( + jmodel.jointConfigFromNqSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorType, qout))); } }; @@ -654,7 +663,7 @@ namespace pinocchio typedef typename Visitor::LieGroupMap LieGroupMap; typename LieGroupMap::template operation::type lgo; - res &= lgo.isNormalized(jmodel.jointConfigSelector(q.derived()), prec); + res &= lgo.isNormalized(jmodel.jointConfigFromNqSelector(q.derived()), prec); } }; @@ -690,7 +699,8 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; isSame &= lgo.isSameConfiguration( - jmodel.jointConfigSelector(q1.derived()), jmodel.jointConfigSelector(q2.derived()), prec); + jmodel.jointConfigFromNqSelector(q1.derived()), + jmodel.jointConfigFromNqSelector(q2.derived()), prec); } }; @@ -719,7 +729,7 @@ namespace pinocchio typedef typename Visitor::LieGroupMap LieGroupMap; typename LieGroupMap::template operation::type lgo; - jmodel.jointConfigSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorType, neutral_elt)) = + jmodel.jointConfigFromNqSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorType, neutral_elt)) = lgo.neutral(); } }; @@ -754,7 +764,7 @@ namespace pinocchio typedef typename LieGroupMap::template operation::type LieGroup; LieGroup lgo; lgo.integrateCoeffWiseJacobian( - jmodel.jointConfigSelector(q.derived()), + jmodel.jointConfigFromNqSelector(q.derived()), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix, jacobian) .template block( jmodel.idx_q(), jmodel.idx_v(), jmodel.nq(), jmodel.nv())); diff --git a/include/pinocchio/multibody/model.hpp b/include/pinocchio/multibody/model.hpp index 37a12fc81e..1936847e2f 100644 --- a/include/pinocchio/multibody/model.hpp +++ b/include/pinocchio/multibody/model.hpp @@ -100,6 +100,9 @@ namespace pinocchio /// \brief Dimension of the velocity vector space. int nv; + /// \brief Dimension of the jacobian space. + int nj; + /// \brief Number of joints. int njoints; @@ -130,6 +133,12 @@ namespace pinocchio /// \brief Dimension of the *i*th joint tangent subspace. std::vector nvs; + /// \brief Starting index of the *i*th joint in the jacobian space. + std::vector idx_js; + + /// \brief Dimension of the *i*th joint jacobian subspace. + std::vector njs; + /// \brief Vector of parent joint indexes. The parent of joint *i*, denoted *li*, corresponds to /// li==parents[i]. std::vector parents; @@ -200,6 +209,7 @@ namespace pinocchio ModelTpl() : nq(0) , nv(0) + , nj(0) , njoints(1) , nbodies(1) , nframes(0) @@ -210,6 +220,8 @@ namespace pinocchio , nqs(1, 0) , idx_vs(1, 0) , nvs(1, 0) + , idx_js(1, 0) + , njs(1, 0) , parents(1, 0) , children(1) , names(1) diff --git a/include/pinocchio/multibody/model.hxx b/include/pinocchio/multibody/model.hxx index f8bd3fcbe2..243c53f303 100644 --- a/include/pinocchio/multibody/model.hxx +++ b/include/pinocchio/multibody/model.hxx @@ -72,7 +72,7 @@ namespace pinocchio assert( (njoints == (int)joints.size()) && (njoints == (int)inertias.size()) && (njoints == (int)parents.size()) && (njoints == (int)jointPlacements.size())); - assert((joint_model.nq() >= 0) && (joint_model.nv() >= 0)); + assert((joint_model.nq() >= 0) && (joint_model.nv() >= 0) && (joint_model.nj() >= 0)); assert(joint_model.nq() >= joint_model.nv()); PINOCCHIO_CHECK_ARGUMENT_SIZE( @@ -97,15 +97,18 @@ namespace pinocchio joints.push_back(JointModel(joint_model.derived())); JointModel & jmodel = joints.back(); - jmodel.setIndexes(joint_id, nq, nv); + jmodel.setIndexes(joint_id, nq, nv, nj); const int joint_nq = jmodel.nq(); const int joint_idx_q = jmodel.idx_q(); const int joint_nv = jmodel.nv(); const int joint_idx_v = jmodel.idx_v(); + const int joint_nj = jmodel.nj(); + const int joint_idx_j = jmodel.idx_j(); assert(joint_idx_q >= 0); assert(joint_idx_v >= 0); + assert(joint_idx_j >= 0); inertias.push_back(Inertia::Zero()); parents.push_back(parent); @@ -120,29 +123,29 @@ namespace pinocchio nv += joint_nv; nvs.push_back(joint_nv); idx_vs.push_back(joint_idx_v); - - if (joint_nq > 0 && joint_nv > 0) - { - effortLimit.conservativeResize(nv); - jmodel.jointVelocitySelector(effortLimit) = max_effort; - velocityLimit.conservativeResize(nv); - jmodel.jointVelocitySelector(velocityLimit) = max_velocity; - lowerPositionLimit.conservativeResize(nq); - jmodel.jointConfigSelector(lowerPositionLimit) = min_config; - upperPositionLimit.conservativeResize(nq); - jmodel.jointConfigSelector(upperPositionLimit) = max_config; - - armature.conservativeResize(nv); - jmodel.jointVelocitySelector(armature).setZero(); - rotorInertia.conservativeResize(nv); - jmodel.jointVelocitySelector(rotorInertia).setZero(); - rotorGearRatio.conservativeResize(nv); - jmodel.jointVelocitySelector(rotorGearRatio).setOnes(); - friction.conservativeResize(nv); - jmodel.jointVelocitySelector(friction) = joint_friction; - damping.conservativeResize(nv); - jmodel.jointVelocitySelector(damping) = joint_damping; - } + nj += joint_nj; + njs.push_back(joint_nj); + idx_js.push_back(joint_idx_j); + + effortLimit.conservativeResize(nv); + jmodel.jointVelocityFromNvSelector(effortLimit) = max_effort; + velocityLimit.conservativeResize(nv); + jmodel.jointVelocityFromNvSelector(velocityLimit) = max_velocity; + lowerPositionLimit.conservativeResize(nq); + jmodel.jointConfigFromNqSelector(lowerPositionLimit) = min_config; + upperPositionLimit.conservativeResize(nq); + jmodel.jointConfigFromNqSelector(upperPositionLimit) = max_config; + + armature.conservativeResize(nv); + jmodel.jointVelocityFromNvSelector(armature).setZero(); + rotorInertia.conservativeResize(nv); + jmodel.jointVelocityFromNvSelector(rotorInertia).setZero(); + rotorGearRatio.conservativeResize(nv); + jmodel.jointVelocityFromNvSelector(rotorGearRatio).setOnes(); + friction.conservativeResize(nv); + jmodel.jointVelocityFromNvSelector(friction) = joint_friction; + damping.conservativeResize(nv); + jmodel.jointVelocityFromNvSelector(damping) = joint_damping; // Init and add joint index to its parent subtrees. subtrees.push_back(IndexVector(1)); @@ -230,6 +233,7 @@ namespace pinocchio ReturnType res; res.nq = nq; res.nv = nv; + res.nj = nj; res.njoints = njoints; res.nbodies = nbodies; res.nframes = nframes; @@ -245,7 +249,8 @@ namespace pinocchio res.nqs = nqs; res.idx_vs = idx_vs; res.nvs = nvs; - + res.idx_js = idx_js; + res.njs = njs; // Eigen Vectors res.armature = armature.template cast(); res.friction = friction.template cast(); @@ -289,12 +294,13 @@ namespace pinocchio template class JointCollectionTpl> bool ModelTpl::operator==(const ModelTpl & other) const { - bool res = other.nq == nq && other.nv == nv && other.njoints == njoints + bool res = other.nq == nq && other.nv == nv && other.nj == nj && other.njoints == njoints && other.nbodies == nbodies && other.nframes == nframes && other.parents == parents && other.children == children && other.names == names && other.subtrees == subtrees && other.gravity == gravity && other.name == name; - res &= other.idx_qs == idx_qs && other.nqs == nqs && other.idx_vs == idx_vs && other.nvs == nvs; + res &= other.idx_qs == idx_qs && other.nqs == nqs && other.idx_vs == idx_vs && other.nvs == nvs + && other.idx_js == idx_js && other.njs == njs; if (other.referenceConfigurations.size() != referenceConfigurations.size()) return false; diff --git a/include/pinocchio/multibody/sample-models.hpp b/include/pinocchio/multibody/sample-models.hpp index 58b088372e..f2a15060f1 100644 --- a/include/pinocchio/multibody/sample-models.hpp +++ b/include/pinocchio/multibody/sample-models.hpp @@ -18,7 +18,8 @@ namespace pinocchio * \param model: model, typically given empty, where the kinematic chain is added. */ template class JointCollectionTpl> - void manipulator(ModelTpl & model); + void + manipulator(ModelTpl & model, const bool mimic = false); #ifdef PINOCCHIO_WITH_HPP_FCL /** \brief Create the geometries on top of the kinematic model created by manipulator function. @@ -71,7 +72,10 @@ namespace pinocchio * This changes the size of the configuration space (33 vs 32). */ template class JointCollectionTpl> - void humanoidRandom(ModelTpl & model, bool usingFF = true); + void humanoidRandom( + ModelTpl & model, + bool usingFF = true, + bool mimic = false); } // namespace buildModels } // namespace pinocchio diff --git a/include/pinocchio/multibody/sample-models.hxx b/include/pinocchio/multibody/sample-models.hxx index 3b0ac01ff1..4fac645a23 100644 --- a/include/pinocchio/multibody/sample-models.hxx +++ b/include/pinocchio/multibody/sample-models.hxx @@ -57,6 +57,7 @@ namespace pinocchio template class JointCollectionTpl> static void addManipulator( ModelTpl & model, + const bool mimic = false, typename ModelTpl::JointIndex root_joint_idx = 0, const typename ModelTpl::SE3 & Mroot = ModelTpl::SE3::Identity(), @@ -104,19 +105,37 @@ namespace pinocchio model, typename JC::JointModelRX(), model.names[joint_id], pre + "wrist1", Marm); model.inertias[joint_id] = Ijoint; - joint_id = addJointAndBody( - model, typename JC::JointModelRY(), model.names[joint_id], pre + "wrist2", Id4); + if (mimic) + { + // Scalar multiplier = JC::JointModelRX::ConfigVector_t::Random(1)(0); + // Scalar offset = JC::JointModelRX::ConfigVector_t::Random(1)(0); + + Scalar multiplier = 2.5; + Scalar offset = 0.75; + joint_id = addJointAndBody( + model, + typename JC::JointModelMimic( + typename JC::JointModelRY(), model.joints[joint_id].derived(), multiplier, offset), + model.names[joint_id], pre + "wrist1_mimic", Id4); + } + else + { + joint_id = addJointAndBody( + model, typename JC::JointModelRY(), model.names[joint_id], pre + "wrist2", Id4); + } + model.inertias[joint_id] = Iarm; model.addBodyFrame(pre + "effector_body", joint_id); + const int nq = mimic ? 5 : 6; const JointModel & base_joint = model.joints[root_joint_id]; const int idx_q = base_joint.idx_q(); const int idx_v = base_joint.idx_v(); - model.lowerPositionLimit.template segment<6>(idx_q).fill(qmin); - model.upperPositionLimit.template segment<6>(idx_q).fill(qmax); - model.velocityLimit.template segment<6>(idx_v).fill(vmax); - model.effortLimit.template segment<6>(idx_v).fill(taumax); + model.lowerPositionLimit.segment(idx_q, nq).fill(qmin); + model.upperPositionLimit.segment(idx_q, nq).fill(qmax); + model.velocityLimit.segment(idx_v, nq).fill(vmax); + model.effortLimit.segment(idx_v, nq).fill(taumax); } #ifdef PINOCCHIO_WITH_HPP_FCL @@ -199,9 +218,9 @@ namespace pinocchio } // namespace details template class JointCollectionTpl> - void manipulator(ModelTpl & model) + void manipulator(ModelTpl & model, const bool mimic) { - details::addManipulator(model); + details::addManipulator(model, mimic); } #ifdef PINOCCHIO_WITH_HPP_FCL @@ -214,7 +233,8 @@ namespace pinocchio #endif template class JointCollectionTpl> - void humanoidRandom(ModelTpl & model, bool usingFF) + void + humanoidRandom(ModelTpl & model, bool usingFF, bool mimic) { typedef JointCollectionTpl JC; typedef ModelTpl Model; @@ -269,8 +289,22 @@ namespace pinocchio addJointAndBody(model, typename JC::JointModelRY(), "larm1_joint", "larm2"); addJointAndBody(model, typename JC::JointModelRZ(), "larm2_joint", "larm3"); addJointAndBody(model, typename JC::JointModelRY(), "larm3_joint", "larm4"); - addJointAndBody(model, typename JC::JointModelRY(), "larm4_joint", "larm5"); - addJointAndBody(model, typename JC::JointModelRX(), "larm5_joint", "larm6"); + Index joint_id = addJointAndBody(model, typename JC::JointModelRY(), "larm4_joint", "larm5"); + + if (mimic) + { + Scalar multiplier = 2.5; + Scalar offset = 0.75; + addJointAndBody( + model, + typename JC::JointModelMimic( + typename JC::JointModelRX(), model.joints[joint_id].derived(), multiplier, offset), + "larm5_joint", "larm6"); + } + else + { + addJointAndBody(model, typename JC::JointModelRX(), "larm5_joint", "larm6"); + } } template class JointCollectionTpl> @@ -315,11 +349,11 @@ namespace pinocchio /* --- Lower limbs --- */ details::addManipulator( - model, ffidx, + model, false, ffidx, SE3(details::rotate(pi, SE3::Vector3::UnitX()), typename SE3::Vector3(0, -0.2, -.1)), "rleg_"); details::addManipulator( - model, ffidx, + model, false, ffidx, SE3(details::rotate(pi, SE3::Vector3::UnitX()), typename SE3::Vector3(0, 0.2, -.1)), "lleg_"); @@ -359,11 +393,11 @@ namespace pinocchio /* --- Upper Limbs --- */ details::addManipulator( - model, chest, + model, false, chest, SE3(details::rotate(pi, SE3::Vector3::UnitX()), typename SE3::Vector3(0, -0.3, 1.)), "rarm_"); details::addManipulator( - model, chest, + model, false, chest, SE3(details::rotate(pi, SE3::Vector3::UnitX()), typename SE3::Vector3(0, 0.3, 1.)), "larm_"); } diff --git a/include/pinocchio/multibody/sample-models.txx b/include/pinocchio/multibody/sample-models.txx index 38a3d31940..39a108e264 100644 --- a/include/pinocchio/multibody/sample-models.txx +++ b/include/pinocchio/multibody/sample-models.txx @@ -12,14 +12,15 @@ namespace pinocchio namespace buildModels { extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI void - manipulator(context::Model &); + manipulator( + context::Model &, bool); extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI void humanoid(context::Model &, bool); extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI void humanoidRandom( - context::Model &, bool); + context::Model &, bool, bool); } // namespace buildModels } // namespace pinocchio diff --git a/include/pinocchio/parsers/urdf.hpp b/include/pinocchio/parsers/urdf.hpp index 14e3af6f9e..c3dfebfb99 100644 --- a/include/pinocchio/parsers/urdf.hpp +++ b/include/pinocchio/parsers/urdf.hpp @@ -33,6 +33,7 @@ namespace pinocchio /// \param[in] rootJoint The joint at the root of the model tree. /// \param[in] rootJointName Name of the rootJoint. /// \param[in] verbose Print parsing info. + /// \param[in] mimic Parsing joints as mimic or not. /// \param[out] model Reference model where to put the parsed information. /// \return Return the reference on argument model for convenience. /// @@ -42,6 +43,7 @@ namespace pinocchio const typename ModelTpl::JointModel & rootJoint, const std::string & rootJointName, ModelTpl & model, + const bool mimic = false, const bool verbose = false); /// @@ -50,6 +52,7 @@ namespace pinocchio /// /// \param[in] filename The URDF complete file path. /// \param[in] rootJoint The joint at the root of the model tree. + /// \param[in] mimic Parsing joints as mimic or not. /// \param[in] verbose Print parsing info. /// \param[out] model Reference model where to put the parsed information. /// \return Return the reference on argument model for convenience. @@ -59,12 +62,14 @@ namespace pinocchio const std::string & filename, const typename ModelTpl::JointModel & rootJoint, ModelTpl & model, + const bool mimic = true, const bool verbose = false); /// /// \brief Build the model from a URDF file with a fixed joint as root of the model tree. /// /// \param[in] filename The URDF complete file path. + /// \param[in] mimic Parsing joints as mimic or not. /// \param[in] verbose Print parsing info. /// \param[out] model Reference model where to put the parsed information. /// \return Return the reference on argument model for convenience. @@ -73,6 +78,7 @@ namespace pinocchio ModelTpl & buildModel( const std::string & filename, ModelTpl & model, + const bool mimic = true, const bool verbose = false); /// @@ -81,6 +87,7 @@ namespace pinocchio /// /// \param[in] urdfTree the tree build from the URDF /// \param[in] rootJoint The joint at the root of the model tree. + /// \param[in] mimic Parsing joints as mimic or not. /// \param[in] verbose Print parsing info. /// \param[out] model Reference model where to put the parsed information. /// \return Return the reference on argument model for convenience. @@ -92,6 +99,7 @@ namespace pinocchio const std::shared_ptr<::urdf::ModelInterface> urdfTree, const typename ModelTpl::JointModel & rootJoint, ModelTpl & model, + const bool mimic = true, const bool verbose = false); /// @@ -99,6 +107,7 @@ namespace pinocchio /// inside the model given as reference argument. /// /// \param[in] urdfTree the tree build from the URDF + /// \param[in] mimic Parsing joints as mimic or not. /// \param[in] rootJoint The joint at the root of the model tree. /// \param[in] rootJointName Name of the rootJoint. /// \param[in] verbose Print parsing info. @@ -113,12 +122,14 @@ namespace pinocchio const typename ModelTpl::JointModel & rootJoint, const std::string & rootJointName, ModelTpl & model, + const bool mimic = false, const bool verbose = false); /// /// \brief Build the model from a URDF model /// /// \param[in] urdfTree the tree build from the URDF + /// \param[in] mimic Parsing joints as mimic or not. /// \param[in] verbose Print parsing info. /// \param[out] model Reference model where to put the parsed information. /// \return Return the reference on argument model for convenience. @@ -129,6 +140,7 @@ namespace pinocchio ModelTpl & buildModel( const std::shared_ptr<::urdf::ModelInterface> urdfTree, ModelTpl & model, + const bool mimic = true, const bool verbose = false); /// @@ -138,6 +150,7 @@ namespace pinocchio /// \param[in] xml_stream stream containing the URDF model. /// \param[in] rootJoint The joint at the root of the model tree. /// \param[in] rootJointName Name of the rootJoint. + /// \param[in] mimic Parsing joints as mimic or not. /// \param[in] verbose Print parsing info. /// \param[out] model Reference model where to put the parsed information. /// \return Return the reference on argument model for convenience. @@ -150,6 +163,7 @@ namespace pinocchio const typename ModelTpl::JointModel & rootJoint, const std::string & rootJointName, ModelTpl & model, + const bool mimic = false, const bool verbose = false); /// @@ -159,6 +173,7 @@ namespace pinocchio /// \param[in] xml_stream stream containing the URDF model. /// \param[in] rootJoint The joint at the root of the model tree. /// \param[in] verbose Print parsing info. + /// \param[in] mimic Parsing joints as mimic or not. /// \param[out] model Reference model where to put the parsed information. /// \return Return the reference on argument model for convenience. /// @@ -169,12 +184,14 @@ namespace pinocchio const std::string & xml_stream, const typename ModelTpl::JointModel & rootJoint, ModelTpl & model, + const bool mimic = true, const bool verbose = false); /// /// \brief Build the model from an XML stream /// /// \param[in] xml_stream stream containing the URDF model. + /// \param[in] mimic Parsing joints as mimic or not. /// \param[in] verbose Print parsing info. /// \param[out] model Reference model where to put the parsed information. /// \return Return the reference on argument model for convenience. @@ -185,6 +202,7 @@ namespace pinocchio ModelTpl & buildModelFromXML( const std::string & xml_stream, ModelTpl & model, + const bool mimic = true, const bool verbose = false); /** diff --git a/include/pinocchio/parsers/urdf/model.hxx b/include/pinocchio/parsers/urdf/model.hxx index 824a33eaf5..5b6dd49992 100644 --- a/include/pinocchio/parsers/urdf/model.hxx +++ b/include/pinocchio/parsers/urdf/model.hxx @@ -15,6 +15,7 @@ #include #include #include +#include namespace pinocchio { @@ -24,6 +25,9 @@ namespace pinocchio { typedef double urdf_scalar_type; + template + struct MimicInfo; + template class UrdfVisitorBaseTpl { @@ -35,7 +39,8 @@ namespace pinocchio PRISMATIC, FLOATING, PLANAR, - SPHERICAL + SPHERICAL, + MIMIC }; typedef enum ::pinocchio::FrameType FrameType; @@ -108,6 +113,17 @@ namespace pinocchio virtual bool existFrame(const std::string & frame_name, const FrameType type) const = 0; + virtual void addMimicInfo( + const std::string & mimic_name, + const std::string & mimiced_name, + const Vector3 & axis, + const Scalar multiplier, + const Scalar offset, + JointType jointType) = 0; + + virtual void convertMimicJoint( + const std::string & mimic_name, const MimicInfo & mimic_info) = 0; + UrdfVisitorBaseTpl() : log(NULL) { @@ -122,6 +138,41 @@ namespace pinocchio } std::ostream * log; + std::unordered_map> mimicInfo_map; + }; + + template + struct MimicInfo + { + typedef _Scalar Scalar; + typedef Eigen::Matrix Vector3; + + Vector3 axis; + std::string mimiced_name; + Scalar multiplier; + Scalar offset; + + // Use the JointType from UrdfVisitorBaseTpl + typedef typename UrdfVisitorBaseTpl<_Scalar, Options>::JointType JointType; + JointType jointType; + + MimicInfo() + { + } + + MimicInfo( + std::string _mimiced_name, + Scalar _multiplier, + Scalar _offset, + Vector3 _axis, + JointType _jointType) + : mimiced_name(_mimiced_name) + , multiplier(_multiplier) + , offset(_offset) + , axis(_axis) + , jointType(_jointType) + { + } }; template class JointCollectionTpl> @@ -205,7 +256,6 @@ namespace pinocchio { JointIndex joint_id; const Frame & frame = model.frames[parentFrameId]; - switch (type) { case Base::FLOATING: @@ -250,6 +300,13 @@ namespace pinocchio frame.placement * placement, joint_name, max_effort, max_velocity, min_config, max_config, friction, damping); break; + case Base::MIMIC: + joint_id = model.addJoint( + frame.parentJoint, + typename JointCollection::JointModelMimic(model.joints.at(1), 1, 0), + frame.placement * placement, joint_name, max_effort, max_velocity, min_config, + max_config, friction, damping); + break; default: PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The joint type is not correct."); }; @@ -406,6 +463,85 @@ namespace pinocchio } } + void addMimicInfo( + const std::string & mimic_name, + const std::string & mimiced_name, + const Vector3 & axis, + const Scalar multiplier, + const Scalar offset, + JointType jointType) + { + MimicInfo mimic_info(mimiced_name, multiplier, offset, axis, jointType); + this->mimicInfo_map.insert(std::make_pair(mimic_name, mimic_info)); + } + + void convertMimicJoint( + const std::string & mimic_name, const MimicInfo & mimic_info) + { + switch (mimic_info.jointType) + { + case Base::REVOLUTE: + createMimicJoint< + typename JointCollection::JointModelRX, typename JointCollection::JointModelRY, + typename JointCollection::JointModelRZ, + typename JointCollection::JointModelRevoluteUnaligned>( + mimic_name, mimic_info.mimiced_name, mimic_info.axis, mimic_info.multiplier, + mimic_info.offset); + break; + case Base::PRISMATIC: + createMimicJoint< + typename JointCollection::JointModelPX, typename JointCollection::JointModelPY, + typename JointCollection::JointModelPZ, + typename JointCollection::JointModelPrismaticUnaligned>( + mimic_name, mimic_info.mimiced_name, mimic_info.axis, mimic_info.multiplier, + mimic_info.offset); + break; + + default: + PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The joint type is not correct."); + } + } + + template + void createMimicJoint( + const std::string & mimic_name, + const std::string & mimiced_name, + const Vector3 & axis, + const Scalar multiplier, + const Scalar offset) + { + auto mimiced_joint = model.joints[getJointId(mimiced_name)]; + auto mimic_joint = model.joints[getJointId(mimic_name)]; + + CartesianAxis axisType = extractCartesianAxis(axis); + switch (axisType) + { + case AXIS_X: + mimic_joint = + typename JointCollection::JointModelMimic(TypeX(), mimiced_joint, multiplier, offset); + break; + + case AXIS_Y: + mimic_joint = + typename JointCollection::JointModelMimic(TypeY(), mimiced_joint, multiplier, offset); + break; + + case AXIS_Z: + mimic_joint = + typename JointCollection::JointModelMimic(TypeZ(), mimiced_joint, multiplier, offset); + break; + + case AXIS_UNALIGNED: + mimic_joint = typename JointCollection::JointModelMimic( + TypeUnaligned(), mimiced_joint, multiplier, offset); + break; + + default: + PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The axis type of the joint is of wrong type."); + break; + } + } + private: /// /// \brief The four possible cartesian types of an 3D axis. @@ -482,14 +618,14 @@ namespace pinocchio typedef UrdfVisitorBaseTpl UrdfVisitorBase; - PINOCCHIO_PARSERS_DLLAPI void - parseRootTree(const ::urdf::ModelInterface * urdfTree, UrdfVisitorBase & model); + PINOCCHIO_PARSERS_DLLAPI void parseRootTree( + const ::urdf::ModelInterface * urdfTree, UrdfVisitorBase & model, const bool mimic = true); PINOCCHIO_PARSERS_DLLAPI void - parseRootTree(const std::string & filename, UrdfVisitorBase & model); + parseRootTree(const std::string & filename, UrdfVisitorBase & model, const bool mimic = true); - PINOCCHIO_PARSERS_DLLAPI void - parseRootTreeFromXML(const std::string & xmlString, UrdfVisitorBase & model); + PINOCCHIO_PARSERS_DLLAPI void parseRootTreeFromXML( + const std::string & xmlString, UrdfVisitorBase & model, const bool mimic = true); } // namespace details template class JointCollectionTpl> @@ -498,6 +634,7 @@ namespace pinocchio const typename ModelTpl::JointModel & rootJoint, const std::string & rootJointName, ModelTpl & model, + const bool mimic, const bool verbose) { if (rootJointName.empty()) @@ -508,7 +645,7 @@ namespace pinocchio model, rootJoint, rootJointName); if (verbose) visitor.log = &std::cout; - details::parseRootTree(filename, visitor); + details::parseRootTree(filename, visitor, mimic); return model; } @@ -517,21 +654,23 @@ namespace pinocchio const std::string & filename, const typename ModelTpl::JointModel & rootJoint, ModelTpl & model, + const bool mimic, const bool verbose) { - return buildModel(filename, rootJoint, "root_joint", model, verbose); + return buildModel(filename, rootJoint, "root_joint", model, mimic, verbose); } template class JointCollectionTpl> ModelTpl & buildModel( const std::string & filename, ModelTpl & model, + const bool mimic, const bool verbose) { details::UrdfVisitor visitor(model); if (verbose) visitor.log = &std::cout; - details::parseRootTree(filename, visitor); + details::parseRootTree(filename, visitor, mimic); return model; } @@ -541,6 +680,7 @@ namespace pinocchio const typename ModelTpl::JointModel & rootJoint, const std::string & rootJointName, ModelTpl & model, + const bool mimic, const bool verbose) { if (rootJointName.empty()) @@ -551,7 +691,7 @@ namespace pinocchio model, rootJoint, rootJointName); if (verbose) visitor.log = &std::cout; - details::parseRootTreeFromXML(xmlStream, visitor); + details::parseRootTreeFromXML(xmlStream, visitor, mimic); return model; } @@ -560,21 +700,23 @@ namespace pinocchio const std::string & xmlStream, const typename ModelTpl::JointModel & rootJoint, ModelTpl & model, + const bool mimic, const bool verbose) { - return buildModelFromXML(xmlStream, rootJoint, "root_joint", model, verbose); + return buildModelFromXML(xmlStream, rootJoint, "root_joint", model, mimic, verbose); } template class JointCollectionTpl> ModelTpl & buildModelFromXML( const std::string & xmlStream, ModelTpl & model, + const bool mimic, const bool verbose) { details::UrdfVisitor visitor(model); if (verbose) visitor.log = &std::cout; - details::parseRootTreeFromXML(xmlStream, visitor); + details::parseRootTreeFromXML(xmlStream, visitor, mimic); return model; } @@ -584,6 +726,7 @@ namespace pinocchio const typename ModelTpl::JointModel & rootJoint, const std::string & rootJointName, ModelTpl & model, + const bool mimic, const bool verbose) { if (rootJointName.empty()) @@ -595,7 +738,7 @@ namespace pinocchio model, rootJoint, rootJointName); if (verbose) visitor.log = &std::cout; - details::parseRootTree(urdfTree.get(), visitor); + details::parseRootTree(urdfTree.get(), visitor, mimic); return model; } @@ -604,22 +747,24 @@ namespace pinocchio const std::shared_ptr<::urdf::ModelInterface> urdfTree, const typename ModelTpl::JointModel & rootJoint, ModelTpl & model, + const bool mimic, const bool verbose) { - return buildModel(urdfTree, rootJoint, "root_joint", model, verbose); + return buildModel(urdfTree, rootJoint, "root_joint", model, mimic, verbose); } template class JointCollectionTpl> ModelTpl & buildModel( const std::shared_ptr<::urdf::ModelInterface> urdfTree, ModelTpl & model, + const bool mimic, const bool verbose) { PINOCCHIO_CHECK_INPUT_ARGUMENT(urdfTree != NULL); details::UrdfVisitor visitor(model); if (verbose) visitor.log = &std::cout; - details::parseRootTree(urdfTree.get(), visitor); + details::parseRootTree(urdfTree.get(), visitor, mimic); return model; } diff --git a/include/pinocchio/serialization/joints-constraint.hpp b/include/pinocchio/serialization/joints-constraint.hpp new file mode 100644 index 0000000000..e69de29bb2 diff --git a/include/pinocchio/serialization/joints-data.hpp b/include/pinocchio/serialization/joints-data.hpp index 14f1e510a5..240629a1f5 100644 --- a/include/pinocchio/serialization/joints-data.hpp +++ b/include/pinocchio/serialization/joints-data.hpp @@ -61,6 +61,32 @@ namespace boost ar & make_nvp("UDinv", joint_data.UDinv()); ar & make_nvp("StU", joint_data.StU()); } + + template< + class Archive, + typename Scalar, + int Options, + template class JointCollection> + void serialize( + Archive & ar, + pinocchio::JointDataBase> & + joint_data, + const unsigned int /*version*/) + { + ar & make_nvp("joint_q", joint_data.joint_q()); + ar & make_nvp("joint_v", joint_data.joint_v()); + + ar & make_nvp("S", joint_data.S()); + // ar & make_nvp("M", joint_data.M()); + // ar & make_nvp("v", joint_data.v()); + // ar & make_nvp("c", joint_data.c()); + + // ar & make_nvp("U", joint_data.U()); + // ar & make_nvp("Dinv", joint_data.Dinv()); + // ar & make_nvp("UDinv", joint_data.UDinv()); + // ar & make_nvp("StU", joint_data.StU()); + } + } // namespace fix template @@ -233,13 +259,22 @@ namespace boost ar & make_nvp("base_variant", base_object(joint)); } - template + template< + class Archive, + typename Scalar, + int Options, + template typename JointCollection> void serialize( - Archive & ar, pinocchio::JointDataMimic & joint, const unsigned int version) + Archive & ar, + pinocchio::JointDataMimicTpl & joint, + const unsigned int version) { - typedef pinocchio::JointDataMimic JointType; + typedef pinocchio::JointDataMimicTpl JointType; fix::serialize(ar, static_cast &>(joint), version); + ar & make_nvp("m_q_transform", joint.q_transformed()); + ar & make_nvp("m_v_transform", joint.v_transformed()); + ar & make_nvp("jdata", joint.jdata()); ar & make_nvp("scaling", joint.scaling()); } diff --git a/include/pinocchio/serialization/joints-model.hpp b/include/pinocchio/serialization/joints-model.hpp index 81d4167fd1..4791ff444d 100644 --- a/include/pinocchio/serialization/joints-model.hpp +++ b/include/pinocchio/serialization/joints-model.hpp @@ -18,10 +18,13 @@ namespace pinocchio ar & make_nvp("m_nq", joint.m_nq); ar & make_nvp("m_nv", joint.m_nv); + ar & make_nvp("m_nj", joint.m_nj); ar & make_nvp("m_idx_q", joint.m_idx_q); ar & make_nvp("m_nqs", joint.m_nqs); ar & make_nvp("m_idx_v", joint.m_idx_v); ar & make_nvp("m_nvs", joint.m_nvs); + ar & make_nvp("m_idx_j", joint.m_idx_j); + ar & make_nvp("m_njs", joint.m_njs); ar & make_nvp("njoints", joint.njoints); ar & make_nvp("joints", joint.joints); @@ -54,25 +57,59 @@ namespace boost const unsigned int /*version*/) { const pinocchio::JointIndex i_id = joint.id(); - const int i_q = joint.idx_q(), i_v = joint.idx_v(); + const int i_q = joint.idx_q(), i_v = joint.idx_v(), i_j = joint.idx_j(); ar & make_nvp("i_id", i_id); ar & make_nvp("i_q", i_q); ar & make_nvp("i_v", i_v); + ar & make_nvp("i_j", i_j); } + template + class SetJointIndexes + { + Derived & joint; + + public: + explicit SetJointIndexes(Derived & joint) + : joint(joint) {}; + + void run(pinocchio::JointIndex i_id, int i_q, int i_v, int i_j) + { + joint.setIndexes(i_id, i_q, i_v, i_j); + } + }; + + template class JointCollectionTpl> + class SetJointIndexes> + { + pinocchio::JointModelMimicTpl & joint_m; + + public: + explicit SetJointIndexes( + pinocchio::JointModelMimicTpl & joint) + : joint_m(joint) {}; + + void run(pinocchio::JointIndex i_id, int i_q, int i_v, int i_j) + { + joint_m.setIndexes(i_id, 0, 0, i_j); + joint_m.setMimicIndexes(0, i_q, i_v, 0); + } + }; + template void load(Archive & ar, pinocchio::JointModelBase & joint, const unsigned int /*version*/) { pinocchio::JointIndex i_id; - int i_q, i_v; + int i_q, i_v, i_j; ar & make_nvp("i_id", i_id); ar & make_nvp("i_q", i_q); ar & make_nvp("i_v", i_v); + ar & make_nvp("i_j", i_j); - joint.setIndexes(i_id, i_q, i_v); + SetJointIndexes(joint.derived()).run(i_id, i_q, i_v, i_j); } template @@ -284,11 +321,17 @@ namespace boost ar & make_nvp("base_variant", base_object(joint)); } - template + template< + class Archive, + typename Scalar, + int Options, + template class JointCollectionTpl> void serialize( - Archive & ar, pinocchio::JointModelMimic & joint, const unsigned int version) + Archive & ar, + pinocchio::JointModelMimicTpl & joint, + const unsigned int version) { - typedef pinocchio::JointModelMimic JointType; + typedef pinocchio::JointModelMimicTpl JointType; // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase // >(joint)); fix::serialize(ar, *static_cast *>(&joint), version); diff --git a/include/pinocchio/serialization/joints-motion-subspace.hpp b/include/pinocchio/serialization/joints-motion-subspace.hpp index 46a3571be1..6491cc5c0a 100644 --- a/include/pinocchio/serialization/joints-motion-subspace.hpp +++ b/include/pinocchio/serialization/joints-motion-subspace.hpp @@ -101,19 +101,19 @@ namespace boost ar & make_nvp("angularSubspace", S.angularSubspace()); } - template + template void serialize( Archive & ar, - pinocchio::JointMotionSubspaceTpl & S, + pinocchio::JointMotionSubspaceTpl & S, const unsigned int /*version*/) { ar & make_nvp("matrix", S.matrix()); } - template + template void serialize( Archive & ar, - pinocchio::ScaledJointMotionSubspace & S, + pinocchio::ScaledJointMotionSubspaceTpl & S, const unsigned int /*version*/) { ar & make_nvp("scaling", S.scaling()); diff --git a/include/pinocchio/serialization/model.hpp b/include/pinocchio/serialization/model.hpp index 51a0cd1e63..65f82659ee 100644 --- a/include/pinocchio/serialization/model.hpp +++ b/include/pinocchio/serialization/model.hpp @@ -37,6 +37,9 @@ namespace boost ar & make_nvp("nv", model.nv); ar & make_nvp("nvs", model.nvs); ar & make_nvp("idx_vs", model.idx_vs); + ar & make_nvp("nj", model.nj); + ar & make_nvp("njs", model.njs); + ar & make_nvp("idx_js", model.idx_js); ar & make_nvp("njoints", model.njoints); ar & make_nvp("nbodies", model.nbodies); ar & make_nvp("nframes", model.nframes); diff --git a/src/algorithm/kinematics.cpp b/src/algorithm/kinematics.cpp index 55b6072ee5..77025a86d7 100644 --- a/src/algorithm/kinematics.cpp +++ b/src/algorithm/kinematics.cpp @@ -45,6 +45,16 @@ namespace pinocchio const Eigen::MatrixBase> &, const Eigen::MatrixBase> &); } // namespace impl + + template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI + SE3Tpl + getRelativePlacement( + const context::Model &, + const context::Data &, + const JointIndex, + const JointIndex, + const Convention); + template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI MotionTpl getVelocity( diff --git a/src/algorithm/model.cpp b/src/algorithm/model.cpp index 5c8dd7065a..fc3f5cbd81 100644 --- a/src/algorithm/model.cpp +++ b/src/algorithm/model.cpp @@ -81,6 +81,15 @@ namespace pinocchio context::Model &, std::vector> &); + template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI void + transformJointIntoMimic( + const context::Model &, + const JointIndex &, + const JointIndex &, + const context::Scalar &, + const context::Scalar &, + context::Model &); + template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI JointIndex findCommonAncestor( const context::Model &, JointIndex, JointIndex, size_t &, size_t &); diff --git a/src/multibody/sample-models.cpp b/src/multibody/sample-models.cpp index af2d62565d..f5a376d52f 100644 --- a/src/multibody/sample-models.cpp +++ b/src/multibody/sample-models.cpp @@ -13,14 +13,15 @@ namespace pinocchio namespace buildModels { template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI void - manipulator(context::Model &); + manipulator( + context::Model &, bool); template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI void humanoid(context::Model &, bool); template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI void humanoidRandom( - context::Model &, bool); + context::Model &, bool, bool); } // namespace buildModels } // namespace pinocchio diff --git a/src/parsers/urdf/model.cpp b/src/parsers/urdf/model.cpp index fb530c6468..e247702c58 100644 --- a/src/parsers/urdf/model.cpp +++ b/src/parsers/urdf/model.cpp @@ -64,7 +64,7 @@ namespace pinocchio /// \param[in] link The current URDF link. /// \param[in] model The model where the link must be added. /// - void parseTree(::urdf::LinkConstSharedPtr link, UrdfVisitorBase & model) + void parseTree(::urdf::LinkConstSharedPtr link, UrdfVisitorBase & model, const bool mimic) { typedef UrdfVisitorBase::Scalar Scalar; typedef UrdfVisitorBase::SE3 SE3; @@ -137,10 +137,28 @@ namespace pinocchio friction << joint->dynamics->friction; damping << joint->dynamics->damping; } + if (joint->mimic && mimic) + { + max_effort = Vector::Constant(0, infty); + max_velocity = Vector::Constant(0, infty); + min_config = Vector::Constant(0, -infty); + max_config = Vector::Constant(0, infty); - model.addJointAndBody( - UrdfVisitorBase::REVOLUTE, axis, parentFrameId, jointPlacement, joint->name, Y, - link->name, max_effort, max_velocity, min_config, max_config, friction, damping); + friction = Vector::Constant(0, 0.); + damping = Vector::Constant(0, 0.); + + model.addMimicInfo( + joint->name, joint->mimic->joint_name, axis, joint->mimic->multiplier, + joint->mimic->offset, UrdfVisitorBase::REVOLUTE); + + model.addJointAndBody( + UrdfVisitorBase::MIMIC, axis, parentFrameId, jointPlacement, joint->name, Y, + link->name, max_effort, max_velocity, min_config, max_config, friction, damping); + } + else + model.addJointAndBody( + UrdfVisitorBase::REVOLUTE, axis, parentFrameId, jointPlacement, joint->name, Y, + link->name, max_effort, max_velocity, min_config, max_config, friction, damping); break; case ::urdf::Joint::CONTINUOUS: // Revolute joint with no joint limits @@ -191,10 +209,28 @@ namespace pinocchio friction << joint->dynamics->friction; damping << joint->dynamics->damping; } + if (joint->mimic && mimic) + { + max_effort = Vector::Constant(0, infty); + max_velocity = Vector::Constant(0, infty); + min_config = Vector::Constant(0, -infty); + max_config = Vector::Constant(0, infty); - model.addJointAndBody( - UrdfVisitorBase::PRISMATIC, axis, parentFrameId, jointPlacement, joint->name, Y, - link->name, max_effort, max_velocity, min_config, max_config, friction, damping); + friction = Vector::Constant(0, 0.); + damping = Vector::Constant(0, 0.); + + model.addMimicInfo( + joint->name, joint->mimic->joint_name, axis, joint->mimic->multiplier, + joint->mimic->offset, UrdfVisitorBase::PRISMATIC); + + model.addJointAndBody( + UrdfVisitorBase::MIMIC, axis, parentFrameId, jointPlacement, joint->name, Y, + link->name, max_effort, max_velocity, min_config, max_config, friction, damping); + } + else + model.addJointAndBody( + UrdfVisitorBase::PRISMATIC, axis, parentFrameId, jointPlacement, joint->name, Y, + link->name, max_effort, max_velocity, min_config, max_config, friction, damping); break; case ::urdf::Joint::PLANAR: @@ -251,7 +287,7 @@ namespace pinocchio BOOST_FOREACH (::urdf::LinkConstSharedPtr child, link->child_links) { - parseTree(child, model); + parseTree(child, model, mimic); } } @@ -263,7 +299,8 @@ namespace pinocchio /// \param[in] link The current URDF link. /// \param[in] model The model where the link must be added. /// - void parseRootTree(const ::urdf::ModelInterface * urdfTree, UrdfVisitorBase & model) + void parseRootTree( + const ::urdf::ModelInterface * urdfTree, UrdfVisitorBase & model, const bool mimic) { model.setName(urdfTree->getName()); @@ -272,15 +309,20 @@ namespace pinocchio BOOST_FOREACH (::urdf::LinkConstSharedPtr child, root_link->child_links) { - parseTree(child, model); + parseTree(child, model, mimic); + } + if (mimic) + { + for (const auto & entry : model.mimicInfo_map) + model.convertMimicJoint(entry.first, entry.second); } } - void parseRootTree(const std::string & filename, UrdfVisitorBase & model) + void parseRootTree(const std::string & filename, UrdfVisitorBase & model, const bool mimic) { ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile(filename); if (urdfTree) - return parseRootTree(urdfTree.get(), model); + return parseRootTree(urdfTree.get(), model, mimic); else throw std::invalid_argument( "The file " + filename @@ -288,11 +330,12 @@ namespace pinocchio "contain a valid URDF model."); } - void parseRootTreeFromXML(const std::string & xmlString, UrdfVisitorBase & model) + void + parseRootTreeFromXML(const std::string & xmlString, UrdfVisitorBase & model, const bool mimic) { ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDF(xmlString); if (urdfTree) - return parseRootTree(urdfTree.get(), model); + return parseRootTree(urdfTree.get(), model, mimic); else throw std::invalid_argument("The XML stream does not contain a valid " "URDF model."); diff --git a/unittest/aba.cpp b/unittest/aba.cpp index a8204b9336..b213083589 100644 --- a/unittest/aba.cpp +++ b/unittest/aba.cpp @@ -74,7 +74,7 @@ struct TestJointMethods void operator()(const pinocchio::JointModelBase &) const { JointModel jmodel; - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test_joint_methods(jmodel); } @@ -84,7 +84,7 @@ struct TestJointMethods pinocchio::JointModelComposite jmodel_composite; jmodel_composite.addJoint(pinocchio::JointModelRX()); jmodel_composite.addJoint(pinocchio::JointModelRY()); - jmodel_composite.setIndexes(0, 0, 0); + jmodel_composite.setIndexes(0, 0, 0, 0); // TODO: correct LieGroup // test_joint_methods(jmodel_composite); @@ -93,7 +93,7 @@ struct TestJointMethods void operator()(const pinocchio::JointModelBase &) const { pinocchio::JointModelRevoluteUnaligned jmodel(1.5, 1., 0.); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test_joint_methods(jmodel); } @@ -101,7 +101,7 @@ struct TestJointMethods void operator()(const pinocchio::JointModelBase &) const { pinocchio::JointModelPrismaticUnaligned jmodel(1.5, 1., 0.); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test_joint_methods(jmodel); } diff --git a/unittest/all-joints.cpp b/unittest/all-joints.cpp index 072188ed94..de7bdee47a 100644 --- a/unittest/all-joints.cpp +++ b/unittest/all-joints.cpp @@ -25,7 +25,7 @@ struct init static JointModel_ run() { JointModel_ jmodel; - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -40,7 +40,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -55,7 +55,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -70,7 +70,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -85,7 +85,7 @@ struct init> typedef pinocchio::JointModelRevoluteTpl JointModelRX; JointModel jmodel((JointModelRX())); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -102,23 +102,24 @@ struct init> JointModel jmodel((JointModelRX())); jmodel.addJoint(JointModelRY()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; -template -struct init> +template class JointCollection> +struct init> { - typedef pinocchio::JointModelMimic JointModel; + typedef pinocchio::JointModelMimicTpl JointModel; static JointModel run() { - JointModel_ jmodel_ref = init::run(); - JointModel jmodel(jmodel_ref, 1., 0.); - jmodel.setIndexes(0, 0, 0); + typedef pinocchio::JointModelRevoluteTpl JointModelRX; + JointModelRX jmodel_ref = init::run(); + JointModel jmodel(jmodel_ref, 1., 0.); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -132,7 +133,7 @@ struct init> { JointModel jmodel(XAxis::vector(), YAxis::vector()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -146,7 +147,7 @@ struct init> { JointModel jmodel(static_cast(0.5)); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -161,7 +162,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -216,7 +217,7 @@ BOOST_AUTO_TEST_CASE(isEqual) BOOST_CHECK(joint_revolutex != joint_revolutey); JointModel jmodelx(joint_revolutex); - jmodelx.setIndexes(0, 0, 0); + jmodelx.setIndexes(0, 0, 0, 0); TestJointModelIsEqual()(JointModel()); JointModel jmodel_any; diff --git a/unittest/casadi/algorithms.cpp b/unittest/casadi/algorithms.cpp index 1cc9d5efb2..cde719fce7 100644 --- a/unittest/casadi/algorithms.cpp +++ b/unittest/casadi/algorithms.cpp @@ -55,7 +55,7 @@ BOOST_AUTO_TEST_CASE(test_jacobian) Model::Index joint_id = model.existJointName("rarm2") ? model.getJointId("rarm2") : (Model::Index)(model.njoints - 1); - Data::Matrix6x jacobian_local(6, model.nv), jacobian_world(6, model.nv); + Data::Matrix6x jacobian_local(6, model.nj), jacobian_world(6, model.nj); jacobian_local.setZero(); jacobian_world.setZero(); diff --git a/unittest/casadi/joints.cpp b/unittest/casadi/joints.cpp index 81c904d1ad..5b657d7766 100644 --- a/unittest/casadi/joints.cpp +++ b/unittest/casadi/joints.cpp @@ -40,7 +40,7 @@ BOOST_AUTO_TEST_CASE(test_jointRX_motion_space) typedef JointCollection::JointDataRX JointDataRX; JointModelRX jmodel; - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); JointDataRX jdata(jmodel.createData()); JointModelRXAD jmodel_ad = jmodel.cast(); @@ -152,7 +152,7 @@ struct init static JointModel_ run() { JointModel_ jmodel; - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } @@ -172,7 +172,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } @@ -192,7 +192,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } @@ -212,7 +212,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } @@ -232,7 +232,7 @@ struct init> typedef pinocchio::JointModelRevoluteTpl JointModelRX; JointModel jmodel((JointModelRX())); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } @@ -254,7 +254,7 @@ struct init> JointModel jmodel((JointModelRX())); jmodel.addJoint(JointModelRY()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } @@ -264,17 +264,18 @@ struct init> } }; -template -struct init> +template class JointCollection> +struct init> { - typedef pinocchio::JointModelMimic JointModel; + typedef pinocchio::JointModelMimicTpl JointModel; static JointModel run() { - JointModel_ jmodel_ref = init::run(); + typedef pinocchio::JointModelRevoluteTpl JointModelRX; + JointModelRX jmodel_ref = init::run(); JointModel jmodel(jmodel_ref, 1., 0.); - + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } @@ -290,7 +291,7 @@ struct TestADOnJoints void operator()(const pinocchio::JointModelBase &) const { JointModel_ jmodel = init::run(); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } @@ -299,7 +300,7 @@ struct TestADOnJoints { typedef pinocchio::JointModelHelicalTpl JointModel; JointModel jmodel(Scalar(0.4)); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } @@ -310,7 +311,7 @@ struct TestADOnJoints typedef pinocchio::JointModelUniversalTpl JointModel; typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::UnitX(), Vector3::UnitY()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } @@ -321,7 +322,7 @@ struct TestADOnJoints typedef pinocchio::JointModelHelicalUnalignedTpl JointModel; typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } @@ -332,7 +333,7 @@ struct TestADOnJoints typedef pinocchio::JointModelRevoluteUnalignedTpl JointModel; typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } @@ -343,7 +344,7 @@ struct TestADOnJoints typedef pinocchio::JointModelRevoluteUnboundedUnalignedTpl JointModel; typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } @@ -354,7 +355,7 @@ struct TestADOnJoints typedef pinocchio::JointModelPrismaticUnalignedTpl JointModel; typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } @@ -365,7 +366,7 @@ struct TestADOnJoints typedef pinocchio::JointModelRevoluteTpl JointModelRX; typedef pinocchio::JointModelTpl JointModel; JointModel jmodel((JointModelRX())); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } @@ -378,14 +379,15 @@ struct TestADOnJoints typedef pinocchio::JointModelCompositeTpl JointModel; JointModel jmodel((JointModelRX())); jmodel.addJoint(JointModelRY()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } // TODO: get the nq and nv quantity from LieGroups - template - static void test(const pinocchio::JointModelMimic & /*jmodel*/) + template class JointCollection> + static void + test(const pinocchio::JointModelMimicTpl & /*jmodel*/) { /* do nothing */ } diff --git a/unittest/centroidal.cpp b/unittest/centroidal.cpp index b5bab82481..6494864950 100644 --- a/unittest/centroidal.cpp +++ b/unittest/centroidal.cpp @@ -90,6 +90,104 @@ BOOST_AUTO_TEST_CASE(test_ccrba) BOOST_CHECK(data.Ag.isApprox(Ag_ref, 1e-12)); } +void test_mimic_against_full_model( + const pinocchio::Model & model_full, + const pinocchio::JointIndex & primary_id, + const pinocchio::JointIndex & secondary_id) +{ + // constants + const int primary_idxq = model_full.joints[primary_id].idx_q(); + const int primary_idxv = model_full.joints[primary_id].idx_v(); + const int secondary_idxq = model_full.joints[secondary_id].idx_q(); + const int secondary_idxv = model_full.joints[secondary_id].idx_v(); + const double ratio = 2.5; + const double offset = 0.75; + + // Build mimic model + pinocchio::Model model_mimic; + pinocchio::transformJointIntoMimic( + model_full, primary_id, secondary_id, ratio, offset, model_mimic); + pinocchio::Data data_mimic(model_mimic); + pinocchio::Data data_ref_mimic(model_mimic); + pinocchio::Data data_full(model_full); + + // Prepare test data + Eigen::MatrixXd G = Eigen::MatrixXd::Zero(model_full.nv, model_mimic.nv); + G.topLeftCorner(secondary_idxv, secondary_idxv).setIdentity(); + G.bottomRightCorner(model_mimic.nv - secondary_idxv, model_mimic.nv - secondary_idxv) + .setIdentity(); + G(secondary_idxv, primary_idxv) = ratio; + + Eigen::VectorXd q = pinocchio::randomConfiguration(model_mimic); + Eigen::VectorXd v = Eigen::VectorXd::Ones(model_mimic.nv); + + Eigen::VectorXd q_full = Eigen::VectorXd::Zero(model_full.nq); + Eigen::VectorXd v_full = G * v; + + for (int n = 1; n < model_full.njoints; n++) + { + const double joint_ratio = ((n == secondary_id) ? ratio : 1.0); + const double joint_offset = ((n == secondary_id) ? offset : 0.0); + model_full.joints[n].jointConfigFromDofSelector(q_full) = + joint_ratio * model_mimic.joints[n].jointConfigFromDofSelector(q) + + joint_offset * Eigen::VectorXd::Ones(model_full.joints[n].nq()); + } + + model_mimic.lowerPositionLimit.head<3>().fill(-1.); + model_mimic.upperPositionLimit.head<3>().fill(1.); + + pinocchio::crba(model_mimic, data_ref_mimic, q, pinocchio::Convention::LOCAL); + data_ref_mimic.M.triangularView() = + data_ref_mimic.M.transpose().triangularView(); + + data_ref_mimic.Ycrb[0] = data_ref_mimic.liMi[1].act(data_ref_mimic.Ycrb[1]); + pinocchio::Data data_ref_other(model_mimic); + pinocchio::crba(model_mimic, data_ref_other, q, pinocchio::Convention::WORLD); + data_ref_other.M.triangularView() = + data_ref_other.M.transpose().triangularView(); + + pinocchio::SE3 cMo(pinocchio::SE3::Matrix3::Identity(), -data_ref_mimic.Ycrb[0].lever()); + BOOST_CHECK(data_ref_mimic.Ycrb[0].isApprox(data_ref_other.oYcrb[0])); + + ccrba(model_mimic, data_mimic, q, v); + ccrba(model_full, data_full, q_full, v_full); + BOOST_CHECK(data_mimic.com[0].isApprox(data_full.com[0], 1e-12)); + BOOST_CHECK(data_mimic.oYcrb[0].matrix().isApprox(data_full.oYcrb[0].matrix(), 1e-12)); + + BOOST_CHECK(data_mimic.com[0].isApprox(-cMo.translation(), 1e-12)); + BOOST_CHECK(data_mimic.oYcrb[0].matrix().isApprox(data_ref_mimic.Ycrb[0].matrix(), 1e-12)); + + pinocchio::Inertia Ig_ref(cMo.act(data_mimic.oYcrb[0])); + BOOST_CHECK(data_mimic.Ig.matrix().isApprox(Ig_ref.matrix(), 1e-12)); + + pinocchio::SE3 oM1(data_ref_other.liMi[1]); + pinocchio::SE3 cM1(cMo * oM1); + pinocchio::Data::Matrix6x Ag_ref( + cM1.inverse().toActionMatrix().transpose() * data_ref_other.M.topRows<6>()); + BOOST_CHECK(data_mimic.Ag.isApprox(Ag_ref, 1e-12)); +} + +BOOST_AUTO_TEST_CASE(test_ccrba_mimic) +{ + pinocchio::Model humanoid_model; + pinocchio::buildModels::humanoidRandom(humanoid_model); + + // Test for direct parent/child joint mimic + test_mimic_against_full_model( + humanoid_model, humanoid_model.getJointId("rleg3_joint"), + humanoid_model.getJointId("rleg4_joint")); + + // Test for spaced parent/child joint mimic + test_mimic_against_full_model( + humanoid_model, humanoid_model.getJointId("rleg1_joint"), + humanoid_model.getJointId("rleg4_joint")); + + // // Test for parallel joint mimic + // test_mimic_against_full_model( + // humanoid_model, humanoid_model.getJointId("lleg4_joint"), + // humanoid_model.getJointId("rleg4_joint")); +} + BOOST_AUTO_TEST_CASE(test_centroidal_mapping) { pinocchio::Model model; diff --git a/unittest/cppad/joints.cpp b/unittest/cppad/joints.cpp index 023067f249..3ab7e3ec33 100644 --- a/unittest/cppad/joints.cpp +++ b/unittest/cppad/joints.cpp @@ -44,7 +44,7 @@ BOOST_AUTO_TEST_CASE(test_jointRX_motion_space) typedef JointCollection::JointDataRX JointDataRX; JointModelRX jmodel; - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); JointDataRX jdata(jmodel.createData()); JointModelRXAD jmodel_ad = jmodel.cast(); @@ -108,7 +108,7 @@ struct TestADOnJoints void operator()(const pinocchio::JointModelBase &) const { JointModel jmodel; - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } @@ -118,7 +118,7 @@ struct TestADOnJoints { typedef pinocchio::JointModelHelicalTpl JointModel; JointModel jmodel(Scalar(0.4)); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } @@ -129,7 +129,7 @@ struct TestADOnJoints typedef pinocchio::JointModelUniversalTpl JointModel; typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::UnitX(), Vector3::UnitY()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } @@ -140,7 +140,7 @@ struct TestADOnJoints typedef pinocchio::JointModelHelicalUnalignedTpl JointModel; typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } @@ -151,7 +151,7 @@ struct TestADOnJoints typedef pinocchio::JointModelRevoluteUnalignedTpl JointModel; typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } @@ -162,7 +162,7 @@ struct TestADOnJoints typedef pinocchio::JointModelRevoluteUnboundedUnalignedTpl JointModel; typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } @@ -173,7 +173,7 @@ struct TestADOnJoints typedef pinocchio::JointModelPrismaticUnalignedTpl JointModel; typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } @@ -184,14 +184,15 @@ struct TestADOnJoints typedef pinocchio::JointModelRevoluteTpl JointModelRX; typedef pinocchio::JointModelTpl JointModel; JointModel jmodel((JointModelRX())); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } // TODO: implement it - template - void operator()(const pinocchio::JointModelMimic & /*jmodel*/) const + template class JointCollection> + void operator()( + const pinocchio::JointModelMimicTpl & /*jmodel*/) const { /* do nothing */ } @@ -204,7 +205,7 @@ struct TestADOnJoints typedef pinocchio::JointModelCompositeTpl JointModel; JointModel jmodel((JointModelRX())); jmodel.addJoint(JointModelRY()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } diff --git a/unittest/crba.cpp b/unittest/crba.cpp index 19da9b9e35..f96492458b 100644 --- a/unittest/crba.cpp +++ b/unittest/crba.cpp @@ -16,6 +16,7 @@ #include "pinocchio/multibody/model.hpp" #include "pinocchio/multibody/data.hpp" +#include "pinocchio/algorithm/model.hpp" #include "pinocchio/algorithm/crba.hpp" #include "pinocchio/algorithm/centroidal.hpp" #include "pinocchio/algorithm/rnea.hpp" @@ -64,7 +65,7 @@ BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) BOOST_AUTO_TEST_CASE(test_crba) { pinocchio::Model model; - pinocchio::buildModels::humanoidRandom(model); + pinocchio::buildModels::humanoidRandom(model, true); pinocchio::Data data(model); #ifdef NDEBUG @@ -125,10 +126,96 @@ BOOST_AUTO_TEST_CASE(test_crba) #endif // ifndef NDEBUG } +void test_mimic_against_full_model( + const pinocchio::Model & model_full, + const pinocchio::JointIndex & primary_id, + const pinocchio::JointIndex & secondary_id) +{ + // constants + const int primary_idxq = model_full.joints[primary_id].idx_q(); + const int primary_idxv = model_full.joints[primary_id].idx_v(); + const int secondary_idxq = model_full.joints[secondary_id].idx_q(); + const int secondary_idxv = model_full.joints[secondary_id].idx_v(); + const double ratio = 2.5; + const double offset = 0.75; + + // Build mimic model + pinocchio::Model model_mimic; + pinocchio::transformJointIntoMimic( + model_full, primary_id, secondary_id, ratio, offset, model_mimic); + pinocchio::Data data_full(model_full); + pinocchio::Data data_mimic(model_mimic); + pinocchio::Data data_ref_mimic(model_mimic); + + // Prepare test data + Eigen::MatrixXd G = Eigen::MatrixXd::Zero(model_full.nv, model_mimic.nv); + G.topLeftCorner(secondary_idxv, secondary_idxv).setIdentity(); + G.bottomRightCorner(model_mimic.nv - secondary_idxv, model_mimic.nv - secondary_idxv) + .setIdentity(); + G(secondary_idxv, primary_idxv) = ratio; + + Eigen::VectorXd q_mimic = pinocchio::randomConfiguration(model_mimic); + Eigen::VectorXd v_mimic = Eigen::VectorXd::Random(model_mimic.nv); + Eigen::VectorXd a_mimic = Eigen::VectorXd::Random(model_mimic.nv); + + // World vs local + pinocchio::crba(model_mimic, data_ref_mimic, q_mimic, pinocchio::Convention::WORLD); + pinocchio::crba(model_mimic, data_mimic, q_mimic, pinocchio::Convention::LOCAL); + + BOOST_CHECK(data_ref_mimic.M.isApprox(data_mimic.M)); + + Eigen::VectorXd q_full = Eigen::VectorXd::Zero(model_full.nq); + Eigen::VectorXd v_full = G * v_mimic; + Eigen::VectorXd a_full = G * a_mimic; + + for (int n = 1; n < model_full.njoints; n++) + { + const double joint_ratio = ((n == secondary_id) ? ratio : 1.0); + const double joint_offset = ((n == secondary_id) ? offset : 0.0); + model_full.joints[n].jointConfigFromDofSelector(q_full) = + joint_ratio * model_mimic.joints[n].jointConfigFromDofSelector(q_mimic) + + joint_offset * Eigen::VectorXd::Ones(model_full.joints[n].nq()); + } + + // // // Run crba + // // pinocchio::crba(model_mimic, data_mimic, q_mimic); + pinocchio::crba(model_full, data_full, q_full); + + // Compute other half of matrix + data_mimic.M.triangularView() = + data_mimic.M.transpose().triangularView(); + data_full.M.triangularView() = + data_full.M.transpose().triangularView(); + + // Check full model against reduced + BOOST_CHECK(data_mimic.M.isApprox(G.transpose() * data_full.M * G, 1e-12)); +} + +BOOST_AUTO_TEST_CASE(test_crba_mimic) +{ + pinocchio::Model humanoid_model; + pinocchio::buildModels::humanoidRandom(humanoid_model, true); + + // Test for direct parent/child joint mimic + test_mimic_against_full_model( + humanoid_model, humanoid_model.getJointId("rleg4_joint"), + humanoid_model.getJointId("rleg5_joint")); + + // Test for spaced parent/child joint mimic + test_mimic_against_full_model( + humanoid_model, humanoid_model.getJointId("rleg1_joint"), + humanoid_model.getJointId("rleg4_joint")); + + // // Test for parallel joint mimic + test_mimic_against_full_model( + humanoid_model, humanoid_model.getJointId("lleg4_joint"), + humanoid_model.getJointId("rleg4_joint")); +} + BOOST_AUTO_TEST_CASE(test_minimal_crba) { pinocchio::Model model; - pinocchio::buildModels::humanoidRandom(model); + pinocchio::buildModels::humanoidRandom(model, true); pinocchio::Data data(model), data_ref(model); model.lowerPositionLimit.head<7>().fill(-1.); @@ -166,7 +253,7 @@ BOOST_AUTO_TEST_CASE(test_minimal_crba) BOOST_AUTO_TEST_CASE(test_roto_inertia_effects) { pinocchio::Model model, model_ref; - pinocchio::buildModels::humanoidRandom(model); + pinocchio::buildModels::humanoidRandom(model, true); model_ref = model; BOOST_CHECK(model == model_ref); @@ -194,7 +281,7 @@ BOOST_AUTO_TEST_CASE(test_crba_malloc) { using namespace pinocchio; pinocchio::Model model; - pinocchio::buildModels::humanoidRandom(model); + pinocchio::buildModels::humanoidRandom(model, true, true); model.addJoint( size_t(model.njoints - 1), pinocchio::JointModelRevoluteUnaligned(SE3::Vector3::UnitX()), diff --git a/unittest/finite-differences.cpp b/unittest/finite-differences.cpp index 95ade70874..d4a06ceb1a 100644 --- a/unittest/finite-differences.cpp +++ b/unittest/finite-differences.cpp @@ -69,7 +69,7 @@ struct init static JointModel_ run() { JointModel_ jmodel; - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -84,7 +84,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -99,7 +99,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -114,7 +114,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -129,7 +129,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -144,7 +144,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(XAxis::vector(), YAxis::vector()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -158,7 +158,7 @@ struct init> { JointModel jmodel(static_cast(0.5)); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -173,7 +173,7 @@ struct init> typedef pinocchio::JointModelRevoluteTpl JointModelRX; JointModel jmodel((JointModelRX())); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -190,22 +190,23 @@ struct init> JointModel jmodel((JointModelRX())); jmodel.addJoint(JointModelRY()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; -template -struct init> +template class JointCollection> +struct init> { - typedef pinocchio::JointModelMimic JointModel; + typedef pinocchio::JointModelMimicTpl JointModel; static JointModel run() { - JointModel_ jmodel_ref = init::run(); + typedef pinocchio::JointModelRevoluteTpl JointModelRX; + JointModelRX jmodel_ref = init::run(); JointModel jmodel(jmodel_ref, 1., 0.); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } @@ -217,6 +218,10 @@ struct FiniteDiffJoint { } + void operator()(JointModelMimic & /*jmodel*/) const + { + } + template void operator()(JointModelBase & /*jmodel*/) const { diff --git a/unittest/joint-composite.cpp b/unittest/joint-composite.cpp index 37971f419e..6440a87741 100644 --- a/unittest/joint-composite.cpp +++ b/unittest/joint-composite.cpp @@ -33,7 +33,7 @@ void test_joint_methods( JointData jdata = jmodel.createData(); JointDataComposite jdata_composite = jmodel_composite.createData(); - jmodel_composite.setIndexes(jmodel.id(), jmodel.idx_q(), jmodel.idx_v()); + jmodel_composite.setIndexes(jmodel.id(), jmodel.idx_q(), jmodel.idx_v(), jmodel.idx_j()); typedef typename JointModel::ConfigVector_t ConfigVector_t; typedef typename JointModel::TangentVector_t TangentVector_t; @@ -142,16 +142,34 @@ void test_joint_methods( Model::ConfigVectorType vec(Model::ConfigVectorType::Ones(m)); const Model::ConfigVectorType vec_const(Model::ConfigVectorType::Ones(m)); - BOOST_CHECK(jmodel.jointConfigSelector(vec) == jmodel_composite.jointConfigSelector(vec)); BOOST_CHECK( - jmodel.jointConfigSelector(vec_const) == jmodel_composite.jointConfigSelector(vec_const)); + jmodel.jointConfigFromDofSelector(vec) == jmodel_composite.jointConfigFromDofSelector(vec)); + BOOST_CHECK( + jmodel.jointConfigFromDofSelector(vec_const) + == jmodel_composite.jointConfigFromDofSelector(vec_const)); + + BOOST_CHECK( + jmodel.jointConfigFromNqSelector(vec) == jmodel_composite.jointConfigFromNqSelector(vec)); + BOOST_CHECK( + jmodel.jointConfigFromNqSelector(vec_const) + == jmodel_composite.jointConfigFromNqSelector(vec_const)); - BOOST_CHECK(jmodel.jointVelocitySelector(vec) == jmodel_composite.jointVelocitySelector(vec)); BOOST_CHECK( - jmodel.jointVelocitySelector(vec_const) == jmodel_composite.jointVelocitySelector(vec_const)); + jmodel.jointVelocityFromNvSelector(vec) == jmodel_composite.jointVelocityFromNvSelector(vec)); + BOOST_CHECK( + jmodel.jointVelocityFromNvSelector(vec_const) + == jmodel_composite.jointVelocityFromNvSelector(vec_const)); + + BOOST_CHECK( + jmodel.jointVelocityFromDofSelector(vec) == jmodel_composite.jointVelocityFromDofSelector(vec)); + BOOST_CHECK( + jmodel.jointVelocityFromDofSelector(vec_const) + == jmodel_composite.jointVelocityFromDofSelector(vec_const)); - BOOST_CHECK(jmodel.jointCols(mat) == jmodel_composite.jointCols(mat)); - BOOST_CHECK(jmodel.jointCols(mat_const) == jmodel_composite.jointCols(mat_const)); + BOOST_CHECK(jmodel.jointVelCols(mat) == jmodel_composite.jointVelCols(mat)); + BOOST_CHECK(jmodel.jointVelCols(mat_const) == jmodel_composite.jointVelCols(mat_const)); + BOOST_CHECK(jmodel.jointJacCols(mat) == jmodel_composite.jointJacCols(mat)); + BOOST_CHECK(jmodel.jointJacCols(mat_const) == jmodel_composite.jointJacCols(mat_const)); } struct TestJointComposite @@ -161,7 +179,7 @@ struct TestJointComposite void operator()(const JointModelBase &) const { JointModel jmodel; - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test_joint_methods(jmodel); } @@ -179,7 +197,7 @@ struct TestJointComposite void operator()(const JointModelBase &) const { JointModelRevoluteUnaligned jmodel(1.5, 1., 0.); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test_joint_methods(jmodel); } @@ -187,7 +205,7 @@ struct TestJointComposite void operator()(const JointModelBase &) const { JointModelPrismaticUnaligned jmodel(1.5, 1., 0.); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test_joint_methods(jmodel); } }; @@ -220,7 +238,7 @@ BOOST_AUTO_TEST_CASE(chain) BOOST_AUTO_TEST_CASE(vsZYX) { JointModelSphericalZYX jmodel_spherical; - jmodel_spherical.setIndexes(0, 0, 0); + jmodel_spherical.setIndexes(0, 0, 0, 0); JointModelComposite jmodel_composite((JointModelRZ())); jmodel_composite.addJoint(JointModelRY()); @@ -232,7 +250,7 @@ BOOST_AUTO_TEST_CASE(vsZYX) BOOST_AUTO_TEST_CASE(vsTranslation) { JointModelTranslation jmodel_translation; - jmodel_translation.setIndexes(0, 0, 0); + jmodel_translation.setIndexes(0, 0, 0, 0); JointModelComposite jmodel_composite((JointModelPX())); jmodel_composite.addJoint(JointModelPY()); @@ -259,7 +277,7 @@ BOOST_AUTO_TEST_CASE(test_copy) JointModelComposite jmodel_composite_planar((JointModelPX())); jmodel_composite_planar.addJoint(JointModelPY()); jmodel_composite_planar.addJoint(JointModelRZ()); - jmodel_composite_planar.setIndexes(0, 0, 0); + jmodel_composite_planar.setIndexes(0, 0, 0, 0); JointDataComposite jdata_composite_planar = jmodel_composite_planar.createData(); diff --git a/unittest/joint-generic.cpp b/unittest/joint-generic.cpp index 13242896ac..844f2a6376 100644 --- a/unittest/joint-generic.cpp +++ b/unittest/joint-generic.cpp @@ -137,7 +137,7 @@ struct init static JointModel_ run() { JointModel_ jmodel; - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -152,7 +152,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -167,7 +167,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -182,7 +182,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -197,7 +197,7 @@ struct init> typedef pinocchio::JointModelRevoluteTpl JointModelRX; JointModel jmodel((JointModelRX())); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -214,22 +214,22 @@ struct init> JointModel jmodel((JointModelRX())); jmodel.addJoint(JointModelRY()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; - -template -struct init> +template class JointCollection> +struct init> { - typedef pinocchio::JointModelMimic JointModel; + typedef pinocchio::JointModelMimicTpl JointModel; static JointModel run() { - JointModel_ jmodel_ref = init::run(); + typedef pinocchio::JointModelRevoluteTpl JointModelRX; + JointModelRX jmodel_ref = init::run(); JointModel jmodel(jmodel_ref, 1., 0.); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } @@ -245,7 +245,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(XAxis::vector(), YAxis::vector()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -259,7 +259,7 @@ struct init> { JointModel jmodel(static_cast(0.5)); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -274,7 +274,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -286,7 +286,7 @@ struct TestJoint void operator()(const JointModelBase &) const { JointModel jmodel = init::run(); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); typename JointModel::JointDataDerived jdata = jmodel.createData(); test_joint_methods(jmodel, jdata); @@ -295,6 +295,10 @@ struct TestJoint void operator()(const pinocchio::JointModelComposite &) const { } + + void operator()(const pinocchio::JointModelMimic &) const + { + } }; namespace pinocchio @@ -328,7 +332,8 @@ namespace pinocchio { Options = _Options, NQ = Eigen::Dynamic, // Dynamic because unknown at compile time - NV = Eigen::Dynamic + NV = Eigen::Dynamic, + NJ = Eigen::Dynamic }; typedef _Scalar Scalar; @@ -346,6 +351,8 @@ namespace pinocchio typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + + typedef boost::mpl::false_ is_mimicable_t; }; template class JointCollectionTpl> @@ -419,7 +426,7 @@ BOOST_AUTO_TEST_CASE(isEqual) BOOST_CHECK(joint_revolutex != joint_revolutey); JointModel jmodelx(joint_revolutex); - jmodelx.setIndexes(0, 0, 0); + jmodelx.setIndexes(0, 0, 0, 0); JointModel jmodelx_copy = jmodelx; BOOST_CHECK(jmodelx_copy == jmodelx); @@ -436,7 +443,7 @@ BOOST_AUTO_TEST_CASE(cast) JointModelRX joint_revolutex; JointModel jmodelx(joint_revolutex); - jmodelx.setIndexes(0, 0, 0); + jmodelx.setIndexes(0, 0, 0, 0); BOOST_CHECK(jmodelx.cast() == jmodelx); BOOST_CHECK(jmodelx.cast().cast() == jmodelx); @@ -472,8 +479,8 @@ struct TestJointOperatorEqual test(jdata); } - template - void operator()(const pinocchio::JointModelMimic &) const + template class JointCollection> + void operator()(const pinocchio::JointModelMimicTpl &) const { } diff --git a/unittest/joint-jacobian.cpp b/unittest/joint-jacobian.cpp index 4dc54ffff4..77ea9f2073 100644 --- a/unittest/joint-jacobian.cpp +++ b/unittest/joint-jacobian.cpp @@ -77,10 +77,9 @@ BOOST_AUTO_TEST_CASE(test_jacobian_time_variation) using namespace pinocchio; pinocchio::Model model; - pinocchio::buildModels::humanoidRandom(model); + pinocchio::buildModels::humanoidRandom(model, true, true); pinocchio::Data data(model); pinocchio::Data data_ref(model); - VectorXd q = randomConfiguration( model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq)); VectorXd v = VectorXd::Random(model.nv); @@ -103,7 +102,6 @@ BOOST_AUTO_TEST_CASE(test_jacobian_time_variation) getJointJacobian(model, data, idx, WORLD, J); BOOST_CHECK(J.isApprox(getJointJacobian(model, data, idx, WORLD))); getJointJacobianTimeVariation(model, data, idx, WORLD, dJ); - Motion v_idx(J * v); BOOST_CHECK(v_idx.isApprox(data_ref.oMi[idx].act(data_ref.v[idx]))); @@ -168,7 +166,6 @@ BOOST_AUTO_TEST_CASE(test_jacobian_time_variation) BOOST_CHECK(dJ.isApprox(dJ_ref, sqrt(alpha))); } - // compare to finite differencies : LOCAL { Data data_ref(model), data_ref_plus(model); diff --git a/unittest/joint-mimic.cpp b/unittest/joint-mimic.cpp index 094658c7b0..d5cbe2c5ae 100644 --- a/unittest/joint-mimic.cpp +++ b/unittest/joint-mimic.cpp @@ -20,13 +20,14 @@ void test_constraint_mimic(const JointModelBase & jmodel) typedef typename traits::JointDerived Joint; typedef typename traits::Constraint_t ConstraintType; typedef typename traits::JointDataDerived JointData; - typedef ScaledJointMotionSubspace ScaledJointMotionSubspaceType; + typedef ScaledJointMotionSubspaceTpl ScaledConstraint; + typedef JointMotionSubspaceTpl ConstraintRef; JointData jdata = jmodel.createData(); const double scaling_factor = 2.; - ConstraintType constraint_ref(jdata.S), constraint_ref_shared(jdata.S); - ScaledJointMotionSubspaceType scaled_constraint(constraint_ref_shared, scaling_factor); + ConstraintRef constraint_ref(jdata.S.matrix()), constraint_ref_shared(jdata.S.matrix()); + ScaledConstraint scaled_constraint(constraint_ref_shared, scaling_factor); BOOST_CHECK(constraint_ref.nv() == scaled_constraint.nv()); @@ -40,33 +41,30 @@ void test_constraint_mimic(const JointModelBase & jmodel) { SE3 M = SE3::Random(); - typename ScaledJointMotionSubspaceType::DenseBase S = M.act(scaled_constraint); - typename ScaledJointMotionSubspaceType::DenseBase S_ref = - scaling_factor * M.act(constraint_ref); + typename ScaledConstraint::DenseBase S = M.act(scaled_constraint); + typename ScaledConstraint::DenseBase S_ref = scaling_factor * M.act(constraint_ref); BOOST_CHECK(S.isApprox(S_ref)); } { - typename ScaledJointMotionSubspaceType::DenseBase S = scaled_constraint.matrix(); - typename ScaledJointMotionSubspaceType::DenseBase S_ref = - scaling_factor * constraint_ref.matrix(); + typename ScaledConstraint::DenseBase S = scaled_constraint.matrix(); + typename ScaledConstraint::DenseBase S_ref = scaling_factor * constraint_ref.matrix(); BOOST_CHECK(S.isApprox(S_ref)); } { Motion v = Motion::Random(); - typename ScaledJointMotionSubspaceType::DenseBase S = v.cross(scaled_constraint); - typename ScaledJointMotionSubspaceType::DenseBase S_ref = - scaling_factor * v.cross(constraint_ref); + typename ScaledConstraint::DenseBase S = v.cross(scaled_constraint); + typename ScaledConstraint::DenseBase S_ref = scaling_factor * v.cross(constraint_ref); BOOST_CHECK(S.isApprox(S_ref)); } // Test transpose operations { - const Eigen::DenseIndex dim = 20; + const Eigen::DenseIndex dim = ScaledConstraint::MaxDim; const Matrix6x Fin = Matrix6x::Random(6, dim); Eigen::MatrixXd Fout = scaled_constraint.transpose() * Fin; Eigen::MatrixXd Fout_ref = scaling_factor * (constraint_ref.transpose() * Fin); @@ -95,7 +93,7 @@ struct TestJointConstraint void operator()(const JointModelBase &) const { JointModel jmodel; - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test_constraint_mimic(jmodel); } @@ -103,7 +101,7 @@ struct TestJointConstraint void operator()(const JointModelBase &) const { JointModelRevoluteUnaligned jmodel(1.5, 1., 0.); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test_constraint_mimic(jmodel); } @@ -111,7 +109,7 @@ struct TestJointConstraint void operator()(const JointModelBase &) const { JointModelPrismaticUnaligned jmodel(1.5, 1., 0.); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test_constraint_mimic(jmodel); } @@ -122,14 +120,15 @@ BOOST_AUTO_TEST_CASE(test_constraint) using namespace pinocchio; typedef boost::variant< JointModelRX, JointModelRY, JointModelRZ, JointModelRevoluteUnaligned, JointModelPX, - JointModelPY, JointModelPZ, JointModelPrismaticUnaligned, JointModelRUBX, JointModelRUBY, - JointModelRUBZ> + JointModelPY, JointModelPZ, JointModelPrismaticUnaligned + //, JointModelRUBX, JointModelRUBY, JointModelRUBZ + > Variant; boost::mpl::for_each(TestJointConstraint()); } -template +template void test_joint_mimic(const JointModelBase & jmodel) { typedef typename traits::JointDerived Joint; @@ -137,16 +136,15 @@ void test_joint_mimic(const JointModelBase & jmodel) JointData jdata = jmodel.createData(); - const double scaling_factor = 1.; - const double offset = 0.; - - typedef JointMimic JointMimicType; - typedef typename traits::JointModelDerived JointModelMimicType; - typedef typename traits::JointDataDerived JointDataMimicType; + const double scaling_factor = MimicIdentity ? 1. : 2.5; + const double offset = MimicIdentity ? 0 : 0.75; // test constructor - JointModelMimicType jmodel_mimic(jmodel.derived(), scaling_factor, offset); - JointDataMimicType jdata_mimic = jmodel_mimic.createData(); + JointModelMimic jmodel_mimic(jmodel.derived(), scaling_factor, offset); + JointDataMimic jdata_mimic = jmodel_mimic.createData(); + + // Non-const ref accessors trigger asserts, usefull const ref to call const ref accessors... + const JointDataMimic & jdata_mimic_const_ref{jdata_mimic}; BOOST_CHECK(jmodel_mimic.nq() == 0); BOOST_CHECK(jmodel_mimic.nv() == 0); @@ -154,32 +152,34 @@ void test_joint_mimic(const JointModelBase & jmodel) BOOST_CHECK(jmodel_mimic.idx_q() == jmodel.idx_q()); BOOST_CHECK(jmodel_mimic.idx_v() == jmodel.idx_v()); - BOOST_CHECK(jmodel_mimic.idx_q() == 0); - BOOST_CHECK(jmodel_mimic.idx_v() == 0); - - typedef typename JointModelMimicType::ConfigVector_t ConfigVectorType; + typedef typename JointModel::ConfigVector_t ConfigVectorType; typedef typename LieGroup::type LieGroupType; ConfigVectorType q0 = LieGroupType().randomConfiguration(-ConfigVectorType::Ones(), ConfigVectorType::Ones()); + ConfigVectorType q0_mimic; + MimicConfigurationTransform::run(q0, scaling_factor, offset, q0_mimic); - jmodel.calc(jdata, q0); + jmodel.calc(jdata, q0_mimic); jmodel_mimic.calc(jdata_mimic, q0); BOOST_CHECK(((SE3)jdata.M).isApprox((SE3)jdata_mimic.M())); - BOOST_CHECK(jdata.S.matrix().isApprox(jdata_mimic.S.matrix())); + BOOST_CHECK((scaling_factor * jdata.S.matrix()).isApprox(jdata_mimic.S.matrix())); - typedef typename JointModelMimicType::TangentVector_t TangentVectorType; + typedef typename JointModel::TangentVector_t TangentVectorType; q0 = LieGroupType().randomConfiguration(-ConfigVectorType::Ones(), ConfigVectorType::Ones()); + MimicConfigurationTransform::run(q0, scaling_factor, offset, q0_mimic); TangentVectorType v0 = TangentVectorType::Random(); - jmodel.calc(jdata, q0, v0); + TangentVectorType v0_mimic = v0 * scaling_factor; + jmodel.calc(jdata, q0_mimic, v0_mimic); jmodel_mimic.calc(jdata_mimic, q0, v0); BOOST_CHECK(((SE3)jdata.M).isApprox((SE3)jdata_mimic.M())); - BOOST_CHECK(jdata.S.matrix().isApprox(jdata_mimic.S.matrix())); + BOOST_CHECK((scaling_factor * jdata.S.matrix()).isApprox(jdata_mimic.S.matrix())); BOOST_CHECK(((Motion)jdata.v).isApprox((Motion)jdata_mimic.v())); } +template struct TestJointMimic { @@ -187,25 +187,27 @@ struct TestJointMimic void operator()(const JointModelBase &) const { JointModel jmodel; - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); - test_joint_mimic(jmodel); + test_joint_mimic(jmodel); } void operator()(const JointModelBase &) const { JointModelRevoluteUnaligned jmodel(1.5, 1., 0.); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); - test_joint_mimic(jmodel); + test_joint_mimic( + jmodel); } void operator()(const JointModelBase &) const { JointModelPrismaticUnaligned jmodel(1.5, 1., 0.); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); - test_joint_mimic(jmodel); + test_joint_mimic( + jmodel); } }; @@ -214,11 +216,20 @@ BOOST_AUTO_TEST_CASE(test_joint) using namespace pinocchio; typedef boost::variant< JointModelRX, JointModelRY, JointModelRZ, JointModelRevoluteUnaligned, JointModelPX, - JointModelPY, JointModelPZ, JointModelPrismaticUnaligned, JointModelRUBX, JointModelRUBY, - JointModelRUBZ> - Variant; + JointModelPY, JointModelPZ, JointModelPrismaticUnaligned> + VariantLinear; - boost::mpl::for_each(TestJointMimic()); + typedef boost::variant VariantUnboundedRevolute; + + // Test all the joints with scaling == 1.0 and offset == 0.0 + boost::mpl::for_each(TestJointMimic()); + boost::mpl::for_each( + TestJointMimic()); + + // Test specific transforms for non trivial affine values + boost::mpl::for_each(TestJointMimic()); + boost::mpl::for_each( + TestJointMimic()); } BOOST_AUTO_TEST_CASE(test_transform_linear_affine) @@ -231,12 +242,16 @@ BOOST_AUTO_TEST_CASE(test_transform_linear_affine) LinearAffineTransform::run(q0, scaling, offset, q1); BOOST_CHECK(q0 == q1); - offset = 2.; + scaling = 2.5; + offset = 1.5; LinearAffineTransform::run(ConfigVectorType::Zero(), scaling, offset, q1); BOOST_CHECK(q1 == ConfigVectorType::Constant(offset)); + + LinearAffineTransform::run(q0, scaling, offset, q1); + BOOST_CHECK((scaling * q0 + ConfigVectorType::Ones() * offset) == q1); } -BOOST_AUTO_TEST_CASE(test_transform_linear_revolute) +BOOST_AUTO_TEST_CASE(test_transform_linear_revolute_unbounded) { typedef JointModelRUBX::ConfigVector_t ConfigVectorType; double scaling = 1., offset = 0.; @@ -246,25 +261,40 @@ BOOST_AUTO_TEST_CASE(test_transform_linear_revolute) UnboundedRevoluteAffineTransform::run(q0, scaling, offset, q1); BOOST_CHECK(q0.isApprox(q1)); - offset = 2.; - UnboundedRevoluteAffineTransform::run(ConfigVectorType::Zero(), scaling, offset, q1); - BOOST_CHECK(q1 == ConfigVectorType(math::cos(offset), math::sin(offset))); + scaling = 2.5; + offset = 1.5; + UnboundedRevoluteAffineTransform::run(q0, scaling, offset, q1); + const double theta = atan2(q0[1], q0[0]); + BOOST_CHECK( + q1 + == ConfigVectorType(math::cos(theta * scaling + offset), math::sin(theta * scaling + offset))); +} + +BOOST_AUTO_TEST_CASE(test_transform_no_affine) +{ + typedef JointModelRX::ConfigVector_t ConfigVectorType; + double scaling = 1., offset = 0.; + + ConfigVectorType q0 = ConfigVectorType::Random(); + ConfigVectorType q1; + NoAffineTransform::run(q0, scaling, offset, q1); + BOOST_CHECK(q0 == q1); } BOOST_AUTO_TEST_CASE(test_joint_generic_cast) { JointModelRX jmodel_ref; - jmodel_ref.setIndexes(1, 2, 3); + jmodel_ref.setIndexes(1, 2, 3, 3); - JointModelMimic jmodel(jmodel_ref, 2., 1.); - jmodel.setIndexes(1, -1, -1); + JointModelMimic jmodel(jmodel_ref, 2., 1.); + jmodel.setIndexes(1, -1, -1, 3); BOOST_CHECK(jmodel.id() == jmodel_ref.id()); BOOST_CHECK(jmodel.idx_q() == jmodel_ref.idx_q()); BOOST_CHECK(jmodel.idx_v() == jmodel_ref.idx_v()); JointModel jmodel_generic(jmodel); - jmodel_generic.setIndexes(1, -2, -2); + jmodel_generic.setIndexes(1, -2, -2, 3); BOOST_CHECK(jmodel_generic.id() == jmodel_ref.id()); } diff --git a/unittest/joint-motion-subspace.cpp b/unittest/joint-motion-subspace.cpp index ba1b90bc38..6c1c899d51 100644 --- a/unittest/joint-motion-subspace.cpp +++ b/unittest/joint-motion-subspace.cpp @@ -75,8 +75,9 @@ void test_jmodel_nq_against_nq_ref(const JointModelBase & jmodel, co BOOST_CHECK(jmodel.nq() == nq_ref); } -template -void test_jmodel_nq_against_nq_ref(const JointModelMimic & jmodel, const int & nq_ref) +template class JointCollection> +void test_jmodel_nq_against_nq_ref( + const JointModelMimicTpl & jmodel, const int & nq_ref) { BOOST_CHECK(jmodel.jmodel().nq() == nq_ref); } @@ -89,9 +90,13 @@ void test_nv_against_jmodel( BOOST_CHECK(constraint.nv() == jmodel.nv()); } -template +template< + typename Scalar, + int Options, + template class JointCollection, + typename ConstraintDerived> void test_nv_against_jmodel( - const JointModelMimic & jmodel, + const JointModelMimicTpl & jmodel, const JointMotionSubspaceBase & constraint) { BOOST_CHECK(constraint.nv() == jmodel.jmodel().nv()); @@ -109,20 +114,20 @@ struct buildModel } }; -template -struct buildModel> -{ - typedef JointModelMimic JointModel_; +// template +// struct buildModel> +// { +// typedef JointModelMimic JointModel_; - static Model run(const JointModel_ & jmodel) - { - Model model; - model.addJoint(0, jmodel.jmodel(), SE3::Identity(), "joint"); - model.addJoint(0, jmodel, SE3::Identity(), "joint_mimic"); +// static Model run(const JointModel_ & jmodel) +// { +// Model model; +// model.addJoint(0, jmodel.jmodel(), SE3::Identity(), "joint"); +// model.addJoint(0, jmodel, SE3::Identity(), "joint_mimic"); - return model; - } -}; +// return model; +// } +// }; template void test_constraint_operations(const JointModelBase & jmodel) @@ -268,6 +273,12 @@ void test_constraint_operations(const JointModelBase & jmodel) } } +template class JointCollection> +void test_constraint_operations( + const JointModelMimicTpl & /*jmodel*/) +{ +} // Disable test for JointMimic + template struct init; @@ -277,7 +288,7 @@ struct init static JointModel_ run() { JointModel_ jmodel; - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -292,7 +303,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -307,7 +318,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -322,7 +333,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -337,7 +348,7 @@ struct init> typedef pinocchio::JointModelRevoluteTpl JointModelRX; JointModel jmodel((JointModelRX())); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -351,7 +362,7 @@ struct init> { JointModel jmodel(XAxis::vector(), YAxis::vector()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -371,23 +382,24 @@ struct init> jmodel.addJoint(JointModelRY(), SE3::Random()); jmodel.addJoint(JointModelRZ(), SE3::Random()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; -template -struct init> +template class JointCollection> +struct init> { - typedef pinocchio::JointModelMimic JointModel; + typedef pinocchio::JointModelMimicTpl JointModel; static JointModel run() { - JointModel_ jmodel_ref = init::run(); + typedef pinocchio::JointModelRevoluteTpl JointModelRX; + JointModelRX jmodel_ref = init::run(); JointModel jmodel(jmodel_ref, 1., 0.); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } @@ -402,7 +414,7 @@ struct init> { JointModel jmodel(static_cast(0.5)); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -417,7 +429,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -429,7 +441,7 @@ struct TestJointConstraint void operator()(const JointModelBase &) const { JointModel jmodel = init::run(); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test_constraint_operations(jmodel); } diff --git a/unittest/joint-prismatic.cpp b/unittest/joint-prismatic.cpp index 36918eb04d..eccc232c79 100644 --- a/unittest/joint-prismatic.cpp +++ b/unittest/joint-prismatic.cpp @@ -102,7 +102,7 @@ BOOST_AUTO_TEST_CASE(test_kinematics) JointDataPX joint_data; JointModelPX joint_model; - joint_model.setIndexes(0, 0, 0); + joint_model.setIndexes(0, 0, 0, 0); Eigen::VectorXd q(Eigen::VectorXd::Zero(1)); Eigen::VectorXd q_dot(Eigen::VectorXd::Zero(1)); diff --git a/unittest/kinematics.cpp b/unittest/kinematics.cpp index afde400324..02d3864e0b 100644 --- a/unittest/kinematics.cpp +++ b/unittest/kinematics.cpp @@ -81,6 +81,57 @@ BOOST_AUTO_TEST_CASE(test_kinematics_first) } } +BOOST_AUTO_TEST_CASE(test_getRelativePlacement) +{ + using namespace Eigen; + using namespace pinocchio; + + Model model; + buildModels::humanoid(model); + Data data(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill(1.); + forwardKinematics(model, data, randomConfiguration(model)); + + const std::vector test_joints{ + 0, 1, model.getJointId("rleg_elbow_joint"), model.getJointId("lleg_elbow_joint"), + (JointIndex)(model.njoints - 1)}; + + for (const JointIndex i : test_joints) + { + for (const JointIndex j : test_joints) + { + SE3 placement_world = getRelativePlacement(model, data, i, j, Convention::WORLD); + SE3 placement_local = getRelativePlacement(model, data, i, j, Convention::LOCAL); + + // Both convention should match + BOOST_CHECK(placement_world.isApprox(placement_local)); + + // Relative placement to itself is identity + if (i == j) + { + BOOST_CHECK(placement_world.isIdentity()); + BOOST_CHECK(placement_local.isIdentity()); + } + + // Relative placement to world + if (i == 0) + { + BOOST_CHECK(placement_world.isApprox(data.oMi[j])); + BOOST_CHECK(placement_local.isApprox(data.oMi[j])); + } + + // Relative placement from world + if (j == 0) + { + BOOST_CHECK(placement_world.isApprox(data.oMi[i].inverse())); + BOOST_CHECK(placement_local.isApprox(data.oMi[i].inverse())); + } + } + } +} + BOOST_AUTO_TEST_CASE(test_kinematics_second) { using namespace Eigen; diff --git a/unittest/liegroups.cpp b/unittest/liegroups.cpp index 3249470680..a72393a8ac 100644 --- a/unittest/liegroups.cpp +++ b/unittest/liegroups.cpp @@ -248,7 +248,7 @@ struct TestJoint void operator()(const T) const { T jmodel; - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); typename T::JointDataDerived jdata = jmodel.createData(); run_tests(jmodel, jdata); @@ -257,7 +257,7 @@ struct TestJoint void operator()(const pinocchio::JointModelRevoluteUnaligned &) const { pinocchio::JointModelRevoluteUnaligned jmodel(1.5, 1., 0.); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); pinocchio::JointModelRevoluteUnaligned::JointDataDerived jdata = jmodel.createData(); run_tests(jmodel, jdata); @@ -266,7 +266,7 @@ struct TestJoint void operator()(const pinocchio::JointModelPrismaticUnaligned &) const { pinocchio::JointModelPrismaticUnaligned jmodel(1.5, 1., 0.); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); pinocchio::JointModelPrismaticUnaligned::JointDataDerived jdata = jmodel.createData(); run_tests(jmodel, jdata); diff --git a/unittest/model.cpp b/unittest/model.cpp index 7e254fbb04..8e7c11e982 100644 --- a/unittest/model.cpp +++ b/unittest/model.cpp @@ -174,7 +174,7 @@ BOOST_AUTO_TEST_CASE(append) Model manipulator, humanoid; GeometryModel geomManipulator, geomHumanoid; - buildModels::manipulator(manipulator); + buildModels::manipulator(manipulator, true); buildModels::manipulatorGeometries(manipulator, geomManipulator); geomManipulator.addAllCollisionPairs(); // Add prefix to joint and frame names @@ -250,8 +250,11 @@ BOOST_AUTO_TEST_CASE(append) const JointModel & joint_model_humanoid = humanoid.joints[humanoid.getJointId(model1.names[joint_id])]; BOOST_CHECK( - joint_model_humanoid.jointConfigSelector(humanoid_config->second) - == joint_model1.jointConfigSelector(config_vector)); + joint_model_humanoid.jointConfigFromNqSelector(humanoid_config->second) + == joint_model1.jointConfigFromNqSelector(config_vector)); + BOOST_CHECK( + joint_model_humanoid.jointConfigFromDofSelector(humanoid_config->second) + == joint_model1.jointConfigFromDofSelector(config_vector)); // std::cerr<<"humanoid "<second) - == joint_model1.jointConfigSelector(config_vector)); + joint_model_manipulator.jointConfigFromNqSelector(manipulator_config->second) + == joint_model1.jointConfigFromNqSelector(config_vector)); + BOOST_CHECK( + joint_model_manipulator.jointConfigFromDofSelector(manipulator_config->second) + == joint_model1.jointConfigFromDofSelector(config_vector)); // std::cerr<<"manipulator "<second) - == joint_model.jointConfigSelector(config_vector)); + joint_model_humanoid.jointConfigFromNqSelector(humanoid_config->second) + == joint_model.jointConfigFromNqSelector(config_vector)); + BOOST_CHECK( + joint_model_humanoid.jointConfigFromDofSelector(humanoid_config->second) + == joint_model.jointConfigFromDofSelector(config_vector)); // std::cerr<<"humanoid "<second) - == joint_model.jointConfigSelector(config_vector)); + joint_model_manipulator.jointConfigFromNqSelector(manipulator_config->second) + == joint_model.jointConfigFromNqSelector(config_vector)); + BOOST_CHECK( + joint_model_manipulator.jointConfigFromDofSelector(manipulator_config->second) + == joint_model.jointConfigFromDofSelector(config_vector)); // std::cerr<<"manipulator "<().fill(-1.); model.upperPositionLimit.head<3>().fill(1.); @@ -79,7 +80,7 @@ BOOST_AUTO_TEST_CASE(test_nle_vs_rnea) using namespace pinocchio; pinocchio::Model model; - buildModels::humanoidRandom(model); + buildModels::humanoidRandom(model, true); model.lowerPositionLimit.head<3>().fill(-1.); model.upperPositionLimit.head<3>().fill(1.); @@ -136,7 +137,7 @@ BOOST_AUTO_TEST_CASE(test_rnea_with_fext) using namespace pinocchio; Model model; - buildModels::humanoidRandom(model); + buildModels::humanoidRandom(model, true); model.lowerPositionLimit.head<3>().fill(-1.); model.upperPositionLimit.head<3>().fill(1.); @@ -179,7 +180,7 @@ BOOST_AUTO_TEST_CASE(test_rnea_with_armature) using namespace pinocchio; Model model; - buildModels::humanoidRandom(model); + buildModels::humanoidRandom(model, true); model.armature = VectorXd::Random(model.nv) + VectorXd::Ones(model.nv); model.lowerPositionLimit.head<3>().fill(-1.); @@ -209,7 +210,7 @@ BOOST_AUTO_TEST_CASE(test_compute_gravity) using namespace pinocchio; Model model; - buildModels::humanoidRandom(model); + buildModels::humanoidRandom(model, true); model.lowerPositionLimit.head<3>().fill(-1.); model.upperPositionLimit.head<3>().fill(1.); @@ -239,7 +240,7 @@ BOOST_AUTO_TEST_CASE(test_compute_static_torque) using namespace pinocchio; Model model; - buildModels::humanoidRandom(model); + buildModels::humanoidRandom(model, true); model.lowerPositionLimit.head<3>().fill(-1.); model.upperPositionLimit.head<3>().fill(1.); @@ -285,7 +286,7 @@ BOOST_AUTO_TEST_CASE(test_compute_coriolis) const double prec = Eigen::NumTraits::dummy_precision(); Model model; - buildModels::humanoidRandom(model); + buildModels::humanoidRandom(model, true); model.lowerPositionLimit.head<3>().fill(-1.); model.upperPositionLimit.head<3>().fill(1.); @@ -344,4 +345,113 @@ BOOST_AUTO_TEST_CASE(test_compute_coriolis) } } +void test_mimic_against_full_model( + const pinocchio::Model & model_full, + const pinocchio::JointIndex & primary_id, + const pinocchio::JointIndex & secondary_id) +{ + // constants + const int primary_idxq = model_full.joints[primary_id].idx_q(); + const int primary_idxv = model_full.joints[primary_id].idx_v(); + const int secondary_idxq = model_full.joints[secondary_id].idx_q(); + const int secondary_idxv = model_full.joints[secondary_id].idx_v(); + const double ratio = 2.5; + const double offset = 0.75; + + // Build mimic model + pinocchio::Model model_mimic; + pinocchio::transformJointIntoMimic( + model_full, primary_id, secondary_id, ratio, offset, model_mimic); + pinocchio::Data data_nle_mimic(model_mimic); + pinocchio::Data data_rnea_mimic(model_mimic); + pinocchio::Data data_full(model_full); + + // Prepare test data + Eigen::MatrixXd G = Eigen::MatrixXd::Zero(model_full.nv, model_mimic.nv); + G.topLeftCorner(secondary_idxv, secondary_idxv).setIdentity(); + G.bottomRightCorner(model_mimic.nv - secondary_idxv, model_mimic.nv - secondary_idxv) + .setIdentity(); + G(secondary_idxv, primary_idxv) = ratio; + + Eigen::VectorXd q = pinocchio::randomConfiguration(model_mimic); + Eigen::VectorXd v = Eigen::VectorXd::Random(model_mimic.nv); + Eigen::VectorXd a = Eigen::VectorXd::Random(model_mimic.nv); + + Eigen::VectorXd q_full = Eigen::VectorXd::Zero(model_full.nq); + Eigen::VectorXd v_full = G * v; + Eigen::VectorXd a_full = G * v; + + for (int n = 1; n < model_full.njoints; n++) + { + const double joint_ratio = ((n == secondary_id) ? ratio : 1.0); + const double joint_offset = ((n == secondary_id) ? offset : 0.0); + model_full.joints[n].jointConfigFromDofSelector(q_full) = + joint_ratio * model_mimic.joints[n].jointConfigFromDofSelector(q) + + joint_offset * Eigen::VectorXd::Ones(model_full.joints[n].nq()); + } + + pinocchio::crba(model_full, data_full, q_full, pinocchio::Convention::WORLD); + data_full.M.triangularView() = + data_full.M.transpose().triangularView(); + const Eigen::VectorXd nle = pinocchio::nonLinearEffects(model_full, data_full, q_full, v_full); + + // // Use equation of motion to compute tau from a_reduced + Eigen::VectorXd tau_ref = G.transpose() * data_full.M * G * a + (G.transpose() * nle); + + pinocchio::rnea(model_mimic, data_rnea_mimic, q, v, a); + BOOST_CHECK(tau_ref.isApprox(data_rnea_mimic.tau)); + + // NLE + pinocchio::Data data_nle(model_mimic); + pinocchio::Data data_ref_nle(model_mimic); + Eigen::VectorXd tau_ref_nle = + pinocchio::rnea(model_mimic, data_ref_nle, q, v, Eigen::VectorXd::Zero(model_mimic.nv)); + Eigen::VectorXd tau_nle = pinocchio::nonLinearEffects(model_mimic, data_nle, q, v); + BOOST_CHECK(tau_nle.isApprox(tau_nle)); + + // Generalized Gravity + pinocchio::Data data_ref_gg(model_mimic); + pinocchio::Data data_gg(model_mimic); + Eigen::VectorXd tau_ref_gg = pinocchio::rnea( + model_mimic, data_ref_gg, q, Eigen::VectorXd::Zero(model_mimic.nv), + Eigen::VectorXd::Zero(model_mimic.nv)); + Eigen::VectorXd tau_gg = pinocchio::computeGeneralizedGravity(model_mimic, data_gg, q); + BOOST_CHECK(tau_gg.isApprox(tau_ref_gg)); + + // Static Torque + typedef PINOCCHIO_ALIGNED_STD_VECTOR(pinocchio::Force) ForceVector; + ForceVector fext((size_t)model_mimic.njoints); + for (ForceVector::iterator it = fext.begin(); it != fext.end(); ++it) + (*it).setRandom(); + + pinocchio::Data data_ref_st(model_mimic); + pinocchio::Data data_st(model_mimic); + pinocchio::rnea( + model_mimic, data_ref_st, q, Eigen::VectorXd::Zero(model_mimic.nv), + Eigen::VectorXd::Zero(model_mimic.nv), fext); + Eigen::VectorXd tau_st = pinocchio::computeStaticTorque(model_mimic, data_gg, q, fext); + BOOST_CHECK(tau_st.isApprox(data_ref_st.tau)); +} + +BOOST_AUTO_TEST_CASE(test_rnea_mimic) +{ + pinocchio::Model humanoid_model; + pinocchio::buildModels::humanoidRandom(humanoid_model); + + // Test for direct parent/child joint mimic + test_mimic_against_full_model( + humanoid_model, humanoid_model.getJointId("rleg1_joint"), + humanoid_model.getJointId("rleg2_joint")); + + // Test for spaced parent/child joint mimic + test_mimic_against_full_model( + humanoid_model, humanoid_model.getJointId("rleg1_joint"), + humanoid_model.getJointId("rleg4_joint")); + + // // Test for parallel joint mimic + test_mimic_against_full_model( + humanoid_model, humanoid_model.getJointId("lleg4_joint"), + humanoid_model.getJointId("rleg4_joint")); +} + BOOST_AUTO_TEST_SUITE_END() diff --git a/unittest/sample-models.cpp b/unittest/sample-models.cpp index 4ad4815988..b716b43b5c 100644 --- a/unittest/sample-models.cpp +++ b/unittest/sample-models.cpp @@ -9,8 +9,29 @@ #include "pinocchio/algorithm/kinematics.hpp" #include "pinocchio/algorithm/geometry.hpp" #include "pinocchio/multibody/sample-models.hpp" +#include +#include #include +#include + +// Helper functions to map reduced to full model +Eigen::VectorXd +toFull(const Eigen::VectorXd & vec, int mimPrim, int mimSec, double scaling, double offset) +{ + Eigen::VectorXd vecFull(vec.size() + 1); + vecFull << vec.head(mimSec), scaling * vec[mimPrim] + offset, vec.tail(vec.size() - mimSec); + return vecFull; +} + +Eigen::MatrixXd create_G(const pinocchio::Model & model, const pinocchio::Model & modelMimic) +{ + Eigen::MatrixXd G = Eigen::MatrixXd::Zero(model.nv, modelMimic.nv); + for (int i = 0; i < modelMimic.nv; ++i) + G(i, i) = 1; + + return G; +} BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) @@ -27,15 +48,27 @@ BOOST_AUTO_TEST_CASE(build_model_sample_humanoid_random) BOOST_CHECK(modelff.nq == 32); BOOST_CHECK(modelff.nv == 32); + + pinocchio::Model modelMimic; + pinocchio::buildModels::humanoidRandom(modelMimic, true, true); + + BOOST_CHECK(modelMimic.nq == 32); + BOOST_CHECK(modelMimic.nv == 31); + + pinocchio::Model modelMimicff; + pinocchio::buildModels::humanoidRandom(modelMimicff, false, true); + + BOOST_CHECK(modelMimicff.nq == 31); + BOOST_CHECK(modelMimicff.nv == 31); } BOOST_AUTO_TEST_CASE(build_model_sample_manipulator) { pinocchio::Model model; - pinocchio::buildModels::manipulator(model); + pinocchio::buildModels::manipulator(model, true); - BOOST_CHECK(model.nq == 6); - BOOST_CHECK(model.nv == 6); + BOOST_CHECK(model.nq == 5); + BOOST_CHECK(model.nv == 5); #ifdef PINOCCHIO_WITH_HPP_FCL pinocchio::Data data(model); @@ -67,4 +100,71 @@ BOOST_AUTO_TEST_CASE(build_model_sample_humanoid) * direct geometry with respect to reference values. */ } +BOOST_AUTO_TEST_CASE(compare_mimic) +{ + pinocchio::Model model, model_m; + pinocchio::buildModels::manipulator(model); + pinocchio::buildModels::manipulator(model_m, true); + + // Reduced model configuration (with the mimic joint) + Eigen::VectorXd q_reduced = pinocchio::neutral(model_m); + Eigen::VectorXd v_reduced = Eigen::VectorXd::Random(model_m.nv); + Eigen::VectorXd a_reduced = Eigen::VectorXd::Random(model_m.nv); + Eigen::VectorXd tau_reduced = Eigen::VectorXd::Random(model_m.nv); + + auto jointMimic = boost::get( + model_m.joints[model_m.getJointId("wrist1_mimic_joint")]); + double scaling = jointMimic.scaling(); + double offset = jointMimic.offset(); + // Reduced model configuration (without the mimic joint) + Eigen::VectorXd q_full = toFull(q_reduced, 4, 5, scaling, offset); + Eigen::VectorXd v_full = toFull(v_reduced, 4, 5, scaling, 0); + Eigen::VectorXd a_full = toFull(a_reduced, 4, 5, scaling, 0); + + pinocchio::Data dataFKFull(model); + pinocchio::forwardKinematics(model, dataFKFull, q_full, v_full, a_full); + + pinocchio::Data dataFKRed(model_m); + pinocchio::forwardKinematics(model_m, dataFKRed, q_reduced, v_reduced, a_reduced); + + for (int i = 0; i < model.njoints; i++) + { + BOOST_CHECK(dataFKRed.oMi[i].isApprox(dataFKFull.oMi[i])); + BOOST_CHECK(dataFKRed.liMi[i].isApprox(dataFKFull.liMi[i])); + BOOST_CHECK(model.inertias[i].isApprox(model_m.inertias[i])); + } + + // Build G matrix to go from full to reduced system + Eigen::MatrixXd G = create_G(model, model_m); + G(model.nv - 1, model_m.nv - 1) = scaling; + + // Test crba + pinocchio::Data dataCRBAMimic(model_m); + pinocchio::crba(model_m, dataCRBAMimic, q_reduced); + dataCRBAMimic.M.triangularView() = + dataCRBAMimic.M.transpose().triangularView(); + + pinocchio::Data dataCRBAFull(model); + pinocchio::crba(model, dataCRBAFull, q_full); + dataCRBAFull.M.triangularView() = + dataCRBAFull.M.transpose().triangularView(); + + Eigen::MatrixXd M_reduced_computed = (G.transpose() * dataCRBAFull.M * G); + + BOOST_CHECK((M_reduced_computed - dataCRBAMimic.M).isZero()); + + // Test rnea + pinocchio::Data dataBiasFull(model); + pinocchio::nonLinearEffects(model, dataBiasFull, q_full, v_full); + Eigen::VectorXd C_full = dataBiasFull.nle; + + // // Use equation of motion to compute tau from a_reduced + Eigen::VectorXd tau_reduced_computed = M_reduced_computed * a_reduced + (G.transpose() * C_full); + + pinocchio::Data dataRneaRed(model_m); + pinocchio::rnea(model_m, dataRneaRed, q_reduced, v_reduced, a_reduced); + + BOOST_CHECK((dataRneaRed.tau - tau_reduced_computed).isZero()); +} + BOOST_AUTO_TEST_SUITE_END() diff --git a/unittest/serialization.cpp b/unittest/serialization.cpp index 2ff9f8e57a..68dafa72e1 100644 --- a/unittest/serialization.cpp +++ b/unittest/serialization.cpp @@ -272,7 +272,7 @@ struct init static JointModel_ run() { JointModel_ jmodel; - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -287,7 +287,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -302,7 +302,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -317,7 +317,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -332,7 +332,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -346,7 +346,7 @@ struct init> { JointModel jmodel(static_cast(0.0)); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -361,7 +361,7 @@ struct init> typedef pinocchio::JointModelRevoluteTpl JointModelRX; JointModel jmodel((JointModelRX())); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -375,7 +375,7 @@ struct init> { JointModel jmodel(pinocchio::XAxis::vector(), pinocchio::YAxis::vector()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -392,21 +392,23 @@ struct init> JointModel jmodel((JointModelRX())); jmodel.addJoint(JointModelRY()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; -template -struct init> +template class JointCollection> +struct init> { - typedef pinocchio::JointModelMimic JointModel; + typedef pinocchio::JointModelMimicTpl JointModel; static JointModel run() { - JointModel_ jmodel_ref = init::run(); + typedef pinocchio::JointModelRevoluteTpl JointModelRX; + JointModelRX jmodel_ref = init::run(); JointModel jmodel(jmodel_ref, 1., 0.); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } @@ -471,35 +473,38 @@ struct TestJointTransform // Do nothing } - template - void operator()(const pinocchio::JointModelMimic &) + template class JointCollection> + void operator()(const pinocchio::JointModelMimicTpl &) { - typedef pinocchio::JointModelMimic JointModelMimic; - typedef typename JointModelMimic::JointDerived JointDerived; - typedef typename pinocchio::traits::Transformation_t Transform; - typedef typename pinocchio::traits::Constraint_t Constraint; - typedef typename pinocchio::traits::JointDataDerived JointDataMimic; - typedef pinocchio::JointDataBase JointDataBase; - JointModelMimic jmodel_mimic = init::run(); - JointModel jmodel = init::run(); + // typedef pinocchio::JointModelMimicTpl JointModelMimic; + // typedef typename JointModelMimic::JointDerived JointDerived; + // typedef typename pinocchio::traits::Transformation_t Transform; + // typedef typename pinocchio::traits::Constraint_t Constraint; + // typedef typename pinocchio::traits::JointDataDerived JointDataMimic; + // typedef pinocchio::JointDataBase JointDataBase; + // JointModelMimic jmodel_mimic = init::run(); + // typedef pinocchio::JointModelRevoluteTpl JointModel; + // JointModel jmodel = init::run(); - JointDataMimic jdata_mimic = jmodel_mimic.createData(); - JointDataBase & jdata_mimic_base = static_cast(jdata_mimic); + // JointDataMimic jdata_mimic = jmodel_mimic.createData(); + // JointDataBase & jdata_mimic_base = static_cast(jdata_mimic); - typedef typename pinocchio::LieGroup::type LieGroupType; - LieGroupType lg; + // typedef typename pinocchio::LieGroup::type LieGroupType; + // LieGroupType lg; - Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(), -1.)); - Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.)); + // Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(),-1.)); + // Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.)); - Eigen::VectorXd q_random = lg.randomConfiguration(lb, ub); + // Eigen::VectorXd q_random = lg.randomConfiguration(lb,ub); - jmodel_mimic.calc(jdata_mimic, q_random); - Transform & m = jdata_mimic_base.M(); - test(m); + // jmodel_mimic.calc(jdata_mimic,q_random); + // // Copy Transform (and don't take ref) as non const ref of mimic buffers trigger assert + // Transform m = const_cast(jdata_mimic_base).M(); + // test(m); - Constraint & S = jdata_mimic_base.S(); - test(S); + // // Copy Constraint (and don't take ref) as non const ref of mimic buffers trigger assert + // Constraint S = const_cast(jdata_mimic_base).S(); + // test(S); } template @@ -554,37 +559,40 @@ struct TestJointMotion // Do nothing } - template - void operator()(const pinocchio::JointModelMimic &) + template class JointCollection> + void operator()(const pinocchio::JointModelMimicTpl &) { - typedef pinocchio::JointModelMimic JointModelMimic; - typedef typename JointModelMimic::JointDerived JointDerived; - typedef typename pinocchio::traits::Motion_t Motion; - typedef typename pinocchio::traits::Bias_t Bias; - typedef typename pinocchio::traits::JointDataDerived JointDataMimic; - typedef pinocchio::JointDataBase JointDataBase; - JointModelMimic jmodel_mimic = init::run(); - JointModel jmodel = init::run(); + // typedef pinocchio::JointModelMimicTpl JointModelMimic; + // typedef typename JointModelMimic::JointDerived JointDerived; + // typedef typename pinocchio::traits::Motion_t Motion; + // typedef typename pinocchio::traits::Bias_t Bias; + // typedef typename pinocchio::traits::JointDataDerived JointDataMimic; + // typedef pinocchio::JointDataBase JointDataBase; + // JointModelMimic jmodel_mimic = init::run(); + // typedef pinocchio::JointModelRevoluteTpl JointModel; + // JointModel jmodel = init::run(); - JointDataMimic jdata_mimic = jmodel_mimic.createData(); - JointDataBase & jdata_mimic_base = static_cast(jdata_mimic); + // JointDataMimic jdata_mimic = jmodel_mimic.createData(); + // JointDataBase & jdata_mimic_base = static_cast(jdata_mimic); - typedef typename pinocchio::LieGroup::type LieGroupType; - LieGroupType lg; + // typedef typename pinocchio::LieGroup::type LieGroupType; + // LieGroupType lg; - Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(), -1.)); - Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.)); + // Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(),-1.)); + // Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.)); - Eigen::VectorXd q_random = lg.randomConfiguration(lb, ub); - Eigen::VectorXd v_random = Eigen::VectorXd::Random(jmodel.nv()); + // Eigen::VectorXd q_random = lg.randomConfiguration(lb,ub); + // Eigen::VectorXd v_random = Eigen::VectorXd::Random(jmodel.nv()); - jmodel_mimic.calc(jdata_mimic, q_random, v_random); - Motion & m = jdata_mimic_base.v(); + // jmodel_mimic.calc(jdata_mimic,q_random,v_random); - test(m); + // // Copy Motion (and don't take ref) as non const ref of mimic buffers trigger assert + // Motion m = const_cast(jdata_mimic_base).v(); + // test(m); - Bias & b = jdata_mimic_base.c(); - test(b); + // // Copy Bias (and don't take ref) as non const ref of mimic buffers trigger assert + // Bias b = const_cast(jdata_mimic_base).c(); + // test(b); } template @@ -654,30 +662,32 @@ struct TestJointData test(jdata); } - template - void operator()(const pinocchio::JointModelMimic &) + template class JointCollection> + void operator()(const pinocchio::JointModelMimicTpl &) { - typedef pinocchio::JointModelMimic JointModelMimic; - typedef typename JointModelMimic::JointDerived JointDerived; - typedef typename pinocchio::traits::JointDataDerived JointDataMimic; - JointModelMimic jmodel_mimic = init::run(); - JointModel jmodel = init::run(); + // typedef pinocchio::JointModelMimicTpl JointModelMimic; + // typedef typename JointModelMimic::JointDerived JointDerived; + // typedef typename pinocchio::traits::JointDataDerived JointDataMimic; + // JointModelMimic jmodel_mimic = init::run(); - JointDataMimic jdata_mimic = jmodel_mimic.createData(); + // typedef pinocchio::JointModelRevoluteTpl JointModel; + // JointModel jmodel = init::run(); - typedef typename pinocchio::LieGroup::type LieGroupType; - LieGroupType lg; + // JointDataMimic jdata_mimic = jmodel_mimic.createData(); - Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(), -1.)); - Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.)); + // typedef typename pinocchio::LieGroup::type LieGroupType; + // LieGroupType lg; - Eigen::VectorXd q_random = lg.randomConfiguration(lb, ub); - Eigen::VectorXd v_random = Eigen::VectorXd::Random(jmodel.nv()); + // Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(),-1.)); + // Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.)); - jmodel_mimic.calc(jdata_mimic, q_random, v_random); - pinocchio::Inertia::Matrix6 I(pinocchio::Inertia::Matrix6::Identity()); - jmodel_mimic.calc_aba(jdata_mimic, Eigen::VectorXd::Zero(jmodel.nv()), I, false); - test(jdata_mimic); + // Eigen::VectorXd q_random = lg.randomConfiguration(lb,ub); + // Eigen::VectorXd v_random = Eigen::VectorXd::Random(jmodel.nv()); + + // jmodel_mimic.calc(jdata_mimic,q_random,v_random); + // pinocchio::Inertia::Matrix6 I(pinocchio::Inertia::Matrix6::Identity()); + // jmodel_mimic.calc_aba(jdata_mimic, Eigen::VectorXd::Zero(jmodel.nv()), I, false); + // test(jdata_mimic); } template @@ -698,7 +708,7 @@ BOOST_AUTO_TEST_CASE(test_model_serialization) using namespace pinocchio; Model model; - buildModels::humanoidRandom(model); + buildModels::humanoidRandom(model, true, true); generic_test(model, TEST_SERIALIZATION_FOLDER "/Model", "Model"); } @@ -708,7 +718,7 @@ BOOST_AUTO_TEST_CASE(test_throw_extension) using namespace pinocchio; Model model; - buildModels::humanoidRandom(model); + buildModels::humanoidRandom(model, true, true); const std::string & fake_filename = "this_is_a_fake_filename"; @@ -743,7 +753,7 @@ BOOST_AUTO_TEST_CASE(test_data_serialization) using namespace pinocchio; Model model; - buildModels::humanoidRandom(model); + buildModels::humanoidRandom(model, true, true); Data data(model); diff --git a/unittest/visitor.cpp b/unittest/visitor.cpp index 792cdfdf90..8b4db0d743 100644 --- a/unittest/visitor.cpp +++ b/unittest/visitor.cpp @@ -253,16 +253,19 @@ struct init> } }; -template -struct init> +template class JointCollection> +struct init> { - typedef pinocchio::JointModelMimic JointModel; + typedef pinocchio::JointModelMimicTpl JointModel; static JointModel run(const pinocchio::Model & model) { - const pinocchio::JointIndex joint_id = model.getJointId(JointModel_::classname()); + typedef pinocchio::JointModelRevoluteTpl JointModelRX; + JointModelRX jmodel_ref = init::run(model); + jmodel_ref.setIndexes(0, 0, 0, 0); - JointModel jmodel(boost::get(model.joints[joint_id]), 1., 0.); + JointModel jmodel(jmodel_ref, 1., 0.); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; }