diff --git a/CHANGELOG.md b/CHANGELOG.md index a49bd546f3..02e0574c0a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -48,6 +48,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/algorithm/expose-model.cpp b/bindings/python/algorithm/expose-model.cpp index 0b38ec4630..8e3b3b0048 100644 --- a/bindings/python/algorithm/expose-model.cpp +++ b/bindings/python/algorithm/expose-model.cpp @@ -83,6 +83,22 @@ namespace pinocchio return bp::make_tuple(ancestor_id, index_ancestor_in_support1, index_ancestor_in_support2); } + template class JointCollectionTpl> + ModelTpl transformJointIntoMimic_proxy( + const ModelTpl & input_model, + const JointIndex & index_primary, + const JointIndex & index_secondary, + const Scalar & scaling, + const Scalar & offset) + { + typedef ModelTpl Model; + + Model model; + pinocchio::transformJointIntoMimic( + input_model, index_primary, index_secondary, scaling, offset, model); + return model; + } + void exposeModelAlgo() { using namespace Eigen; @@ -175,6 +191,19 @@ namespace pinocchio "\tjoint2_id: index of the second joint\n" "Returns a tuple containing the index of the common joint ancestor, the position of this " "ancestor in model.supports[joint1_id] and model.supports[joint2_id].\n"); + + bp::def( + "transformJointIntoMimic", + transformJointIntoMimic_proxy, + bp::args("input_model", "index_primary", "index_secondary", "scaling", "offset"), + "Transform of a joint of the model into a mimic joint. Keep the type of the joint as it " + "was previously. \n\n" + "Parameters:\n" + "\tinput_model: model the input model to take joints from.\n" + "\tindex_primary: index of the joint to mimic\n" + "\tindex_secondary: index of the joint that will mimic\n" + "\tscaling: Scaling of joint velocity and configuration\n" + "\toffset: Offset of joint configuration\n"); } } // namespace python diff --git a/bindings/python/multibody/sample-models.cpp b/bindings/python/multibody/sample-models.cpp index 295e04f723..1ae4679dbf 100644 --- a/bindings/python/multibody/sample-models.cpp +++ b/bindings/python/multibody/sample-models.cpp @@ -12,24 +12,17 @@ namespace pinocchio { namespace bp = boost::python; - Model buildSampleModelHumanoidRandom() + Model buildSampleModelHumanoidRandom(bool usingFF, bool mimic) { Model model; - buildModels::humanoidRandom(model); + buildModels::humanoidRandom(model, usingFF, mimic); return model; } - Model buildSampleModelHumanoidRandom(bool usingFF) + Model buildSampleModelManipulator(bool mimic) { Model model; - buildModels::humanoidRandom(model, usingFF); - return model; - } - - Model buildSampleModelManipulator() - { - Model model; - buildModels::manipulator(model); + buildModels::manipulator(model, mimic); return model; } @@ -42,13 +35,6 @@ namespace pinocchio } #endif - Model buildSampleModelHumanoid() - { - Model model; - buildModels::humanoid(model); - return model; - } - Model buildSampleModelHumanoid(bool usingFF) { Model model; @@ -69,21 +55,15 @@ namespace pinocchio { bp::def( "buildSampleModelHumanoidRandom", - static_cast(pinocchio::python::buildSampleModelHumanoidRandom), - "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"), + static_cast(pinocchio::python::buildSampleModelHumanoidRandom), + (bp::arg("using_free_flyer") = true, bp::arg("mimic") = false), "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."); + static_cast(pinocchio::python::buildSampleModelManipulator), + (bp::arg("mimic") = false), "Generate a (hard-coded) model of a simple manipulator."); #ifdef PINOCCHIO_WITH_HPP_FCL bp::def( @@ -93,15 +73,11 @@ namespace pinocchio bp::args("model"), "Generate a (hard-coded) geometry model of a simple manipulator."); #endif - bp::def( - "buildSampleModelHumanoid", - static_cast(pinocchio::python::buildSampleModelHumanoid), - "Generate a (hard-coded) model of a simple humanoid."); - bp::def( "buildSampleModelHumanoid", static_cast(pinocchio::python::buildSampleModelHumanoid), - bp::args("using_free_flyer"), "Generate a (hard-coded) model of a simple humanoid."); + (bp::arg("using_free_flyer") = true), + "Generate a (hard-coded) model of a simple humanoid."); #ifdef PINOCCHIO_WITH_HPP_FCL bp::def( diff --git a/bindings/python/parsers/urdf/model.cpp b/bindings/python/parsers/urdf/model.cpp index a1003f1191..d94ff3539b 100644 --- a/bindings/python/parsers/urdf/model.cpp +++ b/bindings/python/parsers/urdf/model.cpp @@ -20,71 +20,81 @@ namespace pinocchio #ifdef PINOCCHIO_WITH_URDFDOM - Model buildModelFromUrdf(const bp::object & filename) + Model buildModelFromUrdf(const bp::object & filename, const bool mimic) { Model model; - pinocchio::urdf::buildModel(path(filename), model); + pinocchio::urdf::buildModel(path(filename), model, false, mimic); return model; } - Model & buildModelFromUrdf(const bp::object & filename, Model & model) + Model & buildModelFromUrdf(const bp::object & filename, Model & model, const bool mimic) { - return pinocchio::urdf::buildModel(path(filename), model); + return pinocchio::urdf::buildModel(path(filename), model, false, mimic); } - Model buildModelFromUrdf(const bp::object & filename, const JointModel & root_joint) + Model + buildModelFromUrdf(const bp::object & filename, const JointModel & root_joint, const bool mimic) { Model model; - pinocchio::urdf::buildModel(path(filename), root_joint, model); + pinocchio::urdf::buildModel(path(filename), root_joint, model, false, 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) { Model model; - pinocchio::urdf::buildModel(path(filename), root_joint, root_joint_name, model); + pinocchio::urdf::buildModel(path(filename), root_joint, root_joint_name, model, false, 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) { - return pinocchio::urdf::buildModel(path(filename), root_joint, model); + return pinocchio::urdf::buildModel(path(filename), root_joint, model, false, mimic); } Model & buildModelFromUrdf( const bp::object & filename, const JointModel & root_joint, const std::string & root_joint_name, - Model & model) + Model & model, + const bool mimic) { - return pinocchio::urdf::buildModel(path(filename), root_joint, root_joint_name, model); + return pinocchio::urdf::buildModel( + path(filename), root_joint, root_joint_name, model, false, 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) { Model model; - pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, model); + pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, model, false, 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) { Model model; - pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, root_joint_name, model); + pinocchio::urdf::buildModelFromXML( + xml_stream, root_joint, root_joint_name, model, false, 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) { - pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, model); + pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, model, false, mimic); return model; } @@ -92,22 +102,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) { - pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, root_joint_name, model); + pinocchio::urdf::buildModelFromXML( + xml_stream, root_joint, root_joint_name, model, false, mimic); return model; } - Model buildModelFromXML(const std::string & xml_stream) + Model buildModelFromXML(const std::string & xml_stream, const bool mimic) { Model model; - pinocchio::urdf::buildModelFromXML(xml_stream, model); + pinocchio::urdf::buildModelFromXML(xml_stream, model, false, mimic); return model; } - Model & buildModelFromXML(const std::string & xml_stream, Model & model) + Model & buildModelFromXML(const std::string & xml_stream, Model & model, const bool mimic) { - pinocchio::urdf::buildModelFromXML(xml_stream, model); + pinocchio::urdf::buildModelFromXML(xml_stream, model, false, mimic); return model; } @@ -120,39 +132,43 @@ namespace pinocchio bp::def( "buildModelFromUrdf", - static_cast( + static_cast( pinocchio::python::buildModelFromUrdf), - bp::args("urdf_filename", "root_joint"), + (bp::arg("urdf_filename"), bp::arg("root_joint"), bp::arg("mimic") = false), "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::arg("urdf_filename"), bp::arg("root_joint"), bp::arg("root_joint_name"), + bp::arg("mimic") = false), "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::arg("urdf_filename"), bp::arg("mimic") = false), "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::arg("urdf_filename"), bp::arg("model"), bp::arg("mimic") = false), "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::arg("urdf_filename"), bp::arg("root_joint"), bp::arg("model"), + bp::arg("mimic") = false), "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 +177,9 @@ 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::arg("urdf_filename"), bp::arg("root_joint"), bp::arg("root_joint_name"), + bp::arg("model"), bp::arg("mimic") = false), "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 +188,28 @@ namespace pinocchio bp::def( "buildModelFromXML", - static_cast( + static_cast( pinocchio::python::buildModelFromXML), - bp::args("urdf_xml_stream", "root_joint"), + (bp::arg("urdf_xml_stream"), bp::arg("root_joint"), bp::arg("mimic") = false), "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::arg("urdf_xml_stream"), bp::arg("root_joint"), bp::arg("root_joint_name"), + bp::arg("mimic") = false), "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::arg("urdf_xml_stream"), bp::arg("root_joint"), bp::arg("model"), + bp::arg("mimic") = false), "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 +217,25 @@ 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::arg("urdf_xml_stream"), bp::arg("root_joint"), bp::arg("root_joint_name"), + bp::arg("model"), bp::arg("mimic") = false), "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::arg("urdf_xml_stream"), bp::arg("mimic") = false), "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::arg("urdf_xml_stream"), bp::arg("model"), bp::arg("mimic") = false), "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 949794a2e6..2fa33b9b3f 100644 --- a/bindings/python/pinocchio/shortcuts.py +++ b/bindings/python/pinocchio/shortcuts.py @@ -26,6 +26,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. @@ -35,7 +36,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 = [ @@ -45,6 +53,7 @@ def buildModelsFromUrdf( "verbose", "meshLoader", "geometry_types", + "mimic", ] for key, arg in zip(arg_keys, args): @@ -64,16 +73,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/mimic_dynamics.py b/examples/mimic_dynamics.py new file mode 100644 index 0000000000..f393c34d4c --- /dev/null +++ b/examples/mimic_dynamics.py @@ -0,0 +1,68 @@ +from pathlib import Path + +import numpy as np +import pinocchio + +# Get URDF model +pinocchio_model_dir = Path(__file__).parent.parent / "models" +model_path = pinocchio_model_dir / "baxter_simple.urdf" +model_full = pinocchio.buildModelFromUrdf(model_path) + +# To use the mimic tag from URDF set mimic flag to true +# (otherwise the tag will simply be ignored) +model_mimic_from_urdf = pinocchio.buildModelFromUrdf(model_path, mimic=True) + +print(f"{model_full.nq=}") +print(f"{model_mimic_from_urdf.nq=}") + +# Alternatively a "mimic" model can also be made by-hand +model_mimic = pinocchio.transformJointIntoMimic(model_full, 9, 10, -1.0, 0.0) +model_mimic = pinocchio.transformJointIntoMimic(model_mimic, 18, 19, -1.0, 0.0) + +print(f"{(model_mimic_from_urdf == model_mimic)=}") # True + +# Creating manually the G matrix (cf. Featherstone's Rigid Body Dynamics Algorithms, +# chapter 10 about Gears if you want to know +# more about how joints mimic are handled here) +G = np.zeros([model_mimic.nv, model_full.nv]) +for i in range(model_full.njoints): + mimic_scaling = getattr(model_mimic.joints[i].extract(), "scaling", None) + if mimic_scaling: + G[model_mimic.joints[i].idx_v, model_full.joints[i].idx_v] = mimic_scaling + else: + # Not a mimic joint + G[model_mimic.joints[i].idx_v, model_full.joints[i].idx_v] = 1 +print("G = ") +print(np.array_str(G, precision=0, suppress_small=True, max_line_width=80)) + +# Random data +q_mimic = pinocchio.neutral(model_mimic) +v_mimic = np.random.random(model_mimic.nv) +a_mimic = np.random.random(model_mimic.nv) + +# Expand configuration using the G matrix for the full model +# Possible because offset is 0, otherwise +# you need to add it to q of joints mimic +q_full = G.transpose() @ q_mimic + +v_full = G.transpose() @ v_mimic +a_full = G.transpose() @ a_mimic + +# Supported algorithms works as-is +data_mimic = model_mimic.createData() +data_full = model_full.createData() + +tau_mimic = pinocchio.rnea(model_mimic, data_mimic, q_mimic, v_mimic, a_mimic) +tau_full = pinocchio.rnea(model_full, data_full, q_full, v_full, a_full) +print(f"{np.allclose(tau_mimic, G @ tau_full)=}") + +M_mimic = pinocchio.crba(model_mimic, data_mimic, q_mimic) +M_full = pinocchio.crba(model_full, data_full, q_full) +print(f"{np.allclose(M_mimic, G @ M_full @ G.transpose())=}") + +C_full = pinocchio.nle(model_full, data_full, q_full, v_full) +# For forward dynamics, aba does not support joints mimic. +# However it's still possible to compute acceleration of the +# mimic model, using the G matrix +a_computed = np.linalg.solve(G @ M_full @ G.transpose(), tau_mimic - G @ C_full) +print(f"{np.allclose(a_mimic,a_computed)=}") diff --git a/include/pinocchio/algorithm/aba-derivatives.hxx b/include/pinocchio/algorithm/aba-derivatives.hxx index 2e2205c471..c50cb74eef 100644 --- a/include/pinocchio/algorithm/aba-derivatives.hxx +++ b/include/pinocchio/algorithm/aba-derivatives.hxx @@ -403,6 +403,7 @@ namespace pinocchio isZero(model.gravity.angular()), "The gravity must be a pure force vector, no angular part"); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef typename ModelTpl::JointIndex JointIndex; @@ -490,6 +491,7 @@ namespace pinocchio isZero(model.gravity.angular()), "The gravity must be a pure force vector, no angular part"); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef typename ModelTpl::JointIndex JointIndex; @@ -727,6 +729,7 @@ namespace pinocchio isZero(model.gravity.angular()), "The gravity must be a pure force vector, no angular part"); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef typename ModelTpl::JointIndex JointIndex; @@ -791,6 +794,7 @@ namespace pinocchio isZero(model.gravity.angular()), "The gravity must be a pure force vector, no angular part"); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef typename ModelTpl::JointIndex JointIndex; diff --git a/include/pinocchio/algorithm/aba.hxx b/include/pinocchio/algorithm/aba.hxx index e0798504d4..f03c2e2205 100644 --- a/include/pinocchio/algorithm/aba.hxx +++ b/include/pinocchio/algorithm/aba.hxx @@ -247,6 +247,7 @@ namespace pinocchio const Eigen::MatrixBase & tau) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); PINOCCHIO_CHECK_INPUT_ARGUMENT( q.size() == model.nq, "The joint configuration vector is not of right size"); PINOCCHIO_CHECK_INPUT_ARGUMENT( @@ -310,6 +311,8 @@ namespace pinocchio { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + PINOCCHIO_CHECK_INPUT_ARGUMENT( q.size() == model.nq, "The joint configuration vector is not of right size"); PINOCCHIO_CHECK_INPUT_ARGUMENT( @@ -488,6 +491,8 @@ namespace pinocchio const Eigen::MatrixBase & tau) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + PINOCCHIO_CHECK_ARGUMENT_SIZE( q.size(), model.nq, "The joint configuration vector is not of right size"); PINOCCHIO_CHECK_ARGUMENT_SIZE( @@ -553,6 +558,8 @@ namespace pinocchio { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + PINOCCHIO_CHECK_ARGUMENT_SIZE( q.size(), model.nq, "The joint configuration vector is not of right size"); PINOCCHIO_CHECK_ARGUMENT_SIZE( @@ -834,6 +841,8 @@ namespace pinocchio const Eigen::MatrixBase & q) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + PINOCCHIO_CHECK_ARGUMENT_SIZE( q.size(), model.nq, "The joint configuration vector is not of right size"); @@ -871,6 +880,7 @@ namespace pinocchio DataTpl & data) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef typename ModelTpl::JointIndex JointIndex; data.Minv.template triangularView().setZero(); diff --git a/include/pinocchio/algorithm/center-of-mass-derivatives.hxx b/include/pinocchio/algorithm/center-of-mass-derivatives.hxx index 6bd999b8f6..7394ead69f 100644 --- a/include/pinocchio/algorithm/center-of-mass-derivatives.hxx +++ b/include/pinocchio/algorithm/center-of-mass-derivatives.hxx @@ -76,6 +76,7 @@ namespace pinocchio PINOCCHIO_CHECK_ARGUMENT_SIZE(vcom_partial_dq.cols(), model.nv); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef ModelTpl Model; typedef typename Model::JointIndex JointIndex; diff --git a/include/pinocchio/algorithm/center-of-mass.hxx b/include/pinocchio/algorithm/center-of-mass.hxx index 11ff1cea47..700ab33234 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.jointExtendedModelCols(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.nvExtended(); ++col_id) { - jmodel.jointCols(Jcom_).col(col_id) = + jmodel.jointCols(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.jointExtendedModelCols(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.nvExtended(); ++col_id) { - jmodel.jointCols(Jcom_).col(col_id) = + jmodel.jointCols(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)); } @@ -411,6 +412,7 @@ namespace pinocchio typedef ModelTpl Model; assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); PINOCCHIO_CHECK_INPUT_ARGUMENT((int)rootSubtreeId < model.njoints, "Invalid joint id."); PINOCCHIO_CHECK_ARGUMENT_SIZE( res.rows(), 3, "the resulting matrix does not have the right size."); @@ -444,6 +446,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 +477,7 @@ namespace pinocchio Jcom_subtree.middleCols(idx_v, nv_subtree) *= mass_inv_subtree; // Second backward step + typedef JacobianSubtreeCenterOfMassBackwardStep< Scalar, Options, JointCollectionTpl, Matrix3xLike> Pass3; @@ -500,6 +504,7 @@ namespace pinocchio typedef DataTpl Data; assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); PINOCCHIO_CHECK_INPUT_ARGUMENT(((int)rootSubtreeId < model.njoints), "Invalid joint id."); PINOCCHIO_CHECK_ARGUMENT_SIZE( res.rows(), 3, "the resulting matrix does not have the right size."); diff --git a/include/pinocchio/algorithm/centroidal-derivatives.hxx b/include/pinocchio/algorithm/centroidal-derivatives.hxx index 68dafb3a06..d835a53ff7 100644 --- a/include/pinocchio/algorithm/centroidal-derivatives.hxx +++ b/include/pinocchio/algorithm/centroidal-derivatives.hxx @@ -280,6 +280,7 @@ namespace pinocchio PINOCCHIO_CHECK_ARGUMENT_SIZE(dhdot_da.cols(), model.nv); PINOCCHIO_CHECK_ARGUMENT_SIZE(dhdot_da.rows(), 6); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef ModelTpl Model; typedef DataTpl Data; @@ -425,6 +426,7 @@ namespace pinocchio PINOCCHIO_CHECK_ARGUMENT_SIZE(dhdot_da.cols(), model.nv); PINOCCHIO_CHECK_ARGUMENT_SIZE(dhdot_da.rows(), 6); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef ModelTpl Model; typedef DataTpl Data; diff --git a/include/pinocchio/algorithm/centroidal.hxx b/include/pinocchio/algorithm/centroidal.hxx index 3e9f1aaafe..be4b9a71f9 100644 --- a/include/pinocchio/algorithm/centroidal.hxx +++ b/include/pinocchio/algorithm/centroidal.hxx @@ -136,11 +136,13 @@ namespace pinocchio const JointIndex & i = jmodel.id(); const JointIndex & parent = model.parents[i]; - ColsBlock J_cols = jmodel.jointCols(data.J); + ColsBlock J_cols = jmodel.jointExtendedModelCols(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); + // Mimic joint contributes to the inertia of their primary and don't have their own + motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); + data.oYcrb[parent] += data.oYcrb[i]; } @@ -173,6 +175,9 @@ namespace pinocchio data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); typedef CcrbaBackwardStep Pass2; + // Set to zero, because it's not an assignation, that is done in the algorithm but a sum to + // take mimic joint into account + 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 +225,9 @@ namespace pinocchio data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); typedef CcrbaBackwardStep Pass2; + // Set to zero, because it's not an assignation, that is done in the algorithm but a sum to + // take mimic joint into account + 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 +272,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.jointExtendedModelCols(data.J); J_cols = data.oMi[i].act(jdata.S()); - ColsBlock dJ_cols = jmodel.jointCols(data.dJ); + ColsBlock dJ_cols = jmodel.jointExtendedModelCols(data.dJ); motionSet::motionAction(data.ov[i], J_cols, dJ_cols); data.oYcrb[parent] += Y; @@ -299,6 +307,7 @@ namespace pinocchio const Eigen::MatrixBase & v) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); PINOCCHIO_CHECK_ARGUMENT_SIZE( q.size(), model.nq, "The configuration vector is not of right size"); PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size"); @@ -361,6 +370,7 @@ namespace pinocchio const Eigen::MatrixBase & v) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); PINOCCHIO_CHECK_ARGUMENT_SIZE( q.size(), model.nq, "The configuration vector is not of right size"); PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size"); diff --git a/include/pinocchio/algorithm/check-base.hpp b/include/pinocchio/algorithm/check-base.hpp new file mode 100644 index 0000000000..a17e6b28e9 --- /dev/null +++ b/include/pinocchio/algorithm/check-base.hpp @@ -0,0 +1,36 @@ +// +// Copyright (c) 2025 INRIA +// + +#ifndef __pinocchio_algorithm_check_base_hpp__ +#define __pinocchio_algorithm_check_base_hpp__ + +#include "pinocchio/multibody/fwd.hpp" + +namespace pinocchio +{ + + /// CRTP class describing the API of the checkers + template + struct AlgorithmCheckerBase + { + AlgorithmCheckerDerived & derived() + { + return *static_cast(this); + } + + const AlgorithmCheckerDerived & derived() const + { + return *static_cast(this); + } + + template class JointCollectionTpl> + bool checkModel(const ModelTpl & model) const + { + return derived().checkModel_impl(model); + } + }; + +} // namespace pinocchio + +#endif // __pinocchio_algorithm_check_base_hpp__ diff --git a/include/pinocchio/algorithm/check-data.hpp b/include/pinocchio/algorithm/check-data.hpp new file mode 100644 index 0000000000..5390066fb2 --- /dev/null +++ b/include/pinocchio/algorithm/check-data.hpp @@ -0,0 +1,29 @@ +// +// Copyright (c) 2025 INRIA +// + +#ifndef __pinocchio_algorithm_check_data_hpp__ +#define __pinocchio_algorithm_check_data_hpp__ + +#include "pinocchio/multibody/fwd.hpp" + +namespace pinocchio +{ + + /// Check the validity of data wrt to model, in particular if model has been modified. + /// + /// \param[in] model reference model + /// \param[in] data corresponding data + /// + /// \returns True if data is valid wrt model. + template class JointCollectionTpl> + bool checkData( + const ModelTpl & model, + const DataTpl & data); + +} // namespace pinocchio + +/* --- Details -------------------------------------------------------------------- */ +#include "pinocchio/algorithm/check-data.hxx" + +#endif // __pinocchio_algorithm_check_data_hpp__ diff --git a/include/pinocchio/algorithm/check.hxx b/include/pinocchio/algorithm/check-data.hxx similarity index 57% rename from include/pinocchio/algorithm/check.hxx rename to include/pinocchio/algorithm/check-data.hxx index 8b0e9045e6..e00266834e 100644 --- a/include/pinocchio/algorithm/check.hxx +++ b/include/pinocchio/algorithm/check-data.hxx @@ -1,78 +1,15 @@ // -// Copyright (c) 2016-2020 CNRS INRIA +// Copyright (c) 2025 INRIA // -#ifndef __pinocchio_check_hxx__ -#define __pinocchio_check_hxx__ +#ifndef __pinocchio_algorithm_check_data_hxx__ +#define __pinocchio_algorithm_check_data_hxx__ -#include -#include +#include "pinocchio/multibody/model.hpp" +#include "pinocchio/multibody/data.hpp" namespace pinocchio { - namespace internal - { - // Dedicated structure for the fusion::accumulate algorithm: validate the check-algorithm - // for all elements in a fusion list of AlgoCheckers. - template class JointCollectionTpl> - struct AlgoFusionChecker - { - typedef bool result_type; - typedef ModelTpl Model; - const Model & model; - - AlgoFusionChecker(const Model & model) - : model(model) - { - } - - inline bool operator()(const bool & accumul, const boost::fusion::void_ &) const - { - return accumul; - } - - template - inline bool operator()(const bool & accumul, const AlgorithmCheckerBase & t) const - { - return accumul && t.checkModel(model); - } - }; - } // namespace internal - - // Check the validity of the kinematic tree defined by parents. - template class JointCollectionTpl> - inline bool - ParentChecker::checkModel_impl(const ModelTpl & model) const - { - typedef ModelTpl Model; - typedef typename Model::JointIndex JointIndex; - - for (JointIndex j = 1; j < (JointIndex)model.njoints; ++j) - if (model.parents[j] >= j) - return false; - - return true; - } - -#if !defined(BOOST_FUSION_HAS_VARIADIC_LIST) - template - template class JointCollectionTpl> - bool AlgorithmCheckerList:: - checkModel_impl(const ModelTpl & model) const - { - return boost::fusion::accumulate( - checkerList, true, internal::AlgoFusionChecker(model)); - } -#else - template - template class JointCollectionTpl> - bool AlgorithmCheckerList::checkModel_impl( - const ModelTpl & model) const - { - return boost::fusion::accumulate( - checkerList, true, internal::AlgoFusionChecker(model)); - } -#endif template class JointCollectionTpl> inline bool checkData( @@ -102,7 +39,7 @@ namespace pinocchio CHECK_DATA((int)data.Ycrb.size() == model.njoints); CHECK_DATA((int)data.Yaba.size() == model.njoints); CHECK_DATA((int)data.Fcrb.size() == model.njoints); - BOOST_FOREACH (const typename Data::Matrix6x & F, data.Fcrb) + for (const typename Data::Matrix6x & F : data.Fcrb) { CHECK_DATA(F.cols() == model.nv); } @@ -124,7 +61,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.nvExtended); CHECK_DATA(data.Jcom.cols() == model.nv); CHECK_DATA(data.torque_residual.size() == model.nv); CHECK_DATA(data.dq_after.size() == model.nv); @@ -143,8 +80,14 @@ namespace pinocchio CHECK_DATA((int)data.lastChild.size() == model.njoints); CHECK_DATA((int)data.nvSubtree.size() == model.njoints); - CHECK_DATA((int)data.parents_fromRow.size() == model.nv); - CHECK_DATA((int)data.nvSubtree_fromRow.size() == model.nv); + CHECK_DATA((int)data.parents_fromRow.size() == model.nvExtended); + CHECK_DATA((int)data.mimic_parents_fromRow.size() == model.nvExtended); + CHECK_DATA((int)data.non_mimic_parents_fromRow.size() == model.nvExtended); + CHECK_DATA((int)data.idx_v_extended_fromRow.size() == model.nvExtended); + CHECK_DATA((int)data.nvSubtree_fromRow.size() == model.nvExtended); + CHECK_DATA((int)data.idx_v_extended_fromRow.size() == model.nvExtended); + CHECK_DATA((int)data.start_idx_v_fromRow.size() == model.nvExtended); + CHECK_DATA((int)data.end_idx_v_fromRow.size() == model.nvExtended); for (JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id) { @@ -154,34 +97,76 @@ 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.nvExtendeds[joint_id] == jmodel.nvExtended()); + CHECK_DATA(model.idx_vExtendeds[joint_id] == jmodel.idx_vExtended()); } - for (JointIndex j = 1; int(j) < model.njoints; ++j) { JointIndex c = (JointIndex)data.lastChild[j]; CHECK_DATA((int)c < model.njoints); - int nv = model.joints[j].nv(); - for (JointIndex d = j + 1; d <= c; ++d) // explore all descendant + int nv; + // For mimic, since in nvSubtree we're using the idx_vExtended, we need to do the same here + if (boost::get>(&model.joints[j])) + nv = 0; + else { - CHECK_DATA(model.parents[d] >= j); - nv += model.joints[d].nv(); + nv = model.joints[j].nv(); + for (JointIndex d = j + 1; d <= c; ++d) // explore all descendant + { + CHECK_DATA(model.parents[d] >= j); + + nv += model.joints[d].nv(); + } } CHECK_DATA(nv == data.nvSubtree[j]); for (JointIndex d = c + 1; (int)d < model.njoints; ++d) 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]); + CHECK_DATA( + data.nvSubtree[j] == data.nvSubtree_fromRow[(size_t)model.joints[j].idx_vExtended()]); + int row = model.joints[j].idx_vExtended(); const JointModel & jparent = model.joints[model.parents[j]]; if (row == 0) { CHECK_DATA(data.parents_fromRow[(size_t)row] == -1); + CHECK_DATA(data.mimic_parents_fromRow[(size_t)row] == -1); + CHECK_DATA(data.non_mimic_parents_fromRow[(size_t)row] == -1); } else { - CHECK_DATA(jparent.idx_v() + jparent.nv() - 1 == data.parents_fromRow[(size_t)row]); + CHECK_DATA( + jparent.idx_vExtended() + jparent.nvExtended() - 1 == data.parents_fromRow[(size_t)row]); + if (boost::get>(&jparent)) + { + CHECK_DATA(data.parents_fromRow[(size_t)row] == data.mimic_parents_fromRow[(size_t)row]); + } + else + { + CHECK_DATA( + data.parents_fromRow[(size_t)row] == data.non_mimic_parents_fromRow[(size_t)row]); + } + } + } + + if (model.mimicking_joints.size() != 0) + { + for (size_t k = 0; k < model.mimicking_joints.size(); k++) + { + // Check the mimic_subtree_joint + const auto & mimicking_sub = model.subtrees[model.mimicking_joints[k]]; + size_t j = 1; + JointIndex id_subtree = 0; + for (; j < mimicking_sub.size(); j++) + { + if (model.nvs[mimicking_sub[j]] != 0) + { + id_subtree = mimicking_sub[j]; + } + break; + } + CHECK_DATA(id_subtree == data.mimic_subtree_joint[k]); } } @@ -198,4 +183,4 @@ namespace pinocchio } // namespace pinocchio -#endif // ifndef __pinocchio_check_hxx__ +#endif // __pinocchio_algorithm_check_data_hxx__ diff --git a/include/pinocchio/algorithm/check-model.hpp b/include/pinocchio/algorithm/check-model.hpp new file mode 100644 index 0000000000..bc1dd430c1 --- /dev/null +++ b/include/pinocchio/algorithm/check-model.hpp @@ -0,0 +1,57 @@ +// +// Copyright (c) 2025 INRIA +// + +#ifndef __pinocchio_algorithm_check_model_hpp__ +#define __pinocchio_algorithm_check_model_hpp__ + +#include "pinocchio/algorithm/check-base.hpp" + +#include +#include + +namespace pinocchio +{ + +#define PINOCCHIO_DEFINE_ALGO_CHECKER(NAME) \ + struct NAME##Checker : public AlgorithmCheckerBase \ + { \ + template class JointCollectionTpl> \ + bool checkModel_impl(const ModelTpl &) const; \ + } + + /// Simple model checker, that assert that model.parents is indeed a tree. + PINOCCHIO_DEFINE_ALGO_CHECKER(Parent); + /// Simple model checker, that assert that there is a mimic joint in the tree + PINOCCHIO_DEFINE_ALGO_CHECKER(Mimic); + + template + struct AlgorithmCheckerList : AlgorithmCheckerBase> + { + typedef typename boost::fusion::list ArgType; + + AlgorithmCheckerList(const ArgType & checkerList) + : checkerList(checkerList) + { + } + + // Calls model.check for each checker in the fusion::list. + // Each list element is supposed to implement the AlgorithmCheckerBase API. + template class JointCollectionTpl> + bool checkModel_impl(const ModelTpl & model) const; + + const ArgType & checkerList; + }; + + template + AlgorithmCheckerList makeAlgoCheckerList(const T &... args) + { + return AlgorithmCheckerList(boost::fusion::make_list(args...)); + } + +} // namespace pinocchio + +/* --- Details -------------------------------------------------------------------- */ +#include "pinocchio/algorithm/check-model.hxx" + +#endif // __pinocchio_algorithm_check_model_hpp__ diff --git a/include/pinocchio/algorithm/check-model.hxx b/include/pinocchio/algorithm/check-model.hxx new file mode 100644 index 0000000000..3ce010fae7 --- /dev/null +++ b/include/pinocchio/algorithm/check-model.hxx @@ -0,0 +1,80 @@ +// +// Copyright (c) 2025 INRIA +// + +#ifndef __pinocchio_algorithm_check_model_hxx__ +#define __pinocchio_algorithm_check_model_hxx__ + +#include "pinocchio/multibody/model.hpp" + +#include + +namespace pinocchio +{ + namespace internal + { + // Dedicated structure for the fusion::accumulate algorithm: validate the check-algorithm + // for all elements in a fusion list of AlgoCheckers. + template class JointCollectionTpl> + struct AlgoFusionChecker + { + typedef bool result_type; + typedef ModelTpl Model; + const Model & model; + + AlgoFusionChecker(const Model & model) + : model(model) + { + } + + bool operator()(const bool & accumul, const boost::fusion::void_ &) const + { + return accumul; + } + + template + bool operator()(const bool & accumul, const AlgorithmCheckerBase & t) const + { + return accumul && t.checkModel(model); + } + }; + } // namespace internal + + // Check the validity of the kinematic tree defined by parents. + template class JointCollectionTpl> + inline bool + ParentChecker::checkModel_impl(const ModelTpl & model) const + { + typedef ModelTpl Model; + typedef typename Model::JointIndex JointIndex; + + for (JointIndex j = 1; j < (JointIndex)model.njoints; ++j) + if (model.parents[j] >= j) + return false; + + return true; + } + + // Check if there is a mimic joint in the kinematic tree + template class JointCollectionTpl> + inline bool + MimicChecker::checkModel_impl(const ModelTpl & model) const + { + if (!model.mimicking_joints.empty()) + return false; + + return true; + } + + template + template class JointCollectionTpl> + inline bool AlgorithmCheckerList::checkModel_impl( + const ModelTpl & model) const + { + return boost::fusion::accumulate( + checkerList, true, internal::AlgoFusionChecker(model)); + } + +} // namespace pinocchio + +#endif // __pinocchio_algorithm_check_model_hxx__ diff --git a/include/pinocchio/algorithm/check.hpp b/include/pinocchio/algorithm/check.hpp index 041fa094d9..841f91563f 100644 --- a/include/pinocchio/algorithm/check.hpp +++ b/include/pinocchio/algorithm/check.hpp @@ -2,129 +2,11 @@ // Copyright (c) 2016-2018 CNRS // -#ifndef __pinocchio_check_hpp__ -#define __pinocchio_check_hpp__ +#ifndef __pinocchio_algorithm_check_hpp__ +#define __pinocchio_algorithm_check_hpp__ -#include "pinocchio/multibody/model.hpp" -#include "pinocchio/multibody/data.hpp" -#include -#include +// This header is provided for backward compatibility +#include "pinocchio/algorithm/check-model.hpp" +#include "pinocchio/algorithm/check-data.hpp" -#ifndef PINOCCHIO_ALGO_CHECKER_LIST_MAX_LIST_SIZE - #define PINOCCHIO_ALGO_CHECKER_LIST_MAX_LIST_SIZE 5 -#endif - -namespace pinocchio -{ - - /// CRTP class describing the API of the checkers - template - struct AlgorithmCheckerBase - { - inline AlgorithmCheckerDerived & derived() - { - return *static_cast(this); - } - - inline const AlgorithmCheckerDerived & derived() const - { - return *static_cast(this); - } - - template class JointCollectionTpl> - inline bool checkModel(const ModelTpl & model) const - { - return derived().checkModel_impl(model); - } - }; - -#define PINOCCHIO_DEFINE_ALGO_CHECKER(NAME) \ - struct NAME##Checker : public AlgorithmCheckerBase \ - { \ - template class JointCollectionTpl> \ - inline bool checkModel_impl(const ModelTpl &) const; \ - } - - /// Simple model checker, that assert that model.parents is indeed a tree. - PINOCCHIO_DEFINE_ALGO_CHECKER(Parent); - -#if !defined(BOOST_FUSION_HAS_VARIADIC_LIST) - /// Checker having a list of Checker as input argument - template - struct AlgorithmCheckerList - : AlgorithmCheckerBase< - AlgorithmCheckerList> - { - typedef typename boost::fusion::list - ArgType; - - AlgorithmCheckerList(const ArgType & checkerList) - : checkerList(checkerList) - { - } - - // Calls model.check for each checker in the fusion::list. - // Each list element is supposed to implement the AlgorithmCheckerBase API. - template class JointCollectionTpl> - bool checkModel_impl(const ModelTpl & model) const; - - const ArgType & checkerList; - }; - - #define MAKE_ALGO_CHECKER_LIST(z, n, _) \ - /**/ \ - template \ - AlgorithmCheckerList makeAlgoCheckerList( \ - BOOST_PP_ENUM_BINARY_PARAMS(BOOST_PP_INC(n), D, const & arg)) \ - { \ - return AlgorithmCheckerList( \ - boost::fusion::make_list(BOOST_PP_ENUM_PARAMS(BOOST_PP_INC(n), arg))); \ - } - - BOOST_PP_REPEAT(PINOCCHIO_ALGO_CHECKER_LIST_MAX_LIST_SIZE, MAKE_ALGO_CHECKER_LIST, BOOST_PP_EMPTY) -#else - template - struct AlgorithmCheckerList : AlgorithmCheckerBase> - { - typedef typename boost::fusion::list ArgType; - - AlgorithmCheckerList(const ArgType & checkerList) - : checkerList(checkerList) - { - } - - // Calls model.check for each checker in the fusion::list. - // Each list element is supposed to implement the AlgorithmCheckerBase API. - template class JointCollectionTpl> - bool checkModel_impl(const ModelTpl & model) const; - - const ArgType & checkerList; - }; - - template - AlgorithmCheckerList makeAlgoCheckerList(const T &... args) - { - return AlgorithmCheckerList(boost::fusion::make_list(args...)); - } - -#endif - - /// Check the validity of data wrt to model, in particular if model has been modified. - /// - /// \param[in] model reference model - /// \param[in] data corresponding data - /// - /// \returns True if data is valid wrt model. - template class JointCollectionTpl> - inline bool checkData( - const ModelTpl & model, - const DataTpl & data); - -} // namespace pinocchio - -/* --- Details -------------------------------------------------------------------- */ -#include "pinocchio/algorithm/check.hxx" - -#endif // ifndef __pinocchio_check_hpp__ +#endif // __pinocchio_algorithm_check_hpp__ diff --git a/include/pinocchio/algorithm/cholesky.hxx b/include/pinocchio/algorithm/cholesky.hxx index 0edea8947c..6ab5e394cd 100644 --- a/include/pinocchio/algorithm/cholesky.hxx +++ b/include/pinocchio/algorithm/cholesky.hxx @@ -34,6 +34,7 @@ namespace pinocchio */ PINOCCHIO_UNUSED_VARIABLE(model); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef DataTpl Data; @@ -91,6 +92,8 @@ namespace pinocchio EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat) PINOCCHIO_UNUSED_VARIABLE(model); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv); Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, v); @@ -155,6 +158,8 @@ namespace pinocchio PINOCCHIO_UNUSED_VARIABLE(model); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv); Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, v); @@ -215,6 +220,8 @@ namespace pinocchio EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat) assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv); Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, v); @@ -278,6 +285,8 @@ namespace pinocchio EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat) assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv); Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, v); @@ -342,6 +351,8 @@ namespace pinocchio PINOCCHIO_UNUSED_VARIABLE(model); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + PINOCCHIO_CHECK_ARGUMENT_SIZE(vin.size(), model.nv); PINOCCHIO_CHECK_ARGUMENT_SIZE(vout.size(), model.nv); @@ -422,6 +433,8 @@ namespace pinocchio EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat) assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv); Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, v); @@ -479,6 +492,7 @@ namespace pinocchio EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat) assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, v); @@ -522,6 +536,8 @@ namespace pinocchio PINOCCHIO_UNUSED_VARIABLE(model); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + PINOCCHIO_CHECK_INPUT_ARGUMENT(col < model.nv && col >= 0); PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv); @@ -567,6 +583,7 @@ namespace pinocchio PINOCCHIO_CHECK_ARGUMENT_SIZE(Minv.cols(), model.nv); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); Mat & Minv_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, Minv); diff --git a/include/pinocchio/algorithm/compute-all-terms.hxx b/include/pinocchio/algorithm/compute-all-terms.hxx index 3ca4fa2866..68a71bf184 100644 --- a/include/pinocchio/algorithm/compute-all-terms.hxx +++ b/include/pinocchio/algorithm/compute-all-terms.hxx @@ -154,6 +154,7 @@ namespace pinocchio const Eigen::MatrixBase & v) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); PINOCCHIO_CHECK_ARGUMENT_SIZE( q.size(), model.nq, "The configuration vector is not of right size"); PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size"); diff --git a/include/pinocchio/algorithm/constrained-dynamics-derivatives.hxx b/include/pinocchio/algorithm/constrained-dynamics-derivatives.hxx index fb986b8295..1ce9fdae2e 100644 --- a/include/pinocchio/algorithm/constrained-dynamics-derivatives.hxx +++ b/include/pinocchio/algorithm/constrained-dynamics-derivatives.hxx @@ -398,6 +398,7 @@ namespace pinocchio "The gravity must be a pure force vector, no angular part"); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); // TODO: User should make sure the internal quantities are reset. data.dtau_dq.setZero(); diff --git a/include/pinocchio/algorithm/constrained-dynamics.hxx b/include/pinocchio/algorithm/constrained-dynamics.hxx index f1bc7fbb3d..d20111e163 100644 --- a/include/pinocchio/algorithm/constrained-dynamics.hxx +++ b/include/pinocchio/algorithm/constrained-dynamics.hxx @@ -209,6 +209,7 @@ namespace pinocchio typedef RigidConstraintDataTpl RigidConstraintData; assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); PINOCCHIO_CHECK_ARGUMENT_SIZE( q.size(), model.nq, "The joint configuration vector is not of right size"); PINOCCHIO_CHECK_ARGUMENT_SIZE( @@ -756,6 +757,7 @@ namespace pinocchio ProximalSettingsTpl & settings) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); PINOCCHIO_CHECK_ARGUMENT_SIZE( q.size(), model.nq, "The joint configuration vector is not of right size"); PINOCCHIO_CHECK_ARGUMENT_SIZE( @@ -988,6 +990,7 @@ namespace pinocchio using namespace Eigen; assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); PINOCCHIO_CHECK_ARGUMENT_SIZE( q.size(), model.nq, "The joint configuration vector is not of right size"); PINOCCHIO_CHECK_ARGUMENT_SIZE( diff --git a/include/pinocchio/algorithm/contact-cholesky.hxx b/include/pinocchio/algorithm/contact-cholesky.hxx index ece0b0081f..e8834ea79f 100644 --- a/include/pinocchio/algorithm/contact-cholesky.hxx +++ b/include/pinocchio/algorithm/contact-cholesky.hxx @@ -5,7 +5,8 @@ #ifndef __pinocchio_algorithm_contact_cholesky_hxx__ #define __pinocchio_algorithm_contact_cholesky_hxx__ -#include "pinocchio/algorithm/check.hpp" +#include "pinocchio/algorithm/check-model.hpp" +#include "pinocchio/multibody/data.hpp" #include @@ -27,6 +28,8 @@ namespace pinocchio typedef RigidConstraintModelTpl RigidConstraintModel; typedef std::vector RigidConstraintModelVector; + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + nv = model.nv; num_contacts = (Eigen::DenseIndex)contact_models.size(); @@ -189,6 +192,8 @@ namespace pinocchio typedef RigidConstraintDataTpl RigidConstraintData; assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + PINOCCHIO_CHECK_INPUT_ARGUMENT( (Eigen::DenseIndex)contact_models.size() == num_contacts, "The number of contacts inside contact_models and the one during allocation do not match.\n" diff --git a/include/pinocchio/algorithm/contact-dynamics.hxx b/include/pinocchio/algorithm/contact-dynamics.hxx index fb918d6e12..3f5e49c5b4 100644 --- a/include/pinocchio/algorithm/contact-dynamics.hxx +++ b/include/pinocchio/algorithm/contact-dynamics.hxx @@ -99,6 +99,7 @@ namespace pinocchio PINOCCHIO_CHECK_ARGUMENT_SIZE(J.cols(), model.nv); PINOCCHIO_CHECK_ARGUMENT_SIZE(J.rows(), gamma.size()); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef DataTpl Data; @@ -181,6 +182,8 @@ namespace pinocchio const Scalar & inv_damping) { assert(model.check(data)); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + PINOCCHIO_CHECK_INPUT_ARGUMENT( check_expression_if_real(inv_damping >= 0.), "mu must be positive."); @@ -222,6 +225,8 @@ namespace pinocchio const Eigen::MatrixBase & KKTMatrix_inv) { assert(model.check(data)); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + PINOCCHIO_CHECK_ARGUMENT_SIZE(KKTMatrix_inv.cols(), data.JMinvJt.cols() + model.nv); PINOCCHIO_CHECK_ARGUMENT_SIZE(KKTMatrix_inv.rows(), data.JMinvJt.rows() + model.nv); @@ -291,6 +296,7 @@ namespace pinocchio PINOCCHIO_CHECK_ARGUMENT_SIZE(v_before.size(), model.nv); PINOCCHIO_CHECK_ARGUMENT_SIZE(J.cols(), model.nv); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef DataTpl Data; diff --git a/include/pinocchio/algorithm/contact-jacobian.hxx b/include/pinocchio/algorithm/contact-jacobian.hxx index c2c718a448..9dba83bdbf 100644 --- a/include/pinocchio/algorithm/contact-jacobian.hxx +++ b/include/pinocchio/algorithm/contact-jacobian.hxx @@ -24,6 +24,8 @@ namespace pinocchio const Eigen::MatrixBase & J_) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + PINOCCHIO_CHECK_ARGUMENT_SIZE(J_.rows(), constraint_model.size()); PINOCCHIO_CHECK_ARGUMENT_SIZE(J_.cols(), model.nv); diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index a97fefa1bd..acf9dd3fa0 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -51,7 +51,7 @@ namespace pinocchio else data.oMi[i] = data.liMi[i]; - jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S()); + jmodel.jointExtendedModelCols(data.J) = data.oMi[i].act(jdata.S()); data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); } @@ -67,7 +67,11 @@ namespace pinocchio typedef boost::fusion::vector ArgsType; - template + template< + typename JointModel, + typename boost::disable_if< + std::is_same>, + int>::type = 0> static void algo(const JointModelBase & jmodel, const Model & model, Data & data) { typedef typename Model::JointIndex JointIndex; @@ -78,7 +82,7 @@ namespace pinocchio // Centroidal momentum map ColsBlock Ag_cols = jmodel.jointCols(data.Ag); - ColsBlock J_cols = jmodel.jointCols(data.J); + ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); // Joint Space Inertia Matrix @@ -88,6 +92,122 @@ namespace pinocchio const JointIndex & parent = model.parents[i]; data.oYcrb[parent] += data.oYcrb[i]; } + + template< + typename JointModel = JointModelMimicTpl, + typename boost::enable_if< + std::is_same>, + int>::type = 0> + static void algo(const JointModelBase & jmodel, const Model & model, Data & data) + { + typedef typename Model::JointIndex JointIndex; + const JointIndex & i = jmodel.id(); + + const JointIndex & parent = model.parents[i]; + data.oYcrb[parent] += data.oYcrb[i]; + } + }; + + // In case of a tree with a mimic joint, as seen above, the backward pass, is truncated, in + // order not to replace the mimicked joint(s) info in the matrix. This step allows for the + // mimicking joint(s) contribution to be added. It is done in 3 steps: + // - First we compute the mimicking joint(s) Force matrix + // - Then we compute the "old" row of the mimicking joint in the matrix, by getting the subtree + // of mimicking joint + // - Since here only the upper part of the matrix is computed, we need to go over the support of + // the mimicking joint, and compute how the force matrix of the mimicking joint is affecting + // each joint ATTENTION : the last loop is spilt in 2, to only fill the upper part of the matrix + template class JointCollectionTpl> + struct CrbaWorldConventionMimicStep + : public fusion::JointUnaryVisitorBase< + CrbaWorldConventionMimicStep> + { + typedef ModelTpl Model; + typedef DataTpl Data; + + typedef boost::fusion::vector ArgsType; + + template< + typename JointModel, + typename boost::disable_if< + std::is_same>, + int>::type = 0> + static void algo(const JointModelBase &, const Model &, Data &, const int &) + { + } + + template< + typename JointModel, + typename boost::enable_if< + std::is_same>, + int>::type = 0> + static void algo( + const JointModelBase & jmodel, + const Model & model, + Data & data, + const int & mims_id) + { + JointIndex secondary_id = jmodel.id(); + JointIndex primary_id = jmodel.derived().jmodel().id(); + + Eigen::Ref> + J_cols = jmodel.jointExtendedModelCols(data.J); + Eigen::Matrix Ag_sec( + 6, jmodel.nvExtended()); + + motionSet::inertiaAction(data.oYcrb[secondary_id], J_cols, Ag_sec); + + // Compute secondary terms that were previously in the diagonal + data.M.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nvExtended(), jmodel.nvExtended()) + .noalias() += J_cols.transpose() * Ag_sec; + + // Compute for the "subtree" of the mimic + JointIndex sub_mimic_id = data.mimic_subtree_joint[mims_id]; + if (sub_mimic_id != 0) + jmodel.jointRows(data.M) + .middleCols(model.idx_vs[sub_mimic_id], data.nvSubtree[sub_mimic_id]) + .noalias() += + J_cols.transpose() + * data.Ag.middleCols(model.idx_vs[sub_mimic_id], data.nvSubtree[sub_mimic_id]); + + const auto & supports = model.supports[secondary_id]; + size_t j = supports.size() - 2; + Eigen::Matrix< + Scalar, Eigen::Dynamic, Eigen::Dynamic, Options, JointModel::MaxNVMimicked, + JointModel::MaxNVMimicked> + temp_JAG; + for (; model.idx_vs[supports[j]] >= jmodel.idx_v(); j--) + { + int sup_idx_v = model.idx_vs[supports[j]]; + int sup_nv = model.nvs[supports[j]]; + int sup_idx_vExtended = model.idx_vExtendeds[supports[j]]; + int sup_nvExtended = model.nvExtendeds[supports[j]]; + + temp_JAG.noalias() = + data.J.middleCols(sup_idx_vExtended, sup_nvExtended).transpose() * Ag_sec; + data.M.block(jmodel.idx_v(), sup_idx_v, jmodel.nvExtended(), sup_nv).noalias() += + temp_JAG; + if (supports[j] == primary_id) + data.M.block(jmodel.idx_v(), sup_idx_v, jmodel.nvExtended(), sup_nv).noalias() += + temp_JAG; + } + + for (; j > 0; j--) + { + int sup_idx_v = model.idx_vs[supports[j]]; + int sup_nv = model.nvs[supports[j]]; + int sup_idx_vExtended = model.idx_vExtendeds[supports[j]]; + int sup_nvExtended = model.nvExtendeds[supports[j]]; + + data.M.block(sup_idx_v, jmodel.idx_v(), sup_nv, jmodel.nvExtended()).noalias() += + data.J.middleCols(sup_idx_vExtended, sup_nvExtended).transpose() * Ag_sec; + } + + // Mimic joint also have an effect on the centroidal map momentum, so it's important to + // compute it and add it to its mimicked. It is done here Ag[prim] += oYCRB[sec] * J[sec] + motionSet::inertiaAction( + data.oYcrb[secondary_id], J_cols, jmodel.jointCols(data.Ag)); + } }; template< @@ -132,7 +252,11 @@ namespace pinocchio typedef boost::fusion::vector ArgsType; - template + template< + typename JointModel, + typename boost::disable_if< + std::is_same>, + int>::type = 0> static void algo( const JointModelBase & jmodel, JointDataBase & jdata, @@ -150,7 +274,6 @@ namespace pinocchio typedef typename Model::JointIndex JointIndex; typedef typename Data::Matrix6x::ColsBlockXpr Block; const JointIndex & i = jmodel.id(); - /* 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(); @@ -171,6 +294,147 @@ namespace pinocchio forceSet::se3Action(data.liMi[i], iF, jF); } } + + template< + typename JointModel, + typename boost::enable_if< + std::is_same>, + int>::type = 0> + static void algo( + const JointModelBase & jmodel, + JointDataBase & jdata, + const Model & model, + Data & data) + { + typedef typename Model::JointIndex JointIndex; + typedef typename Data::Matrix6x & Matrix; + + const JointIndex & i = jmodel.id(); + const JointIndex & parent = model.parents[i]; + if (parent > 0) + { + /* Yli += liXi Yi */ + data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]); + + /* F[1:6,SUBTREE] = liXi F[1:6,SUBTREE] */ + Matrix jF = data.Fcrb[parent]; + Matrix iF = data.Fcrb[i]; + // Since we don't just copy columns, we need to use ADDTO method, to avoid ecrasing + // already filled columns, in case of parallel arms, it is important to not ecrase already + // filled data from the other part of the tree + // By using data.mimic_subtree_joint, we could do the same as for a non-mimic joint. + // But to do so, we need to change the either this visitor or data.mimic_subtree_joint + forceSet::se3Action(data.liMi[i], iF, jF); + } + } + }; + + // In case of a tree with a mimic joint, as seen above, the backward pass, is truncated, in + // order not to replace the mimicked joint(s) info in the matrix. This step allows for the + // mimicking joint(s) contribution to be added. It is done in 3 steps: + // - First we compute the mimicking joint(s) Force matrix + // - Then we compute the "old" row of the mimicking joint in the matrix, by getting the subtree + // of mimicking joint + // - Since here only the upper part of the matrix is computed, we need to go over the support of + // the mimicking joint, and compute how the force matrix of the mimicking joint is affecting + // each joint ATTENTION : the last loop is spilt in 2, to only fill the upper part of the matrix + template class JointCollectionTpl> + struct CrbaLocalConventionMimicStep + : public fusion::JointUnaryVisitorBase< + CrbaLocalConventionMimicStep> + { + typedef ModelTpl Model; + typedef DataTpl Data; + + typedef boost::fusion::vector ArgsType; + + template< + typename JointModel, + typename boost::disable_if< + std::is_same>, + int>::type = 0> + static void algo( + const JointModelBase &, + JointDataBase &, + const Model &, + Data &, + const int &) + { + } + + template< + typename JointModel, + typename boost::enable_if< + std::is_same>, + int>::type = 0> + static void algo( + const JointModelBase & jmodel, + JointDataBase & jdata, + const Model & model, + Data & data, + const int & mims_id) + { + typedef JointDataTpl JointData; + typedef + typename Eigen::Matrix + Matrix6; + + typedef typename Eigen::Matrix< + Scalar, Eigen::Dynamic, Eigen::Dynamic, Options, JointModel::MaxNVMimicked, + JointModel::MaxNVMimicked> + MatrixMass; + + typedef UseMotionSubspaceNoMalloc UseSNoMalloc; + + const JointIndex & secondary_id = jmodel.id(); + const JointIndex & primary_id = jmodel.derived().jmodel().id(); + auto & F = data.Fcrb[secondary_id]; + + /* F[1:6,i] = Y*S */ + jmodel.jointCols(F) = data.Ycrb[secondary_id] * jdata.S(); + + // Add mimicking joint contribution to the Matrix + jmodel.jointBlock(data.M).noalias() += jdata.S().transpose() * jmodel.jointCols(F); + + // Add mimicking subtree contribution to the matrix. + JointIndex sub_mimic_id = data.mimic_subtree_joint[mims_id]; + if (sub_mimic_id != 0) + { + jmodel.jointRows(data.M) + .middleCols(model.idx_vs[sub_mimic_id], data.nvSubtree[sub_mimic_id]) + .noalias() += jdata.S().transpose() + * F.middleCols(model.idx_vs[sub_mimic_id], data.nvSubtree[sub_mimic_id]); + } + + const auto & supports = model.supports[secondary_id]; + size_t j = supports.size() - 2; + for (; model.idx_vs[supports[j]] >= jmodel.idx_v(); j--) + { + const JointIndex & li = supports[j]; + const JointIndex & i = supports[j + 1]; // Child link to compute placement + + forceSet::se3Action(data.liMi[i], jmodel.jointCols(F), jmodel.jointCols(F)); + int row_idx = jmodel.idx_v(); + int col_idx = model.idx_vs[li]; + Scalar coeff(1.0); + if (li == primary_id) + coeff = 2.0; + data.M.block(row_idx, col_idx, jmodel.nvExtended(), model.nvs[li]).noalias() += + coeff * UseSNoMalloc::run(data.joints[li], jmodel.jointCols(F)); + } + for (; j > 0; j--) + { + const JointIndex & li = supports[j]; + const JointIndex & i = supports[j + 1]; // Child link to compute placement + + forceSet::se3Action(data.liMi[i], jmodel.jointCols(F), jmodel.jointCols(F)); + int col_idx = jmodel.idx_v(); + int row_idx = model.idx_vs[li]; + + data.M.block(row_idx, col_idx, model.nvs[li], jmodel.nvExtended()).noalias() += + UseSNoMalloc::run(data.joints[li], jmodel.jointCols(F)); + } + } }; template< @@ -203,6 +467,14 @@ namespace pinocchio Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data)); } + typedef CrbaLocalConventionMimicStep Pass3; + for (int i = 0; i < model.mimicking_joints.size(); i++) + { + Pass3::run( + model.joints[model.mimicking_joints[i]], data.joints[model.mimicking_joints[i]], + typename Pass3::ArgsType(model, data, i)); + } + // Add the armature contribution data.M.diagonal() += model.armature; @@ -240,6 +512,13 @@ namespace pinocchio Pass2::run(model.joints[i], typename Pass2::ArgsType(model, data)); } + typedef CrbaWorldConventionMimicStep Pass3; + for (int i = 0; i < model.mimicking_joints.size(); i++) + { + Pass3::run( + model.joints[model.mimicking_joints[i]], typename Pass3::ArgsType(model, data, i)); + } + // Add the armature contribution data.M.diagonal() += model.armature; diff --git a/include/pinocchio/algorithm/delassus.hxx b/include/pinocchio/algorithm/delassus.hxx index 61c1efb033..a42354b698 100644 --- a/include/pinocchio/algorithm/delassus.hxx +++ b/include/pinocchio/algorithm/delassus.hxx @@ -125,6 +125,8 @@ namespace pinocchio const Scalar mu) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + PINOCCHIO_CHECK_ARGUMENT_SIZE( q.size(), model.nq, "The joint configuration vector is not of right size"); PINOCCHIO_CHECK_INPUT_ARGUMENT( @@ -290,6 +292,8 @@ namespace pinocchio DataTpl & data, const std::vector, Allocator> & contact_models) { + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + std::fill(data.constraints_supported_dim.begin(), data.constraints_supported_dim.end(), 0); for (std::size_t i = 0; i < contact_models.size(); ++i) { @@ -476,6 +480,8 @@ namespace pinocchio const Scalar mu) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + PINOCCHIO_CHECK_ARGUMENT_SIZE( q.size(), model.nq, "The joint configuration vector is not of right size"); PINOCCHIO_CHECK_INPUT_ARGUMENT( @@ -802,6 +808,8 @@ namespace pinocchio const bool scaled, const bool Pv) { + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + PINOCCHIO_CHECK_INPUT_ARGUMENT( check_expression_if_real(mu >= Eigen::NumTraits::dummy_precision()), "mu is too small."); diff --git a/include/pinocchio/algorithm/energy.hxx b/include/pinocchio/algorithm/energy.hxx index a0bd50208a..209b387346 100644 --- a/include/pinocchio/algorithm/energy.hxx +++ b/include/pinocchio/algorithm/energy.hxx @@ -41,6 +41,7 @@ namespace pinocchio DataTpl & data) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef ModelTpl Model; typedef typename Model::JointIndex JointIndex; @@ -62,7 +63,7 @@ namespace pinocchio DataTpl & data) { assert(model.check(data) && "data is not consistent with model."); - ; + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef ModelTpl Model; typedef DataTpl Data; @@ -89,6 +90,7 @@ namespace pinocchio DataTpl & data) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef ModelTpl Model; typedef DataTpl Data; diff --git a/include/pinocchio/algorithm/geometry.txx b/include/pinocchio/algorithm/geometry.txx index becf526b4b..99d14d0008 100644 --- a/include/pinocchio/algorithm/geometry.txx +++ b/include/pinocchio/algorithm/geometry.txx @@ -5,7 +5,7 @@ #ifndef __pinocchio_algorithm_geometry_txx__ #define __pinocchio_algorithm_geometry_txx__ -#ifndef PINOCCHIO_SKIP_CASADI_UNSUPPORTED +#ifndef PINOCCHIO_SKIP_ALGORTIHM_GEOMETRY namespace pinocchio { @@ -27,6 +27,6 @@ namespace pinocchio } // namespace pinocchio -#endif // PINOCCHIO_SKIP_CASADI_UNSUPPORTED +#endif // PINOCCHIO_SKIP_ALGORITHM_GEOMETRY #endif // ifndef __pinocchio_algorithm_geometry_txx__ diff --git a/include/pinocchio/algorithm/impulse-dynamics-derivatives.hxx b/include/pinocchio/algorithm/impulse-dynamics-derivatives.hxx index e94b59830b..0968ddd9db 100644 --- a/include/pinocchio/algorithm/impulse-dynamics-derivatives.hxx +++ b/include/pinocchio/algorithm/impulse-dynamics-derivatives.hxx @@ -254,6 +254,7 @@ namespace pinocchio const Eigen::MatrixBase & impulse_partial_dv) { const Eigen::DenseIndex nc = data.contact_chol.constraintDim(); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); PINOCCHIO_CHECK_INPUT_ARGUMENT( contact_data.size() == contact_models.size(), diff --git a/include/pinocchio/algorithm/impulse-dynamics.hxx b/include/pinocchio/algorithm/impulse-dynamics.hxx index 6a32329de2..950d33adf7 100644 --- a/include/pinocchio/algorithm/impulse-dynamics.hxx +++ b/include/pinocchio/algorithm/impulse-dynamics.hxx @@ -32,6 +32,7 @@ namespace pinocchio const ProximalSettingsTpl & settings) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); PINOCCHIO_CHECK_INPUT_ARGUMENT( q.size() == model.nq, "The joint configuration vector is not of right size"); PINOCCHIO_CHECK_INPUT_ARGUMENT( diff --git a/include/pinocchio/algorithm/jacobian.hxx b/include/pinocchio/algorithm/jacobian.hxx index 8f91d2a57e..0c89708162 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.jointExtendedModelCols(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.jointExtendedModelCols(data.J) = data.oMi[i].act(jdata.S()); } }; } // namespace impl @@ -184,7 +184,7 @@ 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.nvExtended); PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.rows(), 6); PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.cols(), model.nv); @@ -197,38 +197,74 @@ 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 colRef = + nvExtended(model.joints[joint_id]) + idx_vExtended(model.joints[joint_id]) - 1; + const int colRefMimicPass = + boost::get>(&model.joints[joint_id]) + ? colRef + : data.mimic_parents_fromRow[(size_t)idx_vExtended(model.joints[joint_id])]; switch (rf) { case WORLD: { - for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j]) + for (Eigen::DenseIndex jExtended = colRef; jExtended >= 0; + jExtended = data.non_mimic_parents_fromRow[(size_t)jExtended]) { - MotionIn v_in(Jin.col(j)); - MotionOut v_out(Jout_.col(j)); + MotionIn v_in(Jin.col(jExtended)); + MotionOut v_out(Jout_.col(data.idx_v_extended_fromRow[jExtended])); v_out = v_in; } + // Add mimicking joint effect into mimicked column + for (Eigen::DenseIndex jExtended = colRefMimicPass; jExtended >= 0; + jExtended = data.mimic_parents_fromRow[(size_t)jExtended]) + { + MotionIn v_in(Jin.col(jExtended)); + MotionOut v_out(Jout_.col(data.idx_v_extended_fromRow[jExtended])); + + v_out += v_in; + } break; } case LOCAL_WORLD_ALIGNED: { - for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j]) + for (Eigen::DenseIndex jExtended = colRef; jExtended >= 0; + jExtended = data.non_mimic_parents_fromRow[(size_t)jExtended]) { - MotionIn v_in(Jin.col(j)); - MotionOut v_out(Jout_.col(j)); + MotionIn v_in(Jin.col(jExtended)); + MotionOut v_out(Jout_.col(data.idx_v_extended_fromRow[jExtended])); v_out = v_in; v_out.linear() -= placement.translation().cross(v_in.angular()); } + // Add mimicking joint effect into mimicked column + for (Eigen::DenseIndex jExtended = colRefMimicPass; jExtended >= 0; + jExtended = data.mimic_parents_fromRow[(size_t)jExtended]) + { + MotionIn v_in(Jin.col(jExtended)); + MotionOut v_out(Jout_.col(data.idx_v_extended_fromRow[jExtended])); + + 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 (Eigen::DenseIndex jExtended = colRef; jExtended >= 0; + jExtended = data.non_mimic_parents_fromRow[(size_t)jExtended]) { - MotionIn v_in(Jin.col(j)); - MotionOut v_out(Jout_.col(j)); + MotionIn v_in(Jin.col(jExtended)); + MotionOut v_out(Jout_.col(data.idx_v_extended_fromRow[jExtended])); v_out = placement.actInv(v_in); } + // Add mimicking joint effect into mimicked column + for (Eigen::DenseIndex jExtended = colRefMimicPass; jExtended >= 0; + jExtended = data.mimic_parents_fromRow[(size_t)jExtended]) + { + MotionIn v_in(Jin.col(jExtended)); + MotionOut v_out(Jout_.col(data.idx_v_extended_fromRow[jExtended])); + + v_out += placement.actInv(v_in); + } break; } default: @@ -285,14 +321,16 @@ namespace pinocchio int Options, template class JointCollectionTpl, typename ConfigVectorType, - typename Matrix6xLike> + typename Matrix6xLike, + AssignmentOperatorType op = SETTO> struct JointJacobianForwardStep : public fusion::JointUnaryVisitorBase> + Matrix6xLike, + op>> { typedef ModelTpl Model; typedef DataTpl Data; @@ -319,7 +357,21 @@ 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()); + switch (op) + { + case SETTO: + jmodel.jointCols(J_) = data.iMf[i].actInv(jdata.S()); + break; + case ADDTO: + jmodel.jointCols(J_) += data.iMf[i].actInv(jdata.S()); + break; + case RMTO: + jmodel.jointCols(J_) -= data.iMf[i].actInv(jdata.S()); + break; + default: + assert(false && "Wrong Op requesed value"); + break; + } } }; @@ -344,14 +396,36 @@ namespace pinocchio typedef typename Model::JointIndex JointIndex; data.iMf[jointId].setIdentity(); + Matrix6xLike & J_ = J.const_cast_derived(); typedef JointJacobianForwardStep< Scalar, Options, JointCollectionTpl, ConfigVectorType, Matrix6xLike> Pass; for (JointIndex i = jointId; i > 0; i = model.parents[i]) { Pass::run( - model.joints[i], data.joints[i], - typename Pass::ArgsType(model, data, q.derived(), J.const_cast_derived())); + model.joints[i], data.joints[i], typename Pass::ArgsType(model, data, q.derived(), J_)); + } + + typedef JointJacobianForwardStep< + Scalar, Options, JointCollectionTpl, ConfigVectorType, Matrix6xLike, ADDTO> + MimicPass; + // During the backward pass, mimicking joint jacobian might have been overwritten by the + // mimicked joint (since mimicked_id < mimicking_id) Do another pass to accumulate it back + for (size_t i = 0; i < model.mimicking_joints.size(); i++) + { + const JointIndex mimicking_id = model.mimicking_joints[i]; + const JointIndex mimicked_id = model.mimicked_joints[i]; + const typename Model::IndexVector & joint_support = model.supports[jointId]; + if ( + std::find(joint_support.begin(), joint_support.end(), mimicking_id) + == joint_support.end()) + continue; // This mimicking joint does not support the selected joint, skip + if ( + std::find(joint_support.begin(), joint_support.end(), mimicked_id) == joint_support.end()) + continue; // This mimicked joint does not support the selected joint, skip + MimicPass::run( + model.joints[mimicking_id], data.joints[mimicking_id], + typename Pass::ArgsType(model, data, q.derived(), J_)); } } @@ -410,7 +484,7 @@ namespace pinocchio oMi = data.liMi[i]; } - jmodel.jointCols(data.J) = oMi.act(jdata.S()); + jmodel.jointExtendedModelCols(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 +492,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.jointExtendedModelCols(data.dJ); + ColsBlock Jcols = jmodel.jointExtendedModelCols(data.J); motionSet::motionAction(data.ov[i], Jcols, dJcols); } @@ -489,16 +563,18 @@ 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]) + const int colRef = + nvExtended(model.joints[jointId]) + idx_vExtended(model.joints[jointId]) - 1; + for (Eigen::DenseIndex jExtended = colRef; jExtended >= 0; + jExtended = data.parents_fromRow[(size_t)jExtended]) { 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)); + MotionIn v_in(data.J.col(jExtended)); + MotionOut v_out(dJ.col(data.idx_v_extended_fromRow[jExtended])); v_out -= v_joint.cross(oMjoint.actInv(v_in)); } @@ -507,16 +583,18 @@ namespace pinocchio 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]) + const int colRef = + nvExtended(model.joints[jointId]) + idx_vExtended(model.joints[jointId]) - 1; + for (Eigen::DenseIndex jExtended = colRef; jExtended >= 0; + jExtended = data.parents_fromRow[(size_t)jExtended]) { 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)); + MotionIn v_in(data.J.col(jExtended)); + MotionOut v_out(dJ.col(data.idx_v_extended_fromRow[jExtended])); v_out.linear() -= Vector3(ov_joint.linear() + ov_joint.angular().cross(oMjoint.translation())) diff --git a/include/pinocchio/algorithm/kinematics-derivatives.hxx b/include/pinocchio/algorithm/kinematics-derivatives.hxx index 0e0e171bbe..98d7604ab4 100644 --- a/include/pinocchio/algorithm/kinematics-derivatives.hxx +++ b/include/pinocchio/algorithm/kinematics-derivatives.hxx @@ -113,6 +113,7 @@ namespace pinocchio PINOCCHIO_CHECK_ARGUMENT_SIZE( a.size(), model.nv, "The acceleration vector is not of right size"); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef ModelTpl Model; typedef typename Model::JointIndex JointIndex; @@ -262,6 +263,7 @@ namespace pinocchio PINOCCHIO_CHECK_ARGUMENT_SIZE(v_partial_dv.cols(), model.nv); PINOCCHIO_CHECK_INPUT_ARGUMENT((int)jointId < model.njoints, "The joint id is invalid."); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef typename Model::JointIndex JointIndex; @@ -490,6 +492,7 @@ namespace pinocchio PINOCCHIO_CHECK_ARGUMENT_SIZE(a_partial_da.cols(), model.nv); PINOCCHIO_CHECK_INPUT_ARGUMENT((int)jointId < model.njoints, "The joint id is invalid."); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef ModelTpl Model; typedef typename Model::JointIndex JointIndex; @@ -669,6 +672,7 @@ namespace pinocchio rf == LOCAL || rf == LOCAL_WORLD_ALIGNED, "The reference frame is not valid, expected LOCAL or LOCAL_WORLD_ALIGNED"); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef typename Data::SE3 SE3; typedef typename Data::Motion Motion; @@ -905,6 +909,7 @@ namespace pinocchio rf == LOCAL || rf == LOCAL_WORLD_ALIGNED, "The reference frame is not valid, expected LOCAL or LOCAL_WORLD_ALIGNED"); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); const SE3 oMpoint = data.oMi[joint_id] * placement; const Motion spatial_velocity = oMpoint.actInv(data.ov[joint_id]); @@ -966,6 +971,7 @@ namespace pinocchio DataTpl & data) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef ModelTpl Model; typedef DataTpl Data; @@ -1070,6 +1076,7 @@ namespace pinocchio Tensor & kinematic_hessian) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); assert( joint_id < model.joints.size() && joint_id > 0 && "joint_id is outside the valid index for a joint in model.joints"); diff --git a/include/pinocchio/algorithm/kinematics.hpp b/include/pinocchio/algorithm/kinematics.hpp index cc0bfe8ebe..e92086927c 100644 --- a/include/pinocchio/algorithm/kinematics.hpp +++ b/include/pinocchio/algorithm/kinematics.hpp @@ -101,6 +101,32 @@ 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. LOCAL convention should only be used when aba and crba algorithms are called in + * LOCAL convention as well. + * + * @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 (computation 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. + * + * \note WORLD convention complexity is in O(1) and LOCAL is in O(n). + */ + 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..6316acd36b 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.JointMappedVelocitySelector(a) + jdata.c() + (data.v[i] ^ jdata.v()); data.a[i] += data.liMi[i].actInv(data.a[parent]); } }; @@ -276,6 +277,50 @@ 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 ancestorMref(1); // Initialize to Identity + SE3Tpl ancestorMtarget(1); // Initialize to Identity + + size_t ancestor_ref, ancestor_target; + findCommonAncestor(model, jointIdRef, jointIdTarget, ancestor_ref, ancestor_target); + + // Traverse the kinematic chain backward from Ref to the common ancestor + for (size_t i = model.supports[jointIdRef].size() - 1; i > ancestor_ref; i--) + { + const JointIndex j = model.supports[jointIdRef][(size_t)i]; + ancestorMref = data.liMi[j].act(ancestorMref); + } + + // Traverse the kinematic chain backward from Target to the common ancestor + for (size_t i = model.supports[jointIdTarget].size() - 1; i > ancestor_target; i--) + { + const JointIndex j = model.supports[jointIdTarget][(size_t)i]; + ancestorMtarget = data.liMi[j].act(ancestorMtarget); + } + + return ancestorMref.actInv(ancestorMtarget); + } + 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..b802b5b9c5 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 = (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*/) + { + return jmodel; + } + + 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_vExtended()); + return res; + } + template static void algo( const JointModelBase & jmodel_in, @@ -173,9 +203,16 @@ 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), + 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.jointVelocitySelector( + modelAB + .effortLimit), // Need to select the vector base on the origin idxs, so use jmodel_in. jmodel_in.jointVelocitySelector(modelAB.velocityLimit), jmodel_in.jointConfigSelector(modelAB.lowerPositionLimit), jmodel_in.jointConfigSelector(modelAB.upperPositionLimit), @@ -185,7 +222,8 @@ namespace pinocchio model.appendBodyToJoint(joint_id_out, modelAB.inertias[joint_id_in]); - const typename Model::JointModel & jmodel_out = model.joints[joint_id_out]; + 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) = @@ -601,16 +639,41 @@ namespace pinocchio } else { - // the joint should be added to the Model as it is - 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)); + JointIndex reduced_joint_id; + if (boost::get>(&joint_input_model)) + { + auto mimic_joint = + boost::get>(joint_input_model); + + JointIndex mimicked_id = + reduced_model.getJointId(input_model.names[mimic_joint.jmodel().id()]); + mimic_joint.setMimicIndexes( + mimicked_id, reduced_model.idx_qs[mimicked_id], reduced_model.idx_vs[mimicked_id], + reduced_model.idx_vExtendeds[mimicked_id]); + + reduced_joint_id = reduced_model.addJoint( + reduced_parent_joint_index, mimic_joint, + parent_frame_placement * input_model.jointPlacements[joint_id], joint_name, + mimic_joint.jointVelocitySelector(input_model.effortLimit), + mimic_joint.jointVelocitySelector(input_model.velocityLimit), + mimic_joint.jointConfigSelector(input_model.lowerPositionLimit), + mimic_joint.jointConfigSelector(input_model.upperPositionLimit), + mimic_joint.jointVelocitySelector(input_model.friction), + mimic_joint.jointVelocitySelector(input_model.damping)); + } + else + { + // the joint should be added to the Model as it is + 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)); + } // Append inertia reduced_model.appendBodyToJoint( reduced_joint_id, input_model.inertias[joint_id], SE3::Identity()); @@ -726,8 +789,6 @@ namespace pinocchio ModelTpl & reduced_model, std::vector & list_of_reduced_geom_models) { - - typedef ModelTpl Model; buildReducedModel(input_model, list_of_joints_to_lock, reference_configuration, reduced_model); // for all GeometryModels @@ -751,7 +812,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)) { @@ -788,6 +849,133 @@ 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_primary is greater than the total of joints"); + PINOCCHIO_CHECK_INPUT_ARGUMENT( + index_secondary <= (size_t)input_model.njoints, + "index_primary is greater than the total of joints"); + PINOCCHIO_CHECK_INPUT_ARGUMENT( + index_primary < index_secondary, "index_primary is greater than sindex_secondary"); + + typedef ModelTpl Model; + typedef typename Model::JointModel JointModel; + + output_model = input_model; + + output_model.joints[index_secondary] = JointModelMimic( + input_model.joints[index_secondary], output_model.joints[index_primary], scaling, offset); + + int old_nq = input_model.joints[index_secondary].nq(); + int old_nv = input_model.joints[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.jointVelocitySelector(output_model.effortLimit) = + jmodel_input.jointVelocitySelector(input_model.effortLimit); + jmodel_output.jointVelocitySelector(output_model.velocityLimit) = + jmodel_input.jointVelocitySelector(input_model.velocityLimit); + + jmodel_output.jointConfigSelector(output_model.lowerPositionLimit) = + jmodel_input.jointConfigSelector(input_model.lowerPositionLimit); + jmodel_output.jointConfigSelector(output_model.upperPositionLimit) = + jmodel_input.jointConfigSelector(input_model.upperPositionLimit); + + jmodel_output.jointVelocitySelector(output_model.armature) = + jmodel_input.jointVelocitySelector(input_model.armature); + jmodel_output.jointVelocitySelector(output_model.rotorInertia) = + jmodel_input.jointVelocitySelector(input_model.rotorInertia); + jmodel_output.jointVelocitySelector(output_model.rotorGearRatio) = + jmodel_input.jointVelocitySelector(input_model.rotorGearRatio); + jmodel_output.jointVelocitySelector(output_model.friction) = + jmodel_input.jointVelocitySelector(input_model.friction); + jmodel_output.jointVelocitySelector(output_model.damping) = + jmodel_input.jointVelocitySelector(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_vExtended()); + if ( + boost::get>(&jmodel_output) + && joint_id != index_secondary) + { + auto & jmimic = + boost::get>(jmodel_output); + const JointIndex prim_id = jmimic.jmodel().id(); + jmimic.setMimicIndexes( + prim_id, output_model.idx_qs[prim_id], output_model.idx_vs[prim_id], + jmodel_input.idx_vExtended()); + } + 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.jointVelocitySelector(output_model.effortLimit) = + jmodel_input.jointVelocitySelector(input_model.effortLimit); + jmodel_output.jointVelocitySelector(output_model.velocityLimit) = + jmodel_input.jointVelocitySelector(input_model.velocityLimit); + + jmodel_output.jointConfigSelector(output_model.lowerPositionLimit) = + jmodel_input.jointConfigSelector(input_model.lowerPositionLimit); + jmodel_output.jointConfigSelector(output_model.upperPositionLimit) = + jmodel_input.jointConfigSelector(input_model.upperPositionLimit); + + jmodel_output.jointVelocitySelector(output_model.armature) = + jmodel_input.jointVelocitySelector(input_model.armature); + jmodel_output.jointVelocitySelector(output_model.rotorInertia) = + jmodel_input.jointVelocitySelector(input_model.rotorInertia); + jmodel_output.jointVelocitySelector(output_model.rotorGearRatio) = + jmodel_input.jointVelocitySelector(input_model.rotorGearRatio); + jmodel_output.jointVelocitySelector(output_model.friction) = + jmodel_input.jointVelocitySelector(input_model.friction); + jmodel_output.jointVelocitySelector(output_model.damping) = + jmodel_input.jointVelocitySelector(input_model.damping); + } + } + + output_model.mimicking_joints.push_back(index_secondary); + output_model.mimicked_joints.push_back(index_primary); + } + 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..00a492e991 100644 --- a/include/pinocchio/algorithm/pv.hxx +++ b/include/pinocchio/algorithm/pv.hxx @@ -28,6 +28,8 @@ namespace pinocchio const std::vector, Allocator> & contact_models) { + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + // Allocate memory for the backward propagation of LA, KA and lA typedef typename Model::JointIndex JointIndex; typedef RigidConstraintDataTpl RigidConstraintData; @@ -410,6 +412,7 @@ namespace pinocchio { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); PINOCCHIO_CHECK_ARGUMENT_SIZE( q.size(), model.nq, "The joint configuration vector is not of right size"); PINOCCHIO_CHECK_ARGUMENT_SIZE( @@ -607,6 +610,7 @@ namespace pinocchio { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); PINOCCHIO_CHECK_ARGUMENT_SIZE( q.size(), model.nq, "The joint configuration vector is not of right size"); PINOCCHIO_CHECK_ARGUMENT_SIZE( diff --git a/include/pinocchio/algorithm/regressor.hxx b/include/pinocchio/algorithm/regressor.hxx index 88f7cffa51..7a2c6298ce 100644 --- a/include/pinocchio/algorithm/regressor.hxx +++ b/include/pinocchio/algorithm/regressor.hxx @@ -29,6 +29,7 @@ namespace pinocchio const Eigen::MatrixBase & kinematic_regressor) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); PINOCCHIO_CHECK_ARGUMENT_SIZE(kinematic_regressor.rows(), 6); PINOCCHIO_CHECK_ARGUMENT_SIZE(kinematic_regressor.cols(), 6 * (model.njoints - 1)); @@ -144,6 +145,7 @@ namespace pinocchio const Eigen::MatrixBase & q) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq); typedef ModelTpl Model; @@ -296,6 +298,7 @@ namespace pinocchio JointIndex joint_id) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); PINOCCHIO_UNUSED_VARIABLE(model); @@ -311,6 +314,7 @@ namespace pinocchio FrameIndex frame_id) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef ModelTpl Model; typedef typename Model::Frame Frame; @@ -432,6 +436,7 @@ namespace pinocchio const Eigen::MatrixBase & a) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq); PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv); PINOCCHIO_CHECK_ARGUMENT_SIZE(a.size(), model.nv); @@ -478,6 +483,7 @@ namespace pinocchio const Eigen::MatrixBase & v) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq); PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv); @@ -524,6 +530,7 @@ namespace pinocchio const Eigen::MatrixBase & q) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq); typedef DataTpl Data; diff --git a/include/pinocchio/algorithm/rnea-derivatives.hxx b/include/pinocchio/algorithm/rnea-derivatives.hxx index c9f4812bc5..494382a92a 100644 --- a/include/pinocchio/algorithm/rnea-derivatives.hxx +++ b/include/pinocchio/algorithm/rnea-derivatives.hxx @@ -154,6 +154,7 @@ namespace pinocchio PINOCCHIO_CHECK_ARGUMENT_SIZE(gravity_partial_dq.cols(), model.nv); PINOCCHIO_CHECK_ARGUMENT_SIZE(gravity_partial_dq.rows(), model.nv); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef ModelTpl Model; typedef typename Model::JointIndex JointIndex; @@ -201,6 +202,7 @@ namespace pinocchio PINOCCHIO_CHECK_ARGUMENT_SIZE( fext.size(), (size_t)model.njoints, "The size of the external forces is not of right size"); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef ModelTpl Model; typedef typename Model::JointIndex JointIndex; @@ -493,6 +495,7 @@ namespace pinocchio isZero(model.gravity.angular()), "The gravity must be a pure force vector, no angular part"); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef ModelTpl Model; typedef DataTpl Data; @@ -573,6 +576,7 @@ namespace pinocchio PINOCCHIO_CHECK_ARGUMENT_SIZE(rnea_partial_da.cols(), model.nv); PINOCCHIO_CHECK_ARGUMENT_SIZE(rnea_partial_da.rows(), model.nv); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef ModelTpl Model; typedef DataTpl Data; diff --git a/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx b/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx index b5c1fe412b..9bfdb33a09 100644 --- a/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx +++ b/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx @@ -431,6 +431,7 @@ namespace pinocchio PINOCCHIO_CHECK_ARGUMENT_SIZE(dtau_dadq.dimension(1), model.nv); PINOCCHIO_CHECK_ARGUMENT_SIZE(dtau_dadq.dimension(2), model.nv); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef ModelTpl Model; typedef typename Model::JointIndex JointIndex; diff --git a/include/pinocchio/algorithm/rnea.hxx b/include/pinocchio/algorithm/rnea.hxx index f9ba3c7266..9eca6e7369 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.JointMappedVelocitySelector(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,8 @@ namespace pinocchio const JointIndex i = jmodel.id(); const JointIndex parent = model.parents[i]; - - jmodel.jointVelocitySelector(data.tau) = jdata.S().transpose() * data.f[i]; + // Mimic joint contributes to the torque of their primary and don't have their own + jmodel.JointMappedVelocitySelector(data.tau) += jdata.S().transpose() * data.f[i]; if (parent > 0) data.f[parent] += data.liMi[i].act(data.f[i]); @@ -131,6 +131,9 @@ namespace pinocchio typedef ModelTpl Model; typedef typename Model::JointIndex JointIndex; + // Set to zero, because it's not an assignation, that is done in the algorithm but a sum to + // take mimic joint into account + data.tau.setZero(); data.v[0].setZero(); data.a_gf[0] = -model.gravity; @@ -184,6 +187,9 @@ namespace pinocchio typedef ModelTpl Model; typedef typename Model::JointIndex JointIndex; + // Set to zero, because it's not an assignation, that is done in the algorithm but a sum to + // take mimic joint into account + data.tau.setZero(); data.v[0].setZero(); data.a_gf[0] = -model.gravity; @@ -279,7 +285,8 @@ namespace pinocchio const JointIndex & i = jmodel.id(); const JointIndex & parent = model.parents[i]; - jmodel.jointVelocitySelector(data.nle) = jdata.S().transpose() * data.f[i]; + // Mimic joint contributes to the nle of their primary and don't have their own + jmodel.JointMappedVelocitySelector(data.nle) += jdata.S().transpose() * data.f[i]; if (parent > 0) data.f[parent] += data.liMi[i].act(data.f[i]); } @@ -306,6 +313,9 @@ namespace pinocchio typedef ModelTpl Model; typedef typename Model::JointIndex JointIndex; + // Set to zero, because it's not an assignation, that is done in the algorithm but a sum to + // take mimic joint into account + data.nle.setZero(); data.v[0].setZero(); data.a_gf[0] = -model.gravity; @@ -386,8 +396,8 @@ namespace pinocchio const JointIndex & i = jmodel.id(); const JointIndex & parent = model.parents[i]; - - jmodel.jointVelocitySelector(g) = jdata.S().transpose() * data.f[i]; + // Mimic joint contributes to the gravity of their primary and don't have their own + jmodel.JointMappedVelocitySelector(g) += jdata.S().transpose() * data.f[i]; if (parent > 0) data.f[(size_t)parent] += data.liMi[i].act(data.f[i]); } @@ -411,6 +421,9 @@ namespace pinocchio typedef ModelTpl Model; typedef typename Model::JointIndex JointIndex; + // Set to zero, because it's not an assignation, that is done in the algorithm but a sum to + // take mimic joint into account + data.g.setZero(); data.a_gf[0] = -model.gravity; typedef ComputeGeneralizedGravityForwardStep< @@ -422,6 +435,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) { @@ -463,7 +477,9 @@ namespace pinocchio model.joints[i], data.joints[i], typename Pass1::ArgsType(model, data, q.derived())); data.f[i] -= fext[i]; } - + // Set to zero, because it's not an assignation, that is done in the algorithm but a sum to + // take mimic joint into account + data.tau.setZero(); typedef ComputeGeneralizedGravityBackwardStep Pass2; for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) { @@ -530,11 +546,11 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointCols(data.J); + ColsBlock J_cols = jmodel.jointExtendedModelCols(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.jointExtendedModelCols(data.dJ); motionSet::motionAction(data.ov[i], J_cols, dJ_cols); data.B[i] = data.oYcrb[i].variation(Scalar(0.5) * data.ov[i]); @@ -582,8 +598,8 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock dJ_cols = jmodel.jointCols(data.dJ); - ColsBlock J_cols = jmodel.jointCols(data.J); + ColsBlock dJ_cols = jmodel.jointExtendedModelCols(data.dJ); + ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); ColsBlock Ag_cols = jmodel.jointCols(data.Ag); motionSet::inertiaAction(data.oYcrb[i], dJ_cols, jmodel.jointCols(data.dFdv)); @@ -625,6 +641,7 @@ namespace pinocchio const Eigen::MatrixBase & v) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq); PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv); @@ -711,6 +728,7 @@ namespace pinocchio DataTpl & data) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef DataTpl Data; typedef ModelTpl Model; diff --git a/include/pinocchio/bindings/python/context/generic.hpp b/include/pinocchio/bindings/python/context/generic.hpp index 9f3eb37616..cf2020aa8e 100644 --- a/include/pinocchio/bindings/python/context/generic.hpp +++ b/include/pinocchio/bindings/python/context/generic.hpp @@ -138,6 +138,9 @@ namespace pinocchio typedef JointModelCompositeTpl JointModelComposite; typedef JointDataCompositeTpl JointDataComposite; + typedef JointModelMimicTpl JointModelMimic; + typedef JointDataMimicTpl JointDataMimic; + // Algorithm typedef ProximalSettingsTpl ProximalSettings; typedef ContactCholeskyDecompositionTpl ContactCholeskyDecomposition; diff --git a/include/pinocchio/bindings/python/multibody/data.hpp b/include/pinocchio/bindings/python/multibody/data.hpp index 9548a90a85..1af9e5d837 100644 --- a/include/pinocchio/bindings/python/multibody/data.hpp +++ b/include/pinocchio/bindings/python/multibody/data.hpp @@ -151,8 +151,20 @@ namespace pinocchio .ADD_DATA_PROPERTY(U, "Joint Inertia square root (upper triangle)") .ADD_DATA_PROPERTY(D, "Diagonal of UDUT inertia decomposition") .ADD_DATA_PROPERTY(parents_fromRow, "First previous non-zero row in M (used in Cholesky)") + .ADD_DATA_PROPERTY( + mimic_parents_fromRow, + "First previous non-zero row belonging to a mimic joint in M (used in Jacobian).") + .ADD_DATA_PROPERTY( + non_mimic_parents_fromRow, + "First previous non-zero row belonging to a non mimic joint in M (used in Jacobian).") + .ADD_DATA_PROPERTY( + idx_v_extended_fromRow, "Extended model mapping of the joint rows " + "(idx_v_extended_fromRow[idx_v_extended] = idx_v)") .ADD_DATA_PROPERTY( nvSubtree_fromRow, "Subtree of the current row index (used in Cholesky)") + .ADD_DATA_PROPERTY( + mimic_subtree_joint, "When mimic joints are in the tree, this will store the index of " + "the first non mimic child of each mimic joint. (useful in crba)") .ADD_DATA_PROPERTY(J, "Jacobian of joint placement") .ADD_DATA_PROPERTY(dJ, "Time variation of the Jacobian of joint placement (data.J).") .ADD_DATA_PROPERTY(iMf, "Body placement wrt to algorithm end effector.") diff --git a/include/pinocchio/bindings/python/multibody/joint/joint-derived.hpp b/include/pinocchio/bindings/python/multibody/joint/joint-derived.hpp index 3a51ca2c29..2d254c77bc 100644 --- a/include/pinocchio/bindings/python/multibody/joint/joint-derived.hpp +++ b/include/pinocchio/bindings/python/multibody/joint/joint-derived.hpp @@ -32,17 +32,20 @@ namespace pinocchio .add_property("id", &get_id) .add_property("idx_q", &get_idx_q) .add_property("idx_v", &get_idx_v) + .add_property("idx_vExtended", &get_idx_vExtended) .add_property("nq", &get_nq) .add_property("nv", &get_nv) + .add_property("nvExtended", &get_nvExtended) .add_property( "hasConfigurationLimit", &JointModelDerived::hasConfigurationLimit, "Return vector of boolean if joint has configuration limits.") .add_property( "hasConfigurationLimitInTangent", &JointModelDerived::hasConfigurationLimitInTangent, "Return vector of boolean if joint has configuration limits in tangent space.") + .def("setIndexes", &setIndexes0, bp::args("self", "joint_id", "idx_q", "idx_v")) .def( - "setIndexes", &JointModelDerived::setIndexes, - bp::args("self", "joint_id", "idx_q", "idx_v")) + "setIndexes", &setIndexes1, + bp::args("self", "joint_id", "idx_q", "idx_v", "idx_vExtended")) .def("classname", &JointModelDerived::classname) .staticmethod("classname") .def("calc", &calc0, bp::args("self", "jdata", "q")) @@ -90,6 +93,10 @@ namespace pinocchio { return self.idx_v(); } + static int get_idx_vExtended(const JointModelDerived & self) + { + return self.idx_vExtended(); + } static int get_nq(const JointModelDerived & self) { return self.nq(); @@ -98,6 +105,11 @@ namespace pinocchio { return self.nv(); } + static int get_nvExtended(const JointModelDerived & self) + { + return self.nvExtended(); + } + static void calc0(const JointModelDerived & self, JointDataDerived & jdata, const context::VectorXs & q) { @@ -111,6 +123,22 @@ namespace pinocchio { self.calc(jdata, q, v); } + + static void + setIndexes0(JointModelDerived & self, const int & id, const int & idx_q, const int & idx_v) + { + self.setIndexes(id, idx_q, idx_v); + } + + static void setIndexes1( + JointModelDerived & self, + const int & id, + const int & idx_q, + const int & idx_v, + const int & idx_vExtended) + { + self.setIndexes(id, idx_q, idx_v, idx_vExtended); + } }; template diff --git a/include/pinocchio/bindings/python/multibody/joint/joint.hpp b/include/pinocchio/bindings/python/multibody/joint/joint.hpp index cf589de467..e0dd2af48c 100644 --- a/include/pinocchio/bindings/python/multibody/joint/joint.hpp +++ b/include/pinocchio/bindings/python/multibody/joint/joint.hpp @@ -27,15 +27,18 @@ namespace pinocchio .add_property("id", &getId) .add_property("idx_q", &getIdx_q) .add_property("idx_v", &getIdx_v) + .add_property("idx_vExtended", &getIdx_vExtended) .add_property("nq", &getNq) .add_property("nv", &getNv) + .add_property("nvExtended", &getNvExtended) .def( "hasConfigurationLimit", &JointModel::hasConfigurationLimit, "Return vector of boolean if joint has configuration limits.") .def( "hasConfigurationLimitInTangent", &JointModel::hasConfigurationLimitInTangent, "Return vector of boolean if joint has configuration limits in tangent space.") - .def("setIndexes", &JointModel::setIndexes, bp::args("self", "id", "idx_q", "idx_v")) + .def("setIndexes", setIndexes0, bp::args("self", "id", "idx_q", "idx_v")) + .def("setIndexes", setIndexes1, bp::args("self", "id", "idx_q", "idx_v", "idx_vExtended")) .def( "hasSameIndexes", &JointModel::hasSameIndexes, bp::args("self", "other"), "Check if this has same indexes than other.") @@ -73,6 +76,10 @@ namespace pinocchio { return self.idx_v(); } + static int getIdx_vExtended(const JointModel & self) + { + return self.idx_vExtended(); + } static int getNq(const JointModel & self) { return self.nq(); @@ -81,6 +88,26 @@ namespace pinocchio { return self.nv(); } + static int getNvExtended(const JointModel & self) + { + return self.nvExtended(); + } + + static void + setIndexes0(JointModelDerived & self, const int & id, const int & idx_q, const int & idx_v) + { + self.setIndexes(id, idx_q, idx_v); + } + + static void setIndexes1( + JointModelDerived & self, + const int & id, + const int & idx_q, + const int & idx_v, + const int & idx_vExtended) + { + self.setIndexes(id, idx_q, idx_v, idx_vExtended); + } static void expose() { 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..b94509f167 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("nvExtended", &Model::nvExtended, "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,11 @@ 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_vExtendeds", &Model::idx_vExtendeds, + "Starting index of the *i*th joint in the jacobian space.") + .add_property( + "nvExtendeds", &Model::nvExtendeds, "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*, " @@ -129,6 +135,14 @@ namespace pinocchio "subtrees", &Model::subtrees, "Vector of subtrees. subtree[j] corresponds to the subtree supported by the joint j.") + .def_readwrite( + "mimicking_joints", &Model::mimicking_joints, + "Vector of mimicking joints in the tree (with type MimicTpl)") + + .def_readwrite( + "mimicked_joints", &Model::mimicked_joints, + "Vector of mimicked joints in the tree (can be any joint type)") + .def_readwrite( "gravity", &Model::gravity, "Motion vector corresponding to the gravity field expressed in the world Frame.") diff --git a/include/pinocchio/context/casadi.hpp b/include/pinocchio/context/casadi.hpp index 72104030cb..d26a338316 100644 --- a/include/pinocchio/context/casadi.hpp +++ b/include/pinocchio/context/casadi.hpp @@ -12,6 +12,7 @@ #define PINOCCHIO_SKIP_ALGORITHM_CONTACT_JACOBIAN #define PINOCCHIO_SKIP_ALGORITHM_CHOLESKY #define PINOCCHIO_SKIP_ALGORITHM_MODEL +#define PINOCCHIO_SKIP_ALGORITHM_GEOMETRY #define PINOCCHIO_SKIP_MULTIBODY_SAMPLE_MODELS #define PINOCCHIO_SKIP_CASADI_UNSUPPORTED diff --git a/include/pinocchio/context/cppad.hpp b/include/pinocchio/context/cppad.hpp index ae72ef9a0c..92fe6e248e 100644 --- a/include/pinocchio/context/cppad.hpp +++ b/include/pinocchio/context/cppad.hpp @@ -13,6 +13,8 @@ #define PINOCCHIO_SKIP_ALGORITHM_CONTACT_CHOLESKY #define PINOCCHIO_SKIP_ALGORITHM_CONTACT_JACOBIAN #define PINOCCHIO_SKIP_ALGORITHM_CHOLESKY +#define PINOCCHIO_SKIP_ALGORITHM_MODEL +#define PINOCCHIO_SKIP_ALGORITHM_GEOMETRY #define PINOCCHIO_SKIP_MULTIBODY_SAMPLE_MODELS namespace pinocchio diff --git a/include/pinocchio/context/cppadcg.hpp b/include/pinocchio/context/cppadcg.hpp index 9507b83cc7..6fdce4692a 100644 --- a/include/pinocchio/context/cppadcg.hpp +++ b/include/pinocchio/context/cppadcg.hpp @@ -14,6 +14,8 @@ #define PINOCCHIO_SKIP_ALGORITHM_CONTACT_CHOLESKY #define PINOCCHIO_SKIP_ALGORITHM_CONTACT_JACOBIAN #define PINOCCHIO_SKIP_ALGORITHM_CHOLESKY +#define PINOCCHIO_SKIP_ALGORITHM_MODEL +#define PINOCCHIO_SKIP_ALGORITHM_GEOMETRY #define PINOCCHIO_SKIP_MULTIBODY_SAMPLE_MODELS namespace pinocchio diff --git a/include/pinocchio/multibody/data.hpp b/include/pinocchio/multibody/data.hpp index d19468315c..3398432471 100644 --- a/include/pinocchio/multibody/data.hpp +++ b/include/pinocchio/multibody/data.hpp @@ -324,6 +324,13 @@ namespace pinocchio /// \brief End index of the Joint motion subspace std::vector end_idx_v_fromRow; + /// \brief Extended model mapping of the joint rows (idx_v_extended_fromRow[idx_v_extended] = + /// idx_v) + std::vector idx_v_extended_fromRow; + + /// \brief When mimic joints are in the tree, this will store the index of the first non mimic + /// child of each mimic joint. (useful in crba) + std::vector mimic_subtree_joint; /// \brief Joint space intertia matrix square root (upper trianglular part) computed with a /// Cholesky Decomposition. MatrixXs U; @@ -341,6 +348,12 @@ namespace pinocchio /// \brief First previous non-zero row in M (used in Cholesky Decomposition). std::vector parents_fromRow; + /// \brief First previous non-zero row belonging to a mimic joint in M (used in Jacobian). + std::vector mimic_parents_fromRow; + + /// \brief First previous non-zero row belonging to a non mimic joint in M (used in Jacobian). + std::vector non_mimic_parents_fromRow; + /// \brief Each element of this vector corresponds to the ordered list of indexes belonging to /// the supporting tree of the /// given index at the row level. It may be helpful to retrieve the sparsity pattern @@ -354,9 +367,10 @@ namespace pinocchio /// \note The columns of J corresponds to the basis of the spatial velocities of each joint and /// expressed at the origin of the inertial frame. In other words, if \f$ v_{J_{i}} = S_{i} /// \dot{q}_{i}\f$ is the relative velocity of the joint i regarding to its parent, then \f$J = - /// \begin{bmatrix} ^{0}X_{1} S_{1} & \cdots & ^{0}X_{i} S_{i} & \cdots & ^{0}X_{\text{nj}} - /// S_{\text{nj}} \end{bmatrix} \f$. This Jacobian has no special meaning. To get the jacobian - /// of a precise joint, you need to call pinocchio::getJointJacobian + /// \begin{bmatrix} ^{0}X_{1} S_{1} & \cdots & ^{0}X_{i} S_{i} & \cdots & + /// ^{0}X_{\text{nvExtended}} S_{\text{nvExtended}} \end{bmatrix} \f$. This Jacobian has no + /// special meaning. To get the jacobian of a precise joint, you need to call + /// pinocchio::getJointJacobian Matrix6x J; /// \brief Derivative of the Jacobian with respect to the time. diff --git a/include/pinocchio/multibody/data.hxx b/include/pinocchio/multibody/data.hxx index c0aed6741c..5de70e1ead 100644 --- a/include/pinocchio/multibody/data.hxx +++ b/include/pinocchio/multibody/data.hxx @@ -70,18 +70,21 @@ namespace pinocchio , Fcrb((std::size_t)model.njoints, Matrix6x::Zero(6, model.nv)) , lastChild((std::size_t)model.njoints, -1) , nvSubtree((std::size_t)model.njoints, -1) - , start_idx_v_fromRow((std::size_t)model.nv, -1) - , end_idx_v_fromRow((std::size_t)model.nv, -1) + , start_idx_v_fromRow((std::size_t)model.nvExtended, -1) + , end_idx_v_fromRow((std::size_t)model.nvExtended, -1) , U(MatrixXs::Identity(model.nv, model.nv)) , D(VectorXs::Zero(model.nv)) , Dinv(VectorXs::Zero(model.nv)) , tmp(VectorXs::Zero(model.nv)) - , parents_fromRow((std::size_t)model.nv, -1) + , parents_fromRow((std::size_t)model.nvExtended, -1) + , mimic_parents_fromRow((std::size_t)model.nvExtended, -1) + , non_mimic_parents_fromRow((std::size_t)model.nvExtended, -1) + , idx_v_extended_fromRow((std::size_t)model.nvExtended, -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)) + , nvSubtree_fromRow((std::size_t)model.nvExtended, -1) + , J(Matrix6x::Zero(6, model.nvExtended)) + , dJ(Matrix6x::Zero(6, model.nvExtended)) + , ddJ(Matrix6x::Zero(6, model.nvExtended)) , psid(Matrix6x::Zero(6, model.nv)) , psidd(Matrix6x::Zero(6, model.nv)) , dVdq(Matrix6x::Zero(6, model.nv)) @@ -122,6 +125,7 @@ namespace pinocchio , par_cons_ind((std::size_t)model.njoints, 0) , a_bias((std::size_t)model.njoints, Motion::Zero()) , KAS((std::size_t)model.njoints, MatrixXs::Zero(0, 0)) + #if EIGEN_VERSION_AT_LEAST(3, 2, 90) && !EIGEN_VERSION_AT_LEAST(3, 2, 93) , kinematic_hessians( 6, @@ -158,6 +162,7 @@ namespace pinocchio , constraints_supported_dim((std::size_t)model.njoints, 0) , constraints_supported((std::size_t)model.njoints) , constraints_on_joint((std::size_t)model.njoints) + , mimic_subtree_joint() { typedef typename Model::JointIndex JointIndex; @@ -200,11 +205,39 @@ 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]); - nvSubtree[(Index)i] = model.joints[(Index)lastChild[(Index)i]].idx_v() - + model.joints[(Index)lastChild[(Index)i]].nv() - - model.joints[(Index)i].idx_v(); + // Build a "correct" representation of mimic nvSubtree by using nvExtended, which will cover + // its children nv, and allow for a simple check + if (boost::get>(&model.joints[i])) + nvSubtree[(Index)i] = 0; + else + { + int nv_; + if (boost::get>( + &model.joints[(Index)lastChild[(Index)i]])) + nv_ = model.joints[(Index)lastChild[(Index)i]].nvExtended(); + else + nv_ = model.joints[(Index)lastChild[(Index)i]].nv(); + nvSubtree[(Index)i] = + model.joints[(Index)lastChild[(Index)i]].idx_v() + nv_ - model.joints[(Index)i].idx_v(); + } + } + // fill mimic data + for (const JointIndex mimicking_id : model.mimicking_joints) + { + const auto & mimicking_sub = model.subtrees[mimicking_id]; + size_t j = 1; + for (; j < mimicking_sub.size(); j++) + { + if (model.nvs[mimicking_sub[j]] != 0) + break; + } + if (mimicking_sub.size() == 1) + mimic_subtree_joint.push_back(0); + else + mimic_subtree_joint.push_back(mimicking_sub[j]); } } @@ -217,25 +250,67 @@ namespace pinocchio for (Index joint = 1; joint < (Index)(model.njoints); joint++) { const Index & parent = model.parents[joint]; - const int nvj = model.joints[joint].nv(); const int idx_vj = model.joints[joint].idx_v(); + const int nvj = model.joints[joint].nv(); + const int nvExtended_j = model.joints[joint].nvExtended(); + const int idx_vExtended_j = model.joints[joint].idx_vExtended(); + assert(idx_vExtended_j >= 0 && idx_vExtended_j < model.nvExtended); assert(idx_vj >= 0 && idx_vj < model.nv); + if (parent > 0) - parents_fromRow[(Index)idx_vj] = - model.joints[parent].idx_v() + model.joints[parent].nv() - 1; + parents_fromRow[(Index)idx_vExtended_j] = + model.joints[parent].idx_vExtended() + model.joints[parent].nvExtended() - 1; else - parents_fromRow[(Index)idx_vj] = -1; - nvSubtree_fromRow[(Index)idx_vj] = nvSubtree[joint]; + parents_fromRow[(Index)idx_vExtended_j] = -1; + JointIndex first_non_mimic_parent_id = parent; + while (first_non_mimic_parent_id > 0 + && boost::get>( + &model.joints[first_non_mimic_parent_id])) + { + first_non_mimic_parent_id = model.parents[first_non_mimic_parent_id]; + } + + if (first_non_mimic_parent_id > 0) + non_mimic_parents_fromRow[(Index)idx_vExtended_j] = + model.joints[first_non_mimic_parent_id].idx_vExtended() + + model.joints[first_non_mimic_parent_id].nvExtended() - 1; + else + non_mimic_parents_fromRow[(Index)idx_vExtended_j] = -1; + + JointIndex first_mimic_parent_id = parent; + while (first_mimic_parent_id > 0 + && !boost::get>( + &model.joints[first_mimic_parent_id])) + { + first_mimic_parent_id = model.parents[first_mimic_parent_id]; + } + + if (first_mimic_parent_id > 0) + mimic_parents_fromRow[(Index)idx_vExtended_j] = + model.joints[first_mimic_parent_id].idx_vExtended() + + model.joints[first_mimic_parent_id].nvExtended() - 1; + else + mimic_parents_fromRow[(Index)idx_vExtended_j] = -1; + + nvSubtree_fromRow[(Index)idx_vExtended_j] = nvSubtree[joint]; start_idx_v_fromRow[(size_t)idx_vj] = idx_vj; - end_idx_v_fromRow[(size_t)idx_vj] = idx_vj + nvj - 1; - for (int row = 1; row < nvj; ++row) + end_idx_v_fromRow[(size_t)idx_vj] = idx_vj + nvExtended_j - 1; + idx_v_extended_fromRow[(size_t)idx_vExtended_j] = idx_vj; + + for (int row = 1; row < nvExtended_j; ++row) { - parents_fromRow[(size_t)(idx_vj + row)] = idx_vj + row - 1; - nvSubtree_fromRow[(size_t)(idx_vj + row)] = nvSubtree[joint] - row; - start_idx_v_fromRow[(size_t)(idx_vj + row)] = start_idx_v_fromRow[(size_t)idx_vj]; - end_idx_v_fromRow[(size_t)(idx_vj + row)] = end_idx_v_fromRow[(size_t)idx_vj]; + parents_fromRow[(size_t)(idx_vExtended_j + row)] = idx_vExtended_j + row - 1; + mimic_parents_fromRow[(size_t)(idx_vExtended_j + row)] = idx_vExtended_j + row - 1; + non_mimic_parents_fromRow[(size_t)(idx_vExtended_j + row)] = idx_vExtended_j + row - 1; + nvSubtree_fromRow[(size_t)(idx_vExtended_j + row)] = nvSubtree[joint] - row; + start_idx_v_fromRow[(size_t)(idx_vExtended_j + row)] = + start_idx_v_fromRow[(size_t)idx_vExtended_j]; + end_idx_v_fromRow[(size_t)(idx_vExtended_j + row)] = + end_idx_v_fromRow[(size_t)idx_vExtended_j]; + idx_v_extended_fromRow[(size_t)(idx_vExtended_j + row)] = + idx_v_extended_fromRow[(size_t)idx_vExtended_j] + row; } } } @@ -297,6 +372,9 @@ namespace pinocchio && data1.end_idx_v_fromRow == data2.end_idx_v_fromRow && data1.U == data2.U && data1.D == data2.D && data1.Dinv == data2.Dinv && data1.parents_fromRow == data2.parents_fromRow + && data1.mimic_parents_fromRow == data2.mimic_parents_fromRow + && data1.non_mimic_parents_fromRow == data2.non_mimic_parents_fromRow + && data1.idx_v_extended_fromRow == data2.idx_v_extended_fromRow && data1.supports_fromRow == data2.supports_fromRow && data1.nvSubtree_fromRow == data2.nvSubtree_fromRow && data1.J == data2.J && data1.dJ == data2.dJ && data1.ddJ == data2.ddJ && data1.psid == data2.psid 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..774111bb1e 100644 --- a/include/pinocchio/multibody/joint/fwd.hpp +++ b/include/pinocchio/multibody/joint/fwd.hpp @@ -148,6 +148,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..b40ed05a87 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 JointNvExtendVisitor to get the dimension of + * the joint extended tangent space + * + * @param[in] jmodel The JointModelVariant + * + * @return The dimension of joint extended tangent space + */ + template class JointCollectionTpl> + inline int nvExtended(const JointModelTpl & jmodel); + /** * @brief Visit a JointModelTpl through JointConfigurationLimitVisitor * to get the configurations limits @@ -181,7 +192,7 @@ namespace pinocchio inline int idx_q(const JointModelTpl & jmodel); /** - * @brief Visit a JointModelTpl through JointIdxVVisitor to get the index in the full model + * @brief Visit a JointModelTpl through JointIdxVVisitor to get the index in the model * tangent space corresponding to the first joint tangent space degree * * @param[in] jmodel The JointModelVariant @@ -192,6 +203,18 @@ namespace pinocchio template class JointCollectionTpl> inline int idx_v(const JointModelTpl & jmodel); + /** + * @brief Visit a JointModelTpl through JointIdvExtendedVisitor to get the index in the model + * extended tangent space corresponding to the joint first joint extended tangent space degree + * + * @param[in] jmodel The JointModelVariant + * + * @return The index in the model extended tangent space corresponding to the first + * joint extended tangent space degree + */ + template class JointCollectionTpl> + inline int idx_vExtended(const JointModelTpl & jmodel); + /** * @brief Visit a JointModelTpl through JointIdVisitor to get the index of the joint in the * kinematic chain @@ -203,6 +226,29 @@ namespace pinocchio template class JointCollectionTpl> inline JointIndex id(const JointModelTpl & jmodel); + /** + * @brief Visit a JointModelTpl through JointSetIndexesVisitor to set + * the indexes of the joint in the kinematic chain + * + * @param[in] jmodel The JointModelVariant + * @param[in] id The index of joint in the kinematic chain + * @param[in] q The index in the full model configuration space corresponding to the first + * degree of freedom + * @param[in] v The index in the full model tangent space corresponding to the first joint + * tangent space degree + * @param[in] vExtended The index in the model extended tangent space corresponding to the + * joint first extended tangent 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, + int vExtended); + /** * @brief Visit a JointModelTpl through JointSetIndexesVisitor to set * the indexes of the joint in the kinematic chain @@ -423,6 +469,34 @@ namespace pinocchio const JointDataTpl & jmodel_generic, const JointDataBase & jmodel); + /** + * @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..8ef3f0a4dd 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 JointMappedConfigSelectorVisitor + : 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.JointMappedConfigSelector(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 JointNvExtendedVisitor visitor + */ + struct JointNvExtendedVisitor : boost::static_visitor + { + template + int operator()(const JointModelBase & jmodel) const + { + return jmodel.nvExtended(); + } + + template class JointCollectionTpl> + static int run(const JointModelTpl & jmodel) + { + return boost::apply_visitor(JointNvExtendedVisitor(), jmodel); + } + }; + + template class JointCollectionTpl> + inline int nvExtended(const JointModelTpl & jmodel) + { + return JointNvExtendedVisitor::run(jmodel); + } + /** * @brief JointConfigurationLimitVisitor visitor */ @@ -341,6 +390,30 @@ namespace pinocchio return JointIdxVVisitor::run(jmodel); } + /** + * @brief JointIdxVExtendedVisitor visitor + */ + struct JointIdxVExtendedVisitor : boost::static_visitor + { + template + int operator()(const JointModelBase & jmodel) const + { + return jmodel.idx_vExtended(); + } + + template class JointCollectionTpl> + static int run(const JointModelTpl & jmodel) + { + return boost::apply_visitor(JointIdxVExtendedVisitor(), jmodel); + } + }; + + template class JointCollectionTpl> + inline int idx_vExtended(const JointModelTpl & jmodel) + { + return JointIdxVExtendedVisitor::run(jmodel); + } + /** * @brief JointIdVisitor visitor */ @@ -373,33 +446,50 @@ namespace pinocchio JointIndex id; int q; int v; + int vExtended; - JointSetIndexesVisitor(JointIndex id, int q, int v) + JointSetIndexesVisitor(JointIndex id, int q, int v, int vExtended) : id(id) , q(q) , v(v) + , vExtended(vExtended) { } template void operator()(JointModelBase & jmodel) const { - jmodel.setIndexes(id, q, v); + jmodel.setIndexes(id, q, v, vExtended); } 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 vExtended) { - return boost::apply_visitor(JointSetIndexesVisitor(id, q, v), jmodel); + return boost::apply_visitor(JointSetIndexesVisitor(id, q, v, vExtended), jmodel); } }; + template class JointCollectionTpl> + inline void setIndexes( + JointModelTpl & jmodel, + JointIndex id, + int q, + int v, + int vExtended) + { + return JointSetIndexesVisitor::run(jmodel, id, q, v, vExtended); + } + template class JointCollectionTpl> inline void setIndexes( JointModelTpl & jmodel, JointIndex id, int q, int v) { - return JointSetIndexesVisitor::run(jmodel, id, q, v); + return JointSetIndexesVisitor::run(jmodel, id, q, v, v); } /** @@ -864,6 +954,111 @@ 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 + { + PINOCCHIO_THROW_PRETTY(std::invalid_argument, "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); + } + + // Visitor to use when visiting another joint, but still in need of visiting another + // joint to take advantage of the specialisations that exists for MotionSubspace * Force + template + struct UseMotionSubspaceNoMalloc : public boost::static_visitor + { + ForceType F; + + UseMotionSubspaceNoMalloc(ForceType F_) + : F(F_) + { + } + + template + ReturnType operator()(const JointDataBase & jdata) const + { + return jdata.S().transpose() * F; + } + + static ReturnType run(const JointDataTpl & jdata, const ForceType & F_) + { + return boost::apply_visitor(UseMotionSubspaceNoMalloc(F_), jdata); + } + }; + /// @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..8030e507f6 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,61 @@ 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 &, + const Scalar &, + const Scalar &, + const Eigen::MatrixBase &) + { + assert(false && "Joint cannot be used with JointMimic."); } }; /// /// \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..e36ec56b18 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, + NVExtended = 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 }; @@ -147,21 +150,14 @@ namespace pinocchio { return classname(); } - }; - template class JointCollectionTpl> - inline std::ostream & operator<<( - std::ostream & os, const JointDataCompositeTpl & jdata) - { - typedef typename JointDataCompositeTpl::JointDataVector - JointDataVector; - - os << "JointDataComposite containing following models:\n"; - for (typename JointDataVector::const_iterator it = jdata.joints.begin(); - it != jdata.joints.end(); ++it) - os << " " << shortname(*it) << std::endl; - return os; - } + void disp(std::ostream & os) const + { + os << "JointDataComposite containing following models:\n"; + for (typename JointDataVector::const_iterator it = joints.begin(); it != joints.end(); ++it) + os << " " << it->shortname() << std::endl; + } + }; template< typename NewScalar, @@ -195,8 +191,10 @@ namespace pinocchio using Base::id; using Base::idx_q; using Base::idx_v; + using Base::idx_vExtended; using Base::nq; using Base::nv; + using Base::nvExtended; using Base::setIndexes; /// \brief Default contructor @@ -205,6 +203,7 @@ namespace pinocchio , jointPlacements() , m_nq(0) , m_nv(0) + , m_nvExtended(0) , njoints(0) { } @@ -215,14 +214,17 @@ namespace pinocchio , jointPlacements() , m_nq(0) , m_nv(0) + , m_nvExtended(0) , njoints(0) { joints.reserve(size); jointPlacements.reserve(size); m_idx_q.reserve(size); m_idx_v.reserve(size); + m_idx_vExtended.reserve(size); m_nqs.reserve(size); m_nvs.reserve(size); + m_nvExtendeds.reserve(size); } /// @@ -238,10 +240,13 @@ namespace pinocchio , jointPlacements(1, placement) , m_nq(jmodel.nq()) , m_nv(jmodel.nv()) + , m_nvExtended(jmodel.nvExtended()) , m_idx_q(1, 0) , m_nqs(1, jmodel.nq()) , m_idx_v(1, 0) , m_nvs(1, jmodel.nv()) + , m_idx_vExtended(1, 0) + , m_nvExtendeds(1, jmodel.nvExtended()) , njoints(1) { } @@ -257,10 +262,13 @@ namespace pinocchio , jointPlacements(other.jointPlacements) , m_nq(other.m_nq) , m_nv(other.m_nv) + , m_nvExtended(other.m_nvExtended) , 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_vExtended(other.m_idx_vExtended) + , m_nvExtendeds(other.m_nvExtendeds) , njoints(other.njoints) { } @@ -282,6 +290,7 @@ namespace pinocchio m_nq += jmodel.nq(); m_nv += jmodel.nv(); + m_nvExtended += jmodel.nvExtended(); updateJointIndexes(); njoints++; @@ -367,13 +376,17 @@ namespace pinocchio { return m_nq; } + int nvExtended_impl() const + { + return m_nvExtended; + } /** * @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 vExtended) { - Base::setIndexes_impl(id, q, v); + Base::setIndexes_impl(id, q, v, vExtended); updateJointIndexes(); } @@ -391,10 +404,13 @@ namespace pinocchio Base::operator=(other); m_nq = other.m_nq; m_nv = other.m_nv; + m_nvExtended = other.m_nvExtended; m_idx_q = other.m_idx_q; m_idx_v = other.m_idx_v; + m_idx_vExtended = other.m_idx_vExtended; m_nqs = other.m_nqs; m_nvs = other.m_nvs; + m_nvExtendeds = other.m_nvExtendeds; joints = other.joints; jointPlacements = other.jointPlacements; njoints = other.njoints; @@ -407,10 +423,13 @@ namespace pinocchio { return Base::isEqual(other) && internal::comparison_eq(nq(), other.nq()) && internal::comparison_eq(nv(), other.nv()) + && internal::comparison_eq(nvExtended(), other.nvExtended()) && 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_vExtended, other.m_idx_vExtended) && internal::comparison_eq(m_nqs, other.m_nqs) && internal::comparison_eq(m_nvs, other.m_nvs) + && internal::comparison_eq(m_nvExtendeds, other.m_nvExtendeds) && internal::comparison_eq(joints, other.joints) && internal::comparison_eq(jointPlacements, other.jointPlacements) && internal::comparison_eq(njoints, other.njoints); @@ -422,13 +441,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_vExtended()); res.m_nq = m_nq; res.m_nv = m_nv; + res.m_nvExtended = m_nvExtended; res.m_idx_q = m_idx_q; res.m_idx_v = m_idx_v; + res.m_idx_vExtended = m_idx_vExtended; res.m_nqs = m_nqs; res.m_nvs = m_nvs; + res.m_nvExtendeds = m_nvExtendeds; res.njoints = njoints; res.joints.resize(joints.size()); @@ -448,6 +470,19 @@ namespace pinocchio /// relatively to their parent. PINOCCHIO_ALIGNED_STD_VECTOR(SE3) jointPlacements; + template + typename SizeDepType::template SegmentReturn::ConstType + JointMappedConfigSelector(const Eigen::MatrixBase & a) const + { + return a.segment(Base::i_q, nq()); + } + template + typename SizeDepType::template SegmentReturn::Type + JointMappedConfigSelector(Eigen::MatrixBase & a) const + { + return a.segment(Base::i_q, nq()); + } + template typename SizeDepType::template SegmentReturn::ConstType jointConfigSelector(const Eigen::MatrixBase & a) const @@ -461,6 +496,19 @@ namespace pinocchio return a.segment(Base::i_q, nq()); } + template + typename SizeDepType::template SegmentReturn::ConstType + JointMappedVelocitySelector(const Eigen::MatrixBase & a) const + { + return a.segment(Base::i_v, nv()); + } + template + typename SizeDepType::template SegmentReturn::Type + JointMappedVelocitySelector(Eigen::MatrixBase & a) const + { + return a.segment(Base::i_v, nv()); + } + template typename SizeDepType::template SegmentReturn::ConstType jointVelocitySelector(const Eigen::MatrixBase & a) const @@ -481,10 +529,22 @@ namespace pinocchio return A.middleCols(Base::i_v, nv()); } template + typename SizeDepType::template ColsReturn::ConstType + jointExtendedModelCols(const Eigen::MatrixBase & A) const + { + return A.middleCols(Base::i_vExtended, nvExtended()); + } + template typename SizeDepType::template ColsReturn::Type jointCols(Eigen::MatrixBase & A) const { return A.middleCols(Base::i_v, nv()); } + template + typename SizeDepType::template ColsReturn::Type + jointExtendedModelCols(Eigen::MatrixBase & A) const + { + return A.middleCols(Base::i_vExtended, nvExtended()); + } template typename SizeDepType::template SegmentReturn::ConstType @@ -498,6 +558,33 @@ namespace pinocchio { return a.segment(Base::i_q, nq()); } + + template + typename SizeDepType::template SegmentReturn::ConstType + JointMappedConfigSelector_impl(const Eigen::MatrixBase & a) const + { + return a.segment(Base::i_q, nq()); + } + template + typename SizeDepType::template SegmentReturn::Type + JointMappedConfigSelector_impl(Eigen::MatrixBase & a) const + { + return a.segment(Base::i_q, nq()); + } + + template + typename SizeDepType::template SegmentReturn::ConstType + JointMappedVelocitySelector_impl(const Eigen::MatrixBase & a) const + { + return a.segment(Base::i_v, nv()); + } + template + typename SizeDepType::template SegmentReturn::Type + JointMappedVelocitySelector_impl(Eigen::MatrixBase & a) const + { + return a.segment(Base::i_v, nv()); + } + template typename SizeDepType::template SegmentReturn::ConstType jointVelocitySelector_impl(const Eigen::MatrixBase & a) const @@ -518,11 +605,33 @@ namespace pinocchio return A.middleCols(Base::i_v, nv()); } template + typename SizeDepType::template ColsReturn::ConstType + jointExtendedModelCols_impl(const Eigen::MatrixBase & A) const + { + return A.middleCols(Base::i_vExtended, nvExtended()); + } + template typename SizeDepType::template ColsReturn::Type jointCols_impl(Eigen::MatrixBase & A) const { return A.middleCols(Base::i_v, nv()); } + template + typename SizeDepType::template ColsReturn::Type + jointExtendedModelCols_impl(Eigen::MatrixBase & A) const + { + return A.middleCols(Base::i_vExtended, nvExtended()); + } + + void disp(std::ostream & os) const + { + typedef typename JointModelCompositeTpl::JointModelVector + JointModelVector; + + os << "JointModelComposite containing following models:\n"; + for (typename JointModelVector::const_iterator it = joints.begin(); it != joints.end(); ++it) + os << " " << it->shortname() << std::endl; + } protected: friend struct Serialize; @@ -536,11 +645,14 @@ namespace pinocchio { int idx_q = this->idx_q(); int idx_v = this->idx_v(); + int idx_vExtended = this->idx_vExtended(); m_idx_q.resize(joints.size()); m_idx_v.resize(joints.size()); + m_idx_vExtended.resize(joints.size()); m_nqs.resize(joints.size()); m_nvs.resize(joints.size()); + m_nvExtendeds.resize(joints.size()); for (size_t i = 0; i < joints.size(); ++i) { @@ -548,16 +660,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_vExtended[i] = idx_vExtended; + ::pinocchio::setIndexes(joint, i, idx_q, idx_v, idx_vExtended); m_nqs[i] = ::pinocchio::nq(joint); m_nvs[i] = ::pinocchio::nv(joint); + m_nvExtendeds[i] = ::pinocchio::nvExtended(joint); idx_q += m_nqs[i]; idx_v += m_nvs[i]; + idx_vExtended += m_nvExtendeds[i]; } } /// \brief Dimensions of the config and tangent space of the composite joint. - int m_nq, m_nv; + int m_nq, m_nv, m_nvExtended; /// Keep information of both the dimension and the position of the joints in the composition. @@ -569,27 +684,16 @@ 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_vExtended; + /// \brief Dimension of the segment in the jacobian matrix + std::vector m_nvExtendeds; public: /// \brief Number of joints contained in the JointModelComposite int njoints; }; - template class JointCollectionTpl> - inline std::ostream & operator<<( - std::ostream & os, const JointModelCompositeTpl & jmodel) - { - typedef typename JointModelCompositeTpl::JointModelVector - JointModelVector; - - os << "JointModelComposite containing following models:\n"; - for (typename JointModelVector::const_iterator it = jmodel.joints.begin(); - it != jmodel.joints.end(); ++it) - os << " " << shortname(*it) << std::endl; - - return os; - } - } // namespace pinocchio #include diff --git a/include/pinocchio/multibody/joint/joint-data-base.hpp b/include/pinocchio/multibody/joint/joint-data-base.hpp index e47c38c415..8782310985 100644 --- a/include/pinocchio/multibody/joint/joint-data-base.hpp +++ b/include/pinocchio/multibody/joint/joint-data-base.hpp @@ -273,9 +273,9 @@ namespace pinocchio os << shortname() << endl; } - friend std::ostream & operator<<(std::ostream & os, const JointDataBase & joint) + friend std::ostream & operator<<(std::ostream & os, const JointDataBase & jdata) { - joint.disp(os); + jdata.derived().disp(os); return os; } diff --git a/include/pinocchio/multibody/joint/joint-free-flyer.hpp b/include/pinocchio/multibody/joint/joint-free-flyer.hpp index 7274494c3b..c94044c80a 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, + NVExtended = 6 }; typedef _Scalar Scalar; enum @@ -190,6 +191,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 }; @@ -289,6 +292,7 @@ namespace pinocchio using Base::id; using Base::idx_q; using Base::idx_v; + using Base::idx_vExtended; using Base::setIndexes; JointDataDerived createData() const @@ -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_vExtended()); return res; } diff --git a/include/pinocchio/multibody/joint/joint-generic.hpp b/include/pinocchio/multibody/joint/joint-generic.hpp index 478c1278b8..975f614544 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, + NVExtended = 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 + JointMappedConfigSelector_impl(const Eigen::MatrixBase & a) const + { + typedef const Eigen::MatrixBase & InputType; + typedef typename SizeDepType::template SegmentReturn::ConstType ReturnType; + typedef JointMappedConfigSelectorVisitor Visitor; + typename Visitor::ArgsType arg(a); + return Visitor::run(*this, arg); + } + + // Non-const access + template + typename SizeDepType::template SegmentReturn::Type + JointMappedConfigSelector_impl(Eigen::MatrixBase & a) const + { + typedef Eigen::MatrixBase & InputType; + typedef typename SizeDepType::template SegmentReturn::Type ReturnType; + typedef JointMappedConfigSelectorVisitor 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 nvExtended_impl() const + { + return ::pinocchio::nvExtended(*this); + } int idx_q_impl() const { @@ -405,6 +437,10 @@ namespace pinocchio { return ::pinocchio::idx_v(*this); } + int idx_vExtended_impl() const + { + return ::pinocchio::idx_vExtended(*this); + } JointIndex id_impl() const { @@ -413,7 +449,12 @@ namespace pinocchio void setIndexes(JointIndex id, int nq, int nv) { - ::pinocchio::setIndexes(*this, id, nq, nv); + ::pinocchio::setIndexes(*this, id, nq, nv, nv); + } + + void setIndexes(JointIndex id, int nq, int nv, int nvExtended) + { + ::pinocchio::setIndexes(*this, id, nq, nv, nvExtended); } /// \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..2c1194a425 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, + NVExtended = 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 }; @@ -638,6 +641,7 @@ namespace pinocchio using Base::id; using Base::idx_q; using Base::idx_v; + using Base::idx_vExtended; using Base::setIndexes; JointModelHelicalUnalignedTpl() @@ -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_vExtended()); return res; } diff --git a/include/pinocchio/multibody/joint/joint-helical.hpp b/include/pinocchio/multibody/joint/joint-helical.hpp index dc92b2fb44..ea5a9a5036 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, + NVExtended = 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 }; @@ -837,6 +840,7 @@ namespace pinocchio using Base::id; using Base::idx_q; using Base::idx_v; + using Base::idx_vExtended; using Base::setIndexes; typedef Eigen::Matrix Vector3; @@ -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_vExtended()); return res; } @@ -967,6 +971,12 @@ namespace pinocchio typedef JointDataHelicalTpl JointDataHZ; typedef JointModelHelicalTpl JointModelHZ; + template + struct ConfigVectorAffineTransform> + { + typedef LinearAffineTransform Type; + }; + } // namespace pinocchio #include diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 4725bc968e..f739902a1d 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -5,117 +5,128 @@ #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" 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) - : m_scaling_factor(scaling_factor) + explicit ScaledJointMotionSubspaceTpl(const Scalar & scaling_factor) + : m_constraint(0) + , m_scaling_factor(scaling_factor) { } - 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 +136,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 +159,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 +189,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 +217,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; - - static ReturnType run(const JointMotionSubspaceBase & constraint) - { - const Constraint & constraint_ = constraint.derived(); - return (constraint_.constraint().transpose() * constraint_.constraint()) - * (constraint_.scaling() * constraint_.scaling()); - } - }; - } // namespace details + mutable DenseBase S; + }; // struct ScaledJointMotionSubspaceTpl - 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 +264,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 +289,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, + NVExtended = Eigen::Dynamic, + MaxNVMimicked = 6 }; - typedef typename traits::JointDataDerived JointDataBase; - typedef typename traits::JointModelDerived JointModelBase; + typedef JointCollectionTpl JointCollection; + typedef JointDataMimicTpl JointDataDerived; + typedef JointModelMimicTpl JointModelDerived; - typedef JointDataMimic JointDataDerived; - typedef JointModelMimic JointModelDerived; - - 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 ScaledJointMotionSubspaceTpl Constraint_t; + typedef SE3Tpl Transformation_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 typename traits::ConfigVector_t ConfigVector_t; - typedef typename traits::TangentVector_t TangentVector_t; - - PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE + typedef Eigen::Matrix U_t; + typedef Eigen::Matrix D_t; + typedef Eigen::Matrix UD_t; + + typedef const Constraint_t & ConstraintTypeConstRef; + typedef Constraint_t & ConstraintTypeRef; + typedef Transformation_t TansformTypeConstRef; + typedef Transformation_t TansformTypeRef; + typedef Motion_t MotionTypeConstRef; + typedef Motion_t MotionTypeRef; + typedef Bias_t BiasTypeConstRef; + typedef Bias_t BiasTypeRef; + typedef U_t UTypeConstRef; + typedef U_t UTypeRef; + typedef D_t DTypeConstRef; + typedef D_t DTypeRef; + typedef UD_t UDTypeConstRef; + typedef UD_t UDTypeRef; + + typedef Eigen::Matrix ConfigVector_t; + typedef Eigen::Matrix TangentVector_t; + + typedef const ConfigVector_t & ConfigVectorTypeConstRef; + typedef ConfigVector_t & ConfigVectorTypeRef; + typedef const TangentVector_t TangentVectorTypeConstRef; + typedef TangentVector_t & TangentVectorTypeRef; + + typedef boost::mpl::false_ is_mimicable_t; }; - 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() - : m_scaling((Scalar)0) - , joint_q(ConfigVector_t::Zero()) - , joint_v(TangentVector_t::Zero()) - , S((Scalar)0) - { - } + typedef JointDataTpl<_Scalar, _Options, JointCollectionTpl> RefJointData; + typedef typename RefJointData::JointDataVariant RefJointDataVariant; - JointDataMimic(const JointDataMimic & other) + JointDataMimicTpl() + : S((Scalar)0) { - *this = other; + joint_q.resize(0, 1); + joint_q_transformed.resize(0, 1); + joint_v.resize(0, 1); + joint_v_transformed.resize(0, 1); } - JointDataMimic(const JointDataBase & jdata, const Scalar & scaling) - : m_jdata_ref(jdata.derived()) - , m_scaling(scaling) - , S(m_jdata_ref.S, scaling) + JointDataMimicTpl( + const RefJointData & jdata, const Scalar & scaling, const int & nq, const int & nv) + : m_jdata_mimicking(checkMimic(jdata.derived())) + , S(m_jdata_mimicking.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; + m_jdata_mimicking = other.m_jdata_mimicking; 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(other.S); 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_mimicking == other.m_jdata_mimicking + && 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; @@ -445,157 +436,177 @@ namespace pinocchio return S; } - TansformTypeConstRef M_accessor() const + Transformation_t M_accessor() const { - return m_jdata_ref.M; - } - TansformTypeRef M_accessor() - { - return m_jdata_ref.M; + return m_jdata_mimicking.M(); } - MotionTypeConstRef v_accessor() const - { - return m_jdata_ref.v; - } - MotionTypeRef v_accessor() + Motion_t v_accessor() const { - return m_jdata_ref.v; + return m_jdata_mimicking.v(); } - BiasTypeConstRef c_accessor() const + Bias_t c_accessor() const { - return m_jdata_ref.c; - } - BiasTypeRef c_accessor() - { - return m_jdata_ref.c; + return m_jdata_mimicking.c(); } - UTypeConstRef U_accessor() const + U_t U_accessor() const { - return m_jdata_ref.U; + return m_jdata_mimicking.U(); } - UTypeRef U_accessor() + + D_t Dinv_accessor() const { - return m_jdata_ref.U; + return m_jdata_mimicking.Dinv(); } - DTypeConstRef Dinv_accessor() const + UD_t UDinv_accessor() const { - return m_jdata_ref.Dinv; + return m_jdata_mimicking.UDinv(); } - DTypeRef Dinv_accessor() + + D_t StU_accessor() const { - return m_jdata_ref.Dinv; + return m_jdata_mimicking.StU(); } - UDTypeConstRef UDinv_accessor() const + friend struct JointModelMimicTpl<_Scalar, _Options, JointCollectionTpl>; + + const RefJointData & jdata() const { - return m_jdata_ref.UDinv; + return m_jdata_mimicking; } - UDTypeRef UDinv_accessor() + RefJointData & jdata() { - return m_jdata_ref.UDinv; + return m_jdata_mimicking; } - DTypeConstRef StU_accessor() const + ConfigVectorTypeRef joint_q_accessor() { - return m_jdata_ref.StU; + return joint_q; } - DTypeRef StU_accessor() + ConfigVectorTypeConstRef joint_q_accessor() const { - return m_jdata_ref.StU; + return joint_q; } - template - friend struct JointModelMimic; - - const JointData & jdata() const + ConfigVector_t & q_transformed() { - return m_jdata_ref; + return joint_q_transformed; } - JointData & jdata() + const ConfigVector_t & q_transformed() const { - return m_jdata_ref; + return joint_q_transformed; } - - const Scalar & scaling() const + TangentVectorTypeRef joint_v_accessor() { - return m_scaling; + return joint_v; } - Scalar & scaling() + TangentVectorTypeConstRef joint_v_accessor() const { - return m_scaling; + return joint_v; } - ConfigVector_t & jointConfiguration() + TangentVector_t & v_transformed() { - return joint_q; + return joint_v_transformed; } - const ConfigVector_t & jointConfiguration() const + const TangentVector_t & v_transformed() const { - return joint_q; + return joint_v_transformed; } - TangentVector_t & jointVelocity() + void disp(std::ostream & os) const { - return joint_v; - } - const TangentVector_t & jointVelocity() const - { - return joint_v; + Base::disp(os); + os << " Mimicking joint data: " << m_jdata_mimicking.shortname() << std::endl; } - protected: - JointData m_jdata_ref; - Scalar m_scaling; + RefJointData m_jdata_mimicking; - /// \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; - - public: + /// \brief Transform velocity vector. + TangentVector_t joint_v_transformed; // data Constraint_t S; - - }; // struct JointDataMimic - - template - struct CastType> + }; // 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 + { + MaxNVMimicked = traits::MaxNVMimicked + }; + + typedef JointCollectionTpl JointCollection; + typedef JointModelTpl JointModel; + + typedef SE3Tpl SE3; + typedef MotionTpl Motion; + typedef InertiaTpl Inertia; - typedef JointModelBase Base; using Base::id; using Base::idx_q; using Base::idx_v; + using Base::idx_vExtended; using Base::nq; using Base::nv; + using Base::nvExtended; 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_mimicking(checkMimic((JointModel)jmodel_mimicking.derived())) , m_scaling(scaling) , m_offset(offset) + , m_nqExtended(jmodel_mimicking.nq()) + , m_nvExtended(jmodel_mimicking.nvExtended()) { + assert(jmodel_mimicking.nq() == jmodel_mimicked.nq()); + assert(jmodel_mimicking.nv() == jmodel_mimicked.nv()); + assert(jmodel_mimicking.nvExtended() == jmodel_mimicked.nvExtended()); + + setMimicIndexes( + jmodel_mimicked.id(), jmodel_mimicked.idx_q(), jmodel_mimicked.idx_v(), + jmodel_mimicked.idx_vExtended()); } Base & base() @@ -615,56 +626,71 @@ namespace pinocchio { return 0; } - - inline int idx_q_impl() const + inline int nvExtended_impl() const { - return m_jmodel_ref.idx_q(); + return m_nvExtended; } - inline int idx_v_impl() const + + /** + * @note q and v are ignored in the _impl for mimic joint because most algorithms will pass + * indexes of their current position in the tree, while in this case idx_q and idx_v should + * remain pointing to the mimicked joint. (See setMimicIndexes) + */ + void setIndexes_impl(JointIndex id, int /*q*/, int /*v*/, int vExtended) { - return m_jmodel_ref.idx_v(); + PINOCCHIO_THROW( + (id > m_jmodel_mimicking.id()), std::invalid_argument, + "Mimic joint index is lower than its directing joint. Should never happen"); + Base::i_id = id; + // Base::i_q = q; + // Base::i_v = v; + Base::i_vExtended = vExtended; } - void setIndexes_impl(JointIndex id, int /*q*/, int /*v*/) + /** + * @brief Specific way for mimic joints to set the mimicked q,v indexes. + * Used for manipulating tree (e.g. appendModel) + * + * @param id Set the mimicking joint id + * @param q Set the mimic joint idx_q (should point to the mimicked joint) + * @param v Set the mimic joint idx_v (should point to the mimicked joint) + * @param vExtended Set the mimicking idx_vExtended + */ + void setMimicIndexes(JointIndex id, int q, int v, int vExtended) { - 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 because only the corresponding subsegment of q,v are passed to the + // m_jmodel_mimicking, thus, its indexes starts at 0 + m_jmodel_mimicking.setIndexes(id, 0, 0, vExtended); + + // idx_q, idx_v are kept separately to extract the subsegment + Base::i_q = q; + Base::i_v = v; } JointDataDerived createData() const { - return JointDataDerived(m_jmodel_ref.createData(), scaling()); + return JointDataDerived( + m_jmodel_mimicking.createData(), scaling(), m_nqExtended, m_nvExtended); } const std::vector hasConfigurationLimit() const { - return m_jmodel_ref.hasConfigurationLimit(); + return m_jmodel_mimicking.hasConfigurationLimit(); } const std::vector hasConfigurationLimitInTangent() const { - return m_jmodel_ref.hasConfigurationLimitInTangent(); + return m_jmodel_mimicking.hasConfigurationLimitInTangent(); } template 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(Base::i_q, m_nqExtended); + configVectorAffineTransform( + m_jmodel_mimicking, jdata.joint_q, m_scaling, m_offset, jdata.joint_q_transformed); + m_jmodel_mimicking.calc(jdata.m_jdata_mimicking, jdata.joint_q_transformed); } template @@ -673,11 +699,14 @@ namespace pinocchio const typename Eigen::MatrixBase & qs, const typename Eigen::MatrixBase & vs) const { - typedef typename ConfigVectorAffineTransform::Type AffineTransform; + jdata.joint_q = qs.segment(Base::i_q, m_nqExtended); + jdata.joint_v = vs.segment(Base::i_v, m_nvExtended); + configVectorAffineTransform( + m_jmodel_mimicking, jdata.joint_q, m_scaling, m_offset, jdata.joint_q_transformed); + jdata.joint_v_transformed = m_scaling * jdata.joint_v; - 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); + m_jmodel_mimicking.calc( + jdata.m_jdata_mimicking, jdata.joint_q_transformed, jdata.joint_v_transformed); } template @@ -687,42 +716,43 @@ namespace pinocchio const Eigen::MatrixBase & I, const bool update_I) const { - // TODO: fixme - m_jmodel_ref.calc_aba( - data.m_jdata_ref, armature, PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I), update_I); + assert( + false + && "Joint Mimic is not supported for aba yet. Remove it from your model if you want to use " + "this function"); } 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_mimicking.template cast(), + ScalarCast::cast(m_scaling), + ScalarCast::cast(m_offset)); + res.setIndexes(id(), Base::i_q, Base::i_v, Base::i_vExtended); + res.setMimicIndexes(m_jmodel_mimicking.id(), Base::i_q, Base::i_v, Base::i_vExtended); return res; } const JointModel & jmodel() const { - return m_jmodel_ref; + return m_jmodel_mimicking; } JointModel & jmodel() { - return m_jmodel_ref; + return m_jmodel_mimicking; } const Scalar & scaling() const @@ -745,42 +775,42 @@ namespace pinocchio protected: // data - JointModel m_jmodel_ref; + JointModel m_jmodel_mimicking; Scalar m_scaling, m_offset; + int m_nqExtended, m_nvExtended; public: /* Acces to dedicated segment in robot config space. */ // Const access template typename SizeDepType::template SegmentReturn::ConstType - jointConfigSelector_impl(const Eigen::MatrixBase & a) const + JointMappedConfigSelector_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(), Base::i_q, m_nqExtended); } // Non-const access template typename SizeDepType::template SegmentReturn::Type - jointConfigSelector_impl(Eigen::MatrixBase & a) const + JointMappedConfigSelector_impl(Eigen::MatrixBase & a) const { - return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_q(), m_jmodel_ref.nq()); + return SizeDepType::segment(a.derived(), Base::i_q, m_nqExtended); } - - /* Acces to dedicated segment in robot config velocity space. */ + /* Acces to dedicated segment in robot tangent space. */ // Const access template - typename SizeDepType::template SegmentReturn::ConstType - jointVelocitySelector_impl(const Eigen::MatrixBase & a) const + typename SizeDepType::template SegmentReturn::ConstType + JointMappedVelocitySelector_impl(const Eigen::MatrixBase & a) const { - return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); + return SizeDepType::segment(a.derived(), Base::i_v, m_nvExtended); } // Non-const access template - typename SizeDepType::template SegmentReturn::Type - jointVelocitySelector_impl(Eigen::MatrixBase & a) const + typename SizeDepType::template SegmentReturn::Type + JointMappedVelocitySelector_impl(Eigen::MatrixBase & a) const { - return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); + return SizeDepType::segment(a.derived(), Base::i_v, m_nvExtended); } /* Acces to dedicated columns in a ForceSet or MotionSet matrix.*/ @@ -789,7 +819,7 @@ namespace pinocchio typename SizeDepType::template ColsReturn::ConstType jointCols_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(), Base::i_v, m_nvExtended); } // Non-const access @@ -797,7 +827,7 @@ namespace pinocchio typename SizeDepType::template ColsReturn::Type jointCols_impl(Eigen::MatrixBase & A) const { - return SizeDepType::middleCols(A.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); + return SizeDepType::middleCols(A.derived(), Base::i_v, m_nvExtended); } /* Acces to dedicated rows in a matrix.*/ @@ -806,7 +836,7 @@ namespace pinocchio typename SizeDepType::template RowsReturn::ConstType jointRows_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(), Base::i_v, m_nvExtended); } // Non-const access @@ -814,19 +844,18 @@ namespace pinocchio typename SizeDepType::template RowsReturn::Type jointRows_impl(Eigen::MatrixBase & A) const { - return SizeDepType::middleRows(A.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); + return SizeDepType::middleRows(A.derived(), Base::i_v, m_nvExtended); } - /// \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 { 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(), Base::i_v, Base::i_v, m_nvExtended, m_nvExtended); } // Non-const access @@ -835,12 +864,51 @@ namespace pinocchio jointBlock_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(), Base::i_v, Base::i_v, m_nvExtended, m_nvExtended); } - }; // struct JointModelMimic + void disp(std::ostream & os) const + { + Base::disp(os); + os << " Mimicking joint type: " << m_jmodel_mimicking.shortname() << std::endl; + os << " Mimicked joint id: " << m_jmodel_mimicking.id() << std::endl; + os << " Mimic scaling: " << m_scaling << std::endl; + os << " Mimic offset: " << m_offset << std::endl; + } + + }; // 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..93e423dbea 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, \ + NVExtended = traits::NVExtended \ }; \ 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_vExtended #define PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelTpl) \ template \ @@ -146,6 +149,10 @@ namespace pinocchio { return derived().nq_impl(); } + int nvExtended() const + { + return derived().nvExtended_impl(); + } // Default _impl methods are reimplemented by dynamic-size joints. int nv_impl() const @@ -156,6 +163,10 @@ namespace pinocchio { return NQ; } + int nvExtended_impl() const + { + return NVExtended; + } int idx_q() const { @@ -165,6 +176,10 @@ namespace pinocchio { return derived().idx_v_impl(); } + int idx_vExtended() const + { + return derived().idx_vExtended_impl(); + } JointIndex id() const { return derived().id_impl(); @@ -178,6 +193,10 @@ namespace pinocchio { return i_v; } + int idx_vExtended_impl() const + { + return i_vExtended; + } JointIndex id_impl() const { return i_id; @@ -185,13 +204,19 @@ namespace pinocchio void setIndexes(JointIndex id, int q, int v) { - derived().setIndexes_impl(id, q, v); + derived().setIndexes_impl(id, q, v, v); + } + + void setIndexes(JointIndex id, int q, int v, int vExtended) + { + derived().setIndexes_impl(id, q, v, vExtended); } - void setIndexes_impl(JointIndex id, int q, int v) + void setIndexes_impl(JointIndex id, int q, int v, int vExtended) { i_id = id, i_q = q; i_v = v; + i_vExtended = vExtended; } void disp(std::ostream & os) const @@ -201,13 +226,15 @@ namespace pinocchio << " index: " << id() << endl << " index q: " << idx_q() << endl << " index v: " << idx_v() << endl + << " index vExtended: " << idx_vExtended() << endl << " nq: " << nq() << endl - << " nv: " << nv() << endl; + << " nv: " << nv() << endl + << " nvExtended: " << nvExtended() << endl; } - friend std::ostream & operator<<(std::ostream & os, const JointModelBase & joint) + friend std::ostream & operator<<(std::ostream & os, const JointModelBase & jmodel) { - joint.disp(os); + jmodel.derived().disp(os); return os; } @@ -254,10 +281,41 @@ 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_vExtended(), idx_vExtended()); } /* Acces to dedicated segment in robot config space. */ + // Const access + template + typename SizeDepType::template SegmentReturn::ConstType + JointMappedConfigSelector(const Eigen::MatrixBase & a) const + { + return derived().JointMappedConfigSelector_impl(a); + } + + template + typename SizeDepType::template SegmentReturn::ConstType + JointMappedConfigSelector_impl(const Eigen::MatrixBase & a) const + { + return SizeDepType::segment(a.derived(), idx_q(), nq()); + } + + // Non-const access + template + typename SizeDepType::template SegmentReturn::Type + JointMappedConfigSelector(Eigen::MatrixBase & a) const + { + return derived().JointMappedConfigSelector_impl(a); + } + + template + typename SizeDepType::template SegmentReturn::Type + JointMappedConfigSelector_impl(Eigen::MatrixBase & a) const + { + return SizeDepType::segment(a, idx_q(), nq()); + } + // Const access template typename SizeDepType::template SegmentReturn::ConstType @@ -289,6 +347,36 @@ namespace pinocchio } /* Acces to dedicated segment in robot config velocity space. */ + // Const access + template + typename SizeDepType::template SegmentReturn::ConstType + JointMappedVelocitySelector(const Eigen::MatrixBase & a) const + { + return derived().JointMappedVelocitySelector_impl(a.derived()); + } + + template + typename SizeDepType::template SegmentReturn::ConstType + JointMappedVelocitySelector_impl(const Eigen::MatrixBase & a) const + { + return SizeDepType::segment(a.derived(), idx_v(), nvExtended()); + } + + // Non-const access + template + typename SizeDepType::template SegmentReturn::Type + JointMappedVelocitySelector(Eigen::MatrixBase & a) const + { + return derived().JointMappedVelocitySelector_impl(a.derived()); + } + + template + typename SizeDepType::template SegmentReturn::Type + JointMappedVelocitySelector_impl(Eigen::MatrixBase & a) const + { + return SizeDepType::segment(a.derived(), idx_v(), nvExtended()); + } + // Const access template typename SizeDepType::template SegmentReturn::ConstType @@ -328,6 +416,13 @@ namespace pinocchio return derived().jointCols_impl(A.derived()); } + template + typename SizeDepType::template ColsReturn::ConstType + jointExtendedModelCols(const Eigen::MatrixBase & A) const + { + return derived().jointExtendedModelCols_impl(A.derived()); + } + template typename SizeDepType::template ColsReturn::ConstType jointCols_impl(const Eigen::MatrixBase & A) const @@ -335,13 +430,28 @@ namespace pinocchio return SizeDepType::middleCols(A.derived(), idx_v(), nv()); } + template + typename SizeDepType::template ColsReturn::ConstType + jointExtendedModelCols_impl(const Eigen::MatrixBase & A) const + { + return SizeDepType::middleCols(A.derived(), idx_vExtended(), nvExtended()); + } + // Non-const access + // TODO rename Jac/Vel into Full/Red (for full system and reduced system ?) template typename SizeDepType::template ColsReturn::Type jointCols(Eigen::MatrixBase & A) const { return derived().jointCols_impl(A.derived()); } + template + typename SizeDepType::template ColsReturn::Type + jointExtendedModelCols(Eigen::MatrixBase & A) const + { + return derived().jointExtendedModelCols_impl(A.derived()); + } + template typename SizeDepType::template ColsReturn::Type jointCols_impl(Eigen::MatrixBase & A) const @@ -349,6 +459,13 @@ namespace pinocchio return SizeDepType::middleCols(A.derived(), idx_v(), nv()); } + template + typename SizeDepType::template ColsReturn::Type + jointExtendedModelCols_impl(Eigen::MatrixBase & A) const + { + return SizeDepType::middleCols(A.derived(), idx_vExtended(), nvExtended()); + } + /* Acces to dedicated rows in a matrix.*/ // Const access template @@ -358,6 +475,13 @@ namespace pinocchio return derived().jointRows_impl(A.derived()); } + template + typename SizeDepType::template RowsReturn::ConstType + jointExtendedModelRows(const Eigen::MatrixBase & A) const + { + return derived().jointExtendedModelRows_impl(A.derived()); + } + template typename SizeDepType::template RowsReturn::ConstType jointRows_impl(const Eigen::MatrixBase & A) const @@ -365,6 +489,13 @@ namespace pinocchio return SizeDepType::middleRows(A.derived(), idx_v(), nv()); } + template + typename SizeDepType::template RowsReturn::ConstType + jointExtendedModelRows_impl(const Eigen::MatrixBase & A) const + { + return SizeDepType::middleRows(A.derived(), idx_vExtended(), nvExtended()); + } + // Non-const access template typename SizeDepType::template RowsReturn::Type jointRows(Eigen::MatrixBase & A) const @@ -372,6 +503,13 @@ namespace pinocchio return derived().jointRows_impl(A.derived()); } + template + typename SizeDepType::template RowsReturn::Type + jointExtendedModelRows(Eigen::MatrixBase & A) const + { + return derived().jointExtendedModelRows_impl(A.derived()); + } + template typename SizeDepType::template RowsReturn::Type jointRows_impl(Eigen::MatrixBase & A) const @@ -379,6 +517,13 @@ namespace pinocchio return SizeDepType::middleRows(A.derived(), idx_v(), nv()); } + template + typename SizeDepType::template RowsReturn::Type + jointExtendedModelRows_impl(Eigen::MatrixBase & A) const + { + return SizeDepType::middleRows(A.derived(), idx_vExtended(), nvExtended()); + } + /// \brief Returns a block of dimension nv()xnv() located at position idx_v(),idx_v() in the /// matrix Mat // Const access @@ -389,6 +534,13 @@ namespace pinocchio return derived().jointBlock_impl(Mat.derived()); } + template + typename SizeDepType::template BlockReturn::ConstType + jointExtendedModelBlock(const Eigen::MatrixBase & Mat) const + { + return derived().jointExtendedModelBlock_impl(Mat.derived()); + } + template typename SizeDepType::template BlockReturn::ConstType jointBlock_impl(const Eigen::MatrixBase & Mat) const @@ -396,6 +548,14 @@ namespace pinocchio return SizeDepType::block(Mat.derived(), idx_v(), idx_v(), nv(), nv()); } + template + typename SizeDepType::template BlockReturn::ConstType + jointExtendedModelBlock_impl(const Eigen::MatrixBase & Mat) const + { + return SizeDepType::block( + Mat.derived(), idx_vExtended(), idx_vExtended(), nvExtended(), nvExtended()); + } + // Non-const access template typename SizeDepType::template BlockReturn::Type @@ -404,6 +564,13 @@ namespace pinocchio return derived().jointBlock_impl(Mat.derived()); } + template + typename SizeDepType::template BlockReturn::Type + jointExtendedModelBlock(Eigen::MatrixBase & Mat) const + { + return derived().jointExtendedModelBlock_impl(Mat.derived()); + } + template typename SizeDepType::template BlockReturn::Type jointBlock_impl(Eigen::MatrixBase & Mat) const @@ -411,6 +578,14 @@ namespace pinocchio return SizeDepType::block(Mat.derived(), idx_v(), idx_v(), nv(), nv()); } + template + typename SizeDepType::template BlockReturn::Type + jointExtendedModelBlock_impl(Eigen::MatrixBase & Mat) const + { + return SizeDepType::block( + Mat.derived(), idx_vExtended(), idx_vExtended(), nvExtended(), nvExtended()); + } + protected: /// Default constructor: protected. /// @@ -419,6 +594,7 @@ namespace pinocchio : i_id(std::numeric_limits::max()) , i_q(-1) , i_v(-1) + , i_vExtended(-1) { } @@ -440,6 +616,7 @@ namespace pinocchio i_id = clone.i_id; i_q = clone.i_q; i_v = clone.i_v; + i_vExtended = clone.i_vExtended; return *this; } @@ -447,6 +624,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_vExtended; // 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..5164f608f2 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, + NVExtended = 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 }; @@ -566,6 +569,7 @@ namespace pinocchio using Base::id; using Base::idx_q; using Base::idx_v; + using Base::idx_vExtended; using Base::setIndexes; JointDataDerived createData() const @@ -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_vExtended()); 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..9296725619 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, + NVExtended = 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 }; @@ -591,6 +594,7 @@ namespace pinocchio using Base::id; using Base::idx_q; using Base::idx_v; + using Base::idx_vExtended; using Base::setIndexes; typedef Eigen::Matrix Vector3; @@ -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_vExtended()); 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..1bca694ce7 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, + NVExtended = 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 }; @@ -673,6 +676,7 @@ namespace pinocchio using Base::id; using Base::idx_q; using Base::idx_v; + using Base::idx_vExtended; using Base::setIndexes; typedef Eigen::Matrix Vector3; @@ -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_vExtended()); 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..fc203cad09 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, + NVExtended = 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 }; @@ -617,6 +620,7 @@ namespace pinocchio using Base::id; using Base::idx_q; using Base::idx_v; + using Base::idx_vExtended; using Base::setIndexes; JointModelRevoluteUnalignedTpl() @@ -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_vExtended()); 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..7cf88098b1 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, + NVExtended = 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 }; @@ -141,6 +144,7 @@ namespace pinocchio using Base::id; using Base::idx_q; using Base::idx_v; + using Base::idx_vExtended; using Base::setIndexes; JointModelRevoluteUnboundedUnalignedTpl() @@ -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_vExtended()); 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..90ae6d7d8a 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, + NVExtended = 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 }; @@ -127,6 +130,7 @@ namespace pinocchio using Base::id; using Base::idx_q; using Base::idx_v; + using Base::idx_vExtended; using Base::setIndexes; typedef Eigen::Matrix Vector3; @@ -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_vExtended()); 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..f7195f9aed 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, + NVExtended = 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 }; @@ -761,6 +764,7 @@ namespace pinocchio using Base::id; using Base::idx_q; using Base::idx_v; + using Base::idx_vExtended; using Base::setIndexes; typedef Eigen::Matrix Vector3; @@ -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_vExtended()); 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..14eed470ec 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, + NVExtended = 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 }; @@ -379,6 +382,7 @@ namespace pinocchio using Base::id; using Base::idx_q; using Base::idx_v; + using Base::idx_vExtended; using Base::setIndexes; JointDataDerived createData() const @@ -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_vExtended()); return res; } diff --git a/include/pinocchio/multibody/joint/joint-spherical.hpp b/include/pinocchio/multibody/joint/joint-spherical.hpp index 800696fa97..44870f8476 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, + NVExtended = 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 }; @@ -480,6 +483,7 @@ namespace pinocchio using Base::id; using Base::idx_q; using Base::idx_v; + using Base::idx_vExtended; using Base::setIndexes; JointDataDerived createData() const @@ -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_vExtended()); return res; } diff --git a/include/pinocchio/multibody/joint/joint-translation.hpp b/include/pinocchio/multibody/joint/joint-translation.hpp index bf2b113f66..5520c5a0ab 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, + NVExtended = 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 @@ -577,6 +580,7 @@ namespace pinocchio using Base::id; using Base::idx_q; using Base::idx_v; + using Base::idx_vExtended; using Base::setIndexes; JointDataDerived createData() const @@ -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_vExtended()); 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..75b7cbb485 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, + NVExtended = 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 }; @@ -413,6 +416,7 @@ namespace pinocchio using Base::id; using Base::idx_q; using Base::idx_v; + using Base::idx_vExtended; using Base::setIndexes; JointModelUniversalTpl() @@ -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_vExtended()); return res; } diff --git a/include/pinocchio/multibody/liegroup/liegroup-algo.hxx b/include/pinocchio/multibody/liegroup/liegroup-algo.hxx index 374d5aaea7..adfa560e89 100644 --- a/include/pinocchio/multibody/liegroup/liegroup-algo.hxx +++ b/include/pinocchio/multibody/liegroup/liegroup-algo.hxx @@ -244,7 +244,8 @@ 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.jointExtendedModelBlock(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType, mat)), arg, + op); } }; @@ -299,8 +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.jointExtendedModelRows(mat_in.derived()), + jmodel.jointExtendedModelRows(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixOutType, mat_out)), + arg); } }; @@ -348,7 +350,7 @@ 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.jointExtendedModelRows(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType, mat)), arg); } }; @@ -392,7 +394,7 @@ 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.jointExtendedModelBlock(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix, mat)), arg); } }; diff --git a/include/pinocchio/multibody/model.hpp b/include/pinocchio/multibody/model.hpp index 37a12fc81e..4de5152785 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 nvExtended; + /// \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_vExtendeds; + + /// \brief Dimension of the *i*th joint jacobian subspace. + std::vector nvExtendeds; + /// \brief Vector of parent joint indexes. The parent of joint *i*, denoted *li*, corresponds to /// li==parents[i]. std::vector parents; @@ -138,6 +147,14 @@ namespace pinocchio /// the set (i==parents[k] for k in mu(i)). std::vector children; + /// \brief Vector of mimicking joints in the tree (with type MimicTpl) + std::vector mimicking_joints; + + /// \brief Vector of mimicked joints in the tree (can be any joint type) + /// The i-th element of this vector correspond to the mimicked joint of the i-th mimicking + /// vector in mimicking_joints + std::vector mimicked_joints; + /// \brief Name of the joints. std::vector names; @@ -200,6 +217,7 @@ namespace pinocchio ModelTpl() : nq(0) , nv(0) + , nvExtended(0) , njoints(1) , nbodies(1) , nframes(0) @@ -210,6 +228,8 @@ namespace pinocchio , nqs(1, 0) , idx_vs(1, 0) , nvs(1, 0) + , idx_vExtendeds(1, 0) + , nvExtendeds(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..d36fe9c93a 100644 --- a/include/pinocchio/multibody/model.hxx +++ b/include/pinocchio/multibody/model.hxx @@ -8,6 +8,7 @@ #include "pinocchio/utils/string-generator.hpp" #include "pinocchio/multibody/liegroup/liegroup-algo.hpp" +#include "pinocchio/algorithm/model.hpp" /// @cond DEV @@ -72,7 +73,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.nvExtended() >= 0)); assert(joint_model.nq() >= joint_model.nv()); PINOCCHIO_CHECK_ARGUMENT_SIZE( @@ -97,15 +98,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, nvExtended); 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_nvExtended = jmodel.nvExtended(); + const int joint_idx_vExtended = jmodel.idx_vExtended(); assert(joint_idx_q >= 0); assert(joint_idx_v >= 0); + assert(joint_idx_vExtended >= 0); inertias.push_back(Inertia::Zero()); parents.push_back(parent); @@ -120,29 +124,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; - } + nvExtended += joint_nvExtended; + nvExtendeds.push_back(joint_nvExtended); + idx_vExtendeds.push_back(joint_idx_vExtended); + + 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; // Init and add joint index to its parent subtrees. subtrees.push_back(IndexVector(1)); @@ -153,6 +157,11 @@ namespace pinocchio supports.push_back(supports[parent]); supports[joint_id].push_back(joint_id); + if (auto jmodel_ = boost::get>(&jmodel)) + { + mimicking_joints.push_back(jmodel.id()); + mimicked_joints.push_back(jmodel_->jmodel().id()); + } return joint_id; } @@ -230,6 +239,7 @@ namespace pinocchio ReturnType res; res.nq = nq; res.nv = nv; + res.nvExtended = nvExtended; res.njoints = njoints; res.nbodies = nbodies; res.nframes = nframes; @@ -238,6 +248,8 @@ namespace pinocchio res.names = names; res.subtrees = subtrees; res.supports = supports; + res.mimicking_joints = mimicking_joints; + res.mimicked_joints = mimicked_joints; res.gravity = gravity.template cast(); res.name = name; @@ -245,7 +257,8 @@ namespace pinocchio res.nqs = nqs; res.idx_vs = idx_vs; res.nvs = nvs; - + res.idx_vExtendeds = idx_vExtendeds; + res.nvExtendeds = nvExtendeds; // Eigen Vectors res.armature = armature.template cast(); res.friction = friction.template cast(); @@ -289,12 +302,15 @@ namespace pinocchio template class JointCollectionTpl> bool ModelTpl::operator==(const ModelTpl & other) const { - bool res = other.nq == nq && other.nv == nv && 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; + bool res = other.nq == nq && other.nv == nv && other.nvExtended == nvExtended + && other.njoints == njoints && other.nbodies == nbodies && other.nframes == nframes + && other.parents == parents && other.children == children && other.names == names + && other.subtrees == subtrees && other.mimicking_joints == mimicking_joints + && other.mimicked_joints == mimicked_joints && other.gravity == gravity + && other.name == name; + + res &= other.idx_qs == idx_qs && other.nqs == nqs && other.idx_vs == idx_vs && other.nvs == nvs + && other.idx_vExtendeds == idx_vExtendeds && other.nvExtendeds == nvExtendeds; 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..91c9223523 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,7 +43,8 @@ namespace pinocchio const typename ModelTpl::JointModel & rootJoint, const std::string & rootJointName, ModelTpl & model, - const bool verbose = false); + const bool verbose = false, + const bool mimic = false); /// /// \brief Build the model from a URDF file with a particular joint as root of the model tree @@ -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 verbose = false); + const bool verbose = false, + const bool mimic = 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,7 +78,8 @@ namespace pinocchio ModelTpl & buildModel( const std::string & filename, ModelTpl & model, - const bool verbose = false); + const bool verbose = false, + const bool mimic = false); /// /// \brief Build the model from a URDF model with a particular joint as root of the model tree @@ -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,13 +99,15 @@ namespace pinocchio const std::shared_ptr<::urdf::ModelInterface> urdfTree, const typename ModelTpl::JointModel & rootJoint, ModelTpl & model, - const bool verbose = false); + const bool verbose = false, + const bool mimic = false); /// /// \brief Build the model from a URDF model with a particular joint as root of the model tree /// 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 verbose = false); + const bool verbose = false, + const bool mimic = 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,7 +140,8 @@ namespace pinocchio ModelTpl & buildModel( const std::shared_ptr<::urdf::ModelInterface> urdfTree, ModelTpl & model, - const bool verbose = false); + const bool verbose = false, + const bool mimic = false); /// /// \brief Build the model from an XML stream with a particular joint as root of the model tree @@ -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,7 +163,8 @@ namespace pinocchio const typename ModelTpl::JointModel & rootJoint, const std::string & rootJointName, ModelTpl & model, - const bool verbose = false); + const bool verbose = false, + const bool mimic = false); /// /// \brief Build the model from an XML stream with a particular joint as root of the model tree @@ -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 verbose = false); + const bool verbose = false, + const bool mimic = 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,7 +202,8 @@ namespace pinocchio ModelTpl & buildModelFromXML( const std::string & xml_stream, ModelTpl & model, - const bool verbose = false); + const bool verbose = false, + const bool mimic = false); /** * @brief Build The GeometryModel from a URDF file. Search for meshes diff --git a/include/pinocchio/parsers/urdf/model.hxx b/include/pinocchio/parsers/urdf/model.hxx index 824a33eaf5..b68667eb20 100644 --- a/include/pinocchio/parsers/urdf/model.hxx +++ b/include/pinocchio/parsers/urdf/model.hxx @@ -15,6 +15,8 @@ #include #include #include +#include +#include namespace pinocchio { @@ -24,6 +26,9 @@ namespace pinocchio { typedef double urdf_scalar_type; + template + struct MimicInfo; + template class UrdfVisitorBaseTpl { @@ -35,7 +40,8 @@ namespace pinocchio PRISMATIC, FLOATING, PLANAR, - SPHERICAL + SPHERICAL, + MIMIC }; typedef enum ::pinocchio::FrameType FrameType; @@ -47,6 +53,7 @@ namespace pinocchio typedef Eigen::Matrix Vector; typedef Eigen::Ref VectorRef; typedef Eigen::Ref VectorConstRef; + typedef MimicInfo MimicInfo_; virtual void setName(const std::string & name) = 0; @@ -65,7 +72,8 @@ namespace pinocchio const VectorConstRef & min_config, const VectorConstRef & max_config, const VectorConstRef & friction, - const VectorConstRef & damping) = 0; + const VectorConstRef & damping, + const boost::optional & mimic_info = boost::none) = 0; virtual void addJointAndBody( JointType type, @@ -81,7 +89,8 @@ namespace pinocchio const VectorConstRef & min_config, const VectorConstRef & max_config, const VectorConstRef & friction, - const VectorConstRef & damping) = 0; + const VectorConstRef & damping, + const boost::optional & mimic_info = boost::none) = 0; virtual void addFixedJointAndBody( const FrameIndex & parentFrameId, @@ -124,6 +133,38 @@ namespace pinocchio std::ostream * log; }; + template + struct MimicInfo + { + typedef _Scalar Scalar; + typedef Eigen::Matrix Vector3; + + std::string mimicked_name; + Scalar multiplier; + Scalar offset; + Vector3 axis; + + // Use the JointType from UrdfVisitorBaseTpl + typedef typename UrdfVisitorBaseTpl<_Scalar, Options>::JointType JointType; + JointType jointType; + + MimicInfo() = default; + + MimicInfo( + std::string _mimicked_name, + Scalar _multiplier, + Scalar _offset, + Vector3 _axis, + JointType _jointType) + : mimicked_name(_mimicked_name) + , multiplier(_multiplier) + , offset(_offset) + , axis(_axis) + , jointType(_jointType) + { + } + }; + template class JointCollectionTpl> class UrdfVisitor : public UrdfVisitorBaseTpl { @@ -140,6 +181,8 @@ namespace pinocchio typedef typename Model::JointModel JointModel; typedef typename Model::Frame Frame; + typedef MimicInfo MimicInfo_; + Model & model; std::string root_joint_name; @@ -180,11 +223,12 @@ namespace pinocchio const VectorConstRef & min_config, const VectorConstRef & max_config, const VectorConstRef & friction, - const VectorConstRef & damping) + const VectorConstRef & damping, + const boost::optional & mimic_info = boost::none) { addJointAndBody( type, axis, parentFrameId, placement, joint_name, Y, SE3::Identity(), body_name, - max_effort, max_velocity, min_config, max_config, friction, damping); + max_effort, max_velocity, min_config, max_config, friction, damping, mimic_info); } void addJointAndBody( @@ -201,11 +245,11 @@ namespace pinocchio const VectorConstRef & min_config, const VectorConstRef & max_config, const VectorConstRef & friction, - const VectorConstRef & damping) + const VectorConstRef & damping, + const boost::optional & mimic_info = boost::none) { JointIndex joint_id; const Frame & frame = model.frames[parentFrameId]; - switch (type) { case Base::FLOATING: @@ -250,6 +294,35 @@ namespace pinocchio frame.placement * placement, joint_name, max_effort, max_velocity, min_config, max_config, friction, damping); break; + case Base::MIMIC: + if (mimic_info) + switch (mimic_info->jointType) + { + case Base::REVOLUTE: + joint_id = addMimicJoint< + typename JointCollection::JointModelRX, typename JointCollection::JointModelRY, + typename JointCollection::JointModelRZ, + typename JointCollection::JointModelRevoluteUnaligned>( + frame, placement, joint_name, max_effort, max_velocity, min_config, max_config, + friction, damping, *mimic_info); + break; + case Base::PRISMATIC: + joint_id = addMimicJoint< + typename JointCollection::JointModelPX, typename JointCollection::JointModelPY, + typename JointCollection::JointModelPZ, + typename JointCollection::JointModelPrismaticUnaligned>( + frame, placement, joint_name, max_effort, max_velocity, min_config, max_config, + friction, damping, *mimic_info); + break; + default: + PINOCCHIO_CHECK_INPUT_ARGUMENT( + false, "Cannot mimic this type. Only revolute, prismatic and helicoidal can be " + "mimicked"); + break; + } + else + PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "Cannot create mimic joint. Missing info.") + break; default: PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The joint type is not correct."); }; @@ -406,6 +479,69 @@ namespace pinocchio } } + template + JointIndex addMimicJoint( + const Frame & frame, + const SE3 & placement, + const std::string & joint_name, + const VectorConstRef & max_effort, + const VectorConstRef & max_velocity, + const VectorConstRef & min_config, + const VectorConstRef & max_config, + const VectorConstRef & friction, + const VectorConstRef & damping, + const MimicInfo_ & mimic_info) + { + if (!model.existJointName(mimic_info.mimicked_name)) + PINOCCHIO_CHECK_INPUT_ARGUMENT( + false, "The parent joint of the mimic joint is not in the kinematic tree"); + + auto mimicked_joint = model.joints[getJointId(mimic_info.mimicked_name)]; + + CartesianAxis axisType = extractCartesianAxis(mimic_info.axis); + switch (axisType) + { + case AXIS_X: + return model.addJoint( + frame.parentJoint, + typename JointCollection::JointModelMimic( + TypeX(), mimicked_joint, mimic_info.multiplier, mimic_info.offset), + frame.placement * placement, joint_name, max_effort, max_velocity, min_config, + max_config, friction, damping); + break; + case AXIS_Y: + return model.addJoint( + frame.parentJoint, + typename JointCollection::JointModelMimic( + TypeY(), mimicked_joint, mimic_info.multiplier, mimic_info.offset), + frame.placement * placement, joint_name, max_effort, max_velocity, min_config, + max_config, friction, damping); + break; + + case AXIS_Z: + return model.addJoint( + frame.parentJoint, + typename JointCollection::JointModelMimic( + TypeZ(), mimicked_joint, mimic_info.multiplier, mimic_info.offset), + frame.placement * placement, joint_name, max_effort, max_velocity, min_config, + max_config, friction, damping); + break; + + case AXIS_UNALIGNED: + return model.addJoint( + frame.parentJoint, + typename JointCollection::JointModelMimic( + TypeUnaligned(), mimicked_joint, mimic_info.multiplier, mimic_info.offset), + frame.placement * placement, joint_name, max_effort, max_velocity, min_config, + max_config, friction, damping); + 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. @@ -481,15 +617,16 @@ namespace pinocchio }; typedef UrdfVisitorBaseTpl UrdfVisitorBase; + typedef MimicInfo MimicInfo_; - 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 = false); - PINOCCHIO_PARSERS_DLLAPI void - parseRootTree(const std::string & filename, UrdfVisitorBase & model); + PINOCCHIO_PARSERS_DLLAPI void parseRootTree( + const std::string & filename, UrdfVisitorBase & model, const bool mimic = false); - 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 = false); } // namespace details template class JointCollectionTpl> @@ -498,7 +635,8 @@ namespace pinocchio const typename ModelTpl::JointModel & rootJoint, const std::string & rootJointName, ModelTpl & model, - const bool verbose) + const bool verbose, + const bool mimic) { if (rootJointName.empty()) throw std::invalid_argument( @@ -508,7 +646,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 +655,23 @@ namespace pinocchio const std::string & filename, const typename ModelTpl::JointModel & rootJoint, ModelTpl & model, - const bool verbose) + const bool verbose, + const bool mimic) { - return buildModel(filename, rootJoint, "root_joint", model, verbose); + return buildModel(filename, rootJoint, "root_joint", model, verbose, mimic); } template class JointCollectionTpl> ModelTpl & buildModel( const std::string & filename, ModelTpl & model, - const bool verbose) + const bool verbose, + const bool mimic) { details::UrdfVisitor visitor(model); if (verbose) visitor.log = &std::cout; - details::parseRootTree(filename, visitor); + details::parseRootTree(filename, visitor, mimic); return model; } @@ -541,7 +681,8 @@ namespace pinocchio const typename ModelTpl::JointModel & rootJoint, const std::string & rootJointName, ModelTpl & model, - const bool verbose) + const bool verbose, + const bool mimic) { if (rootJointName.empty()) throw std::invalid_argument( @@ -551,7 +692,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 +701,23 @@ namespace pinocchio const std::string & xmlStream, const typename ModelTpl::JointModel & rootJoint, ModelTpl & model, - const bool verbose) + const bool verbose, + const bool mimic) { - return buildModelFromXML(xmlStream, rootJoint, "root_joint", model, verbose); + return buildModelFromXML(xmlStream, rootJoint, "root_joint", model, verbose, mimic); } template class JointCollectionTpl> ModelTpl & buildModelFromXML( const std::string & xmlStream, ModelTpl & model, - const bool verbose) + const bool verbose, + const bool mimic) { details::UrdfVisitor visitor(model); if (verbose) visitor.log = &std::cout; - details::parseRootTreeFromXML(xmlStream, visitor); + details::parseRootTreeFromXML(xmlStream, visitor, mimic); return model; } @@ -584,7 +727,8 @@ namespace pinocchio const typename ModelTpl::JointModel & rootJoint, const std::string & rootJointName, ModelTpl & model, - const bool verbose) + const bool verbose, + const bool mimic) { if (rootJointName.empty()) throw std::invalid_argument( @@ -595,7 +739,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 +748,24 @@ namespace pinocchio const std::shared_ptr<::urdf::ModelInterface> urdfTree, const typename ModelTpl::JointModel & rootJoint, ModelTpl & model, - const bool verbose) + const bool verbose, + const bool mimic) { - return buildModel(urdfTree, rootJoint, "root_joint", model, verbose); + return buildModel(urdfTree, rootJoint, "root_joint", model, verbose, mimic); } template class JointCollectionTpl> ModelTpl & buildModel( const std::shared_ptr<::urdf::ModelInterface> urdfTree, ModelTpl & model, - const bool verbose) + const bool verbose, + const bool mimic) { 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/data.hpp b/include/pinocchio/serialization/data.hpp index daf0afb1f0..bbf12e72dc 100644 --- a/include/pinocchio/serialization/data.hpp +++ b/include/pinocchio/serialization/data.hpp @@ -89,6 +89,9 @@ namespace boost PINOCCHIO_MAKE_DATA_NVP(ar, data, D); PINOCCHIO_MAKE_DATA_NVP(ar, data, Dinv); PINOCCHIO_MAKE_DATA_NVP(ar, data, parents_fromRow); + PINOCCHIO_MAKE_DATA_NVP(ar, data, mimic_parents_fromRow); + PINOCCHIO_MAKE_DATA_NVP(ar, data, non_mimic_parents_fromRow); + PINOCCHIO_MAKE_DATA_NVP(ar, data, idx_v_extended_fromRow); PINOCCHIO_MAKE_DATA_NVP(ar, data, supports_fromRow); PINOCCHIO_MAKE_DATA_NVP(ar, data, nvSubtree_fromRow); PINOCCHIO_MAKE_DATA_NVP(ar, data, J); diff --git a/include/pinocchio/serialization/joints-data.hpp b/include/pinocchio/serialization/joints-data.hpp index 14f1e510a5..157d3ba7d7 100644 --- a/include/pinocchio/serialization/joints-data.hpp +++ b/include/pinocchio/serialization/joints-data.hpp @@ -61,6 +61,24 @@ 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()); + } + } // namespace fix template @@ -215,7 +233,6 @@ namespace boost { typedef pinocchio::JointDataCompositeTpl JointType; fix::serialize(ar, static_cast &>(joint), version); - ::pinocchio::Serialize::run(ar, joint); } @@ -233,15 +250,23 @@ 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::JointDataMimic & joint, const unsigned int version) + Archive & ar, + pinocchio::JointDataMimicTpl & joint, + const unsigned int version) { - typedef pinocchio::JointDataMimic JointType; - fix::serialize(ar, static_cast &>(joint), version); - ar & make_nvp("jdata", joint.jdata()); - ar & make_nvp("scaling", joint.scaling()); + + ar & make_nvp("m_q_transform", joint.q_transformed()); + ar & make_nvp("m_v_transform", joint.v_transformed()); + + typedef pinocchio::JointDataMimicTpl JointType; + fix::serialize(ar, static_cast &>(joint), version); } } // namespace serialization diff --git a/include/pinocchio/serialization/joints-model.hpp b/include/pinocchio/serialization/joints-model.hpp index 81d4167fd1..8a9cdda041 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_nvExtended", joint.m_nvExtended); 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_vExtended", joint.m_idx_vExtended); + ar & make_nvp("m_nvExtendeds", joint.m_nvExtendeds); 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_vExtended = joint.idx_vExtended(); 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_vExtended", i_vExtended); } + 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_vExtended) + { + joint.setIndexes(i_id, i_q, i_v, i_vExtended); + } + }; + + 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_vExtended) + { + joint_m.setMimicIndexes(joint_m.jmodel().id(), i_q, i_v, joint_m.jmodel().idx_vExtended()); + joint_m.setIndexes(i_id, 0, 0, i_vExtended); + } + }; + 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_vExtended; 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_vExtended", i_vExtended); - joint.setIndexes(i_id, i_q, i_v); + SetJointIndexes(joint.derived()).run(i_id, i_q, i_v, i_vExtended); } template @@ -82,8 +119,6 @@ namespace boost const unsigned int version) { typedef pinocchio::JointModelRevoluteTpl JointType; - // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase - // >(joint)); fix::serialize(ar, *static_cast *>(&joint), version); } @@ -94,8 +129,6 @@ namespace boost const unsigned int version) { typedef pinocchio::JointModelRevoluteUnboundedTpl JointType; - // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase - // >(joint)); fix::serialize(ar, *static_cast *>(&joint), version); } @@ -106,8 +139,6 @@ namespace boost const unsigned int version) { typedef pinocchio::JointModelPrismaticTpl JointType; - // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase - // >(joint)); fix::serialize(ar, *static_cast *>(&joint), version); } @@ -141,8 +172,6 @@ namespace boost const unsigned int version) { typedef pinocchio::JointModelFreeFlyerTpl JointType; - // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase - // >(joint)); fix::serialize(ar, *static_cast *>(&joint), version); } @@ -153,8 +182,6 @@ namespace boost const unsigned int version) { typedef pinocchio::JointModelPlanarTpl JointType; - // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase - // >(joint)); fix::serialize(ar, *static_cast *>(&joint), version); } @@ -165,8 +192,6 @@ namespace boost const unsigned int version) { typedef pinocchio::JointModelSphericalTpl JointType; - // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase - // >(joint)); fix::serialize(ar, *static_cast *>(&joint), version); } @@ -177,8 +202,6 @@ namespace boost const unsigned int version) { typedef pinocchio::JointModelSphericalZYXTpl JointType; - // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase - // >(joint)); fix::serialize(ar, *static_cast *>(&joint), version); } @@ -189,8 +212,6 @@ namespace boost const unsigned int version) { typedef pinocchio::JointModelTranslationTpl JointType; - // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase - // >(joint)); fix::serialize(ar, *static_cast *>(&joint), version); } @@ -201,8 +222,6 @@ namespace boost const unsigned int version) { typedef pinocchio::JointModelRevoluteUnalignedTpl JointType; - // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase - // >(joint)); fix::serialize(ar, *static_cast *>(&joint), version); ar & make_nvp("axis", joint.axis); } @@ -214,8 +233,6 @@ namespace boost const unsigned int version) { typedef pinocchio::JointModelRevoluteUnboundedUnalignedTpl JointType; - // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase - // >(joint)); fix::serialize(ar, *static_cast *>(&joint), version); ar & make_nvp("axis", joint.axis); } @@ -227,8 +244,6 @@ namespace boost const unsigned int version) { typedef pinocchio::JointModelPrismaticUnalignedTpl JointType; - // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase - // >(joint)); fix::serialize(ar, *static_cast *>(&joint), version); ar & make_nvp("axis", joint.axis); } @@ -240,8 +255,6 @@ namespace boost const unsigned int version) { typedef pinocchio::JointModelUniversalTpl JointType; - // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase - // >(joint)); fix::serialize(ar, *static_cast *>(&joint), version); ar & make_nvp("axis1", joint.axis1); ar & make_nvp("axis2", joint.axis2); @@ -258,8 +271,6 @@ namespace boost const unsigned int version) { typedef pinocchio::JointModelCompositeTpl JointType; - // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase - // >(joint)); fix::serialize(ar, *static_cast *>(&joint), version); ::pinocchio::Serialize::run(ar, joint); @@ -276,26 +287,28 @@ namespace boost const unsigned int version) { typedef pinocchio::JointModelTpl JointType; - // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase - // >(joint)); fix::serialize(ar, *static_cast *>(&joint), version); typedef typename JointCollectionTpl::JointModelVariant JointModelVariant; 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; - // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase - // >(joint)); - fix::serialize(ar, *static_cast *>(&joint), version); - + typedef pinocchio::JointModelMimicTpl JointType; ar & make_nvp("jmodel", joint.jmodel()); ar & make_nvp("scaling", joint.scaling()); ar & make_nvp("offset", joint.offset()); + + fix::serialize(ar, *static_cast *>(&joint), version); } } // namespace serialization 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..96c0420ce8 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("nvExtended", model.nvExtended); + ar & make_nvp("nvExtendeds", model.nvExtendeds); + ar & make_nvp("idx_vExtendeds", model.idx_vExtendeds); ar & make_nvp("njoints", model.njoints); ar & make_nvp("nbodies", model.nbodies); ar & make_nvp("nframes", model.nframes); @@ -45,6 +48,8 @@ namespace boost ar & make_nvp("names", model.names); ar & make_nvp("supports", model.supports); ar & make_nvp("subtrees", model.subtrees); + ar & make_nvp("mimicking_joints", model.mimicking_joints); + ar & make_nvp("mimicked_joints", model.mimicked_joints); ar & make_nvp("gravity", model.gravity); ar & make_nvp("name", model.name); diff --git a/sources.cmake b/sources.cmake index f8cd400628..17e0cedac0 100644 --- a/sources.cmake +++ b/sources.cmake @@ -18,7 +18,11 @@ set(${PROJECT_NAME}_CORE_PUBLIC_HEADERS ${PROJECT_SOURCE_DIR}/include/pinocchio/algorithm/centroidal.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/algorithm/centroidal.hxx ${PROJECT_SOURCE_DIR}/include/pinocchio/algorithm/check.hpp - ${PROJECT_SOURCE_DIR}/include/pinocchio/algorithm/check.hxx + ${PROJECT_SOURCE_DIR}/include/pinocchio/algorithm/check-base.hpp + ${PROJECT_SOURCE_DIR}/include/pinocchio/algorithm/check-data.hpp + ${PROJECT_SOURCE_DIR}/include/pinocchio/algorithm/check-data.hxx + ${PROJECT_SOURCE_DIR}/include/pinocchio/algorithm/check-model.hpp + ${PROJECT_SOURCE_DIR}/include/pinocchio/algorithm/check-model.hxx ${PROJECT_SOURCE_DIR}/include/pinocchio/algorithm/cholesky.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/algorithm/cholesky.hxx ${PROJECT_SOURCE_DIR}/include/pinocchio/algorithm/compute-all-terms.hpp diff --git a/src/algorithm/geometry.cpp b/src/algorithm/geometry.cpp index b37548592b..934537902c 100644 --- a/src/algorithm/geometry.cpp +++ b/src/algorithm/geometry.cpp @@ -4,7 +4,7 @@ #include "pinocchio/spatial/fwd.hpp" -#ifndef PINOCCHIO_SKIP_CASADI_UNSUPPORTED +#ifndef PINOCCHIO_SKIP_ALGORITHM_GEOMETRY #include "pinocchio/algorithm/geometry.hpp" @@ -28,4 +28,4 @@ namespace pinocchio } // namespace pinocchio -#endif // PINOCCHIO_SKIP_CASADI_UNSUPPORTED +#endif // PINOCCHIO_SKIP_ALGORITHM_GEOMETRY 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..44608f0446 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,29 @@ namespace pinocchio friction << joint->dynamics->friction; damping << joint->dynamics->damping; } - - model.addJointAndBody( - UrdfVisitorBase::REVOLUTE, axis, parentFrameId, jointPlacement, joint->name, Y, - link->name, max_effort, max_velocity, min_config, max_config, friction, 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); + + friction = Vector::Constant(0, 0.); + damping = Vector::Constant(0, 0.); + + MimicInfo_ mimic_info( + joint->mimic->joint_name, joint->mimic->multiplier, joint->mimic->offset, axis, + UrdfVisitorBase::REVOLUTE); + + model.addJointAndBody( + UrdfVisitorBase::MIMIC, axis, parentFrameId, jointPlacement, joint->name, Y, + link->name, max_effort, max_velocity, min_config, max_config, friction, damping, + mimic_info); + } + 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 +210,29 @@ namespace pinocchio friction << joint->dynamics->friction; damping << joint->dynamics->damping; } - - model.addJointAndBody( - UrdfVisitorBase::PRISMATIC, axis, parentFrameId, jointPlacement, joint->name, Y, - link->name, max_effort, max_velocity, min_config, max_config, friction, 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); + + friction = Vector::Constant(0, 0.); + damping = Vector::Constant(0, 0.); + + MimicInfo_ mimic_info( + joint->mimic->joint_name, joint->mimic->multiplier, joint->mimic->offset, axis, + UrdfVisitorBase::PRISMATIC); + + model.addJointAndBody( + UrdfVisitorBase::MIMIC, axis, parentFrameId, jointPlacement, joint->name, Y, + link->name, max_effort, max_velocity, min_config, max_config, friction, damping, + mimic_info); + } + 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 +289,7 @@ namespace pinocchio BOOST_FOREACH (::urdf::LinkConstSharedPtr child, link->child_links) { - parseTree(child, model); + parseTree(child, model, mimic); } } @@ -263,7 +301,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 +311,15 @@ namespace pinocchio BOOST_FOREACH (::urdf::LinkConstSharedPtr child, root_link->child_links) { - parseTree(child, model); + parseTree(child, model, mimic); } } - 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 +327,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/algo-check.cpp b/unittest/algo-check.cpp index e85cdfed94..0114f008d6 100644 --- a/unittest/algo-check.cpp +++ b/unittest/algo-check.cpp @@ -51,6 +51,12 @@ BOOST_AUTO_TEST_CASE(test_check) Y.inertia().data().fill(-1.); } BOOST_CHECK(!model.check(ABAChecker())); // some inertias are negative ... check fail. + + pinocchio::Model model_mimic; + buildModels::humanoidRandom(model_mimic, false, true); + pinocchio::Data data_mimic(model_mimic); + BOOST_CHECK(!model_mimic.check(MimicChecker())); + BOOST_CHECK(model_mimic.check(data_mimic)); } BOOST_AUTO_TEST_SUITE_END() diff --git a/unittest/all-joints.cpp b/unittest/all-joints.cpp index 072188ed94..4c3b4683c7 100644 --- a/unittest/all-joints.cpp +++ b/unittest/all-joints.cpp @@ -107,18 +107,19 @@ 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(); - 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(1, 0, 0, 0); return jmodel; } }; diff --git a/unittest/casadi/algorithms.cpp b/unittest/casadi/algorithms.cpp index 1cc9d5efb2..8f71e0e864 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.nvExtended), jacobian_world(6, model.nvExtended); jacobian_local.setZero(); jacobian_world.setZero(); diff --git a/unittest/casadi/joints.cpp b/unittest/casadi/joints.cpp index 81c904d1ad..86b33c6a61 100644 --- a/unittest/casadi/joints.cpp +++ b/unittest/casadi/joints.cpp @@ -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(1, 0, 0, 0); return jmodel; } @@ -290,7 +291,6 @@ struct TestADOnJoints void operator()(const pinocchio::JointModelBase &) const { JointModel_ jmodel = init::run(); - jmodel.setIndexes(0, 0, 0); test(jmodel); } @@ -384,8 +384,9 @@ struct TestADOnJoints } // 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..89bc337e90 100644 --- a/unittest/centroidal.cpp +++ b/unittest/centroidal.cpp @@ -18,6 +18,8 @@ #include #include +#include "utils/model-generator.hpp" + template static void addJointAndBody( pinocchio::Model & model, @@ -90,6 +92,116 @@ 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 std::vector & primary_ids, + const std::vector & secondary_ids) +{ + const double ratio = 2.5; + const double offset = 0.75; + pinocchio::Model model_mimic; + Eigen::MatrixXd G; + buildMimicModel(model_full, primary_ids, secondary_ids, ratio, offset, model_mimic, G); + + pinocchio::Data data_mimic(model_mimic); + pinocchio::Data data_ref_mimic(model_mimic); + pinocchio::Data data_full(model_full); + + 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 * a; + + toFull(model_full, model_mimic, secondary_ids, ratio, offset, q, q_full); + + 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); + + // Direct parent/Child + std::vector primaries = {humanoid_model.getJointId("rleg1_joint")}; + + std::vector secondaries = {humanoid_model.getJointId("rleg2_joint")}; + + test_mimic_against_full_model(humanoid_model, primaries, secondaries); + + // Spaced Parent/Child + primaries.clear(); + secondaries.clear(); + + primaries = {humanoid_model.getJointId("rleg1_joint")}; + + secondaries = {humanoid_model.getJointId("rleg4_joint")}; + + test_mimic_against_full_model(humanoid_model, primaries, secondaries); + + // Parallel + primaries.clear(); + secondaries.clear(); + + primaries = {humanoid_model.getJointId("lleg1_joint")}; + + secondaries = {humanoid_model.getJointId("rleg1_joint")}; + + test_mimic_against_full_model(humanoid_model, primaries, secondaries); + + // Double mimic, not same primary + primaries.clear(); + secondaries.clear(); + primaries = {humanoid_model.getJointId("lleg1_joint"), humanoid_model.getJointId("rarm1_joint")}; + + secondaries = { + humanoid_model.getJointId("rleg1_joint"), humanoid_model.getJointId("larm1_joint")}; + test_mimic_against_full_model(humanoid_model, primaries, secondaries); + + // Double Mimic, Same Primary + primaries.clear(); + secondaries.clear(); + primaries = {humanoid_model.getJointId("lleg1_joint"), humanoid_model.getJointId("lleg1_joint")}; + + secondaries = { + humanoid_model.getJointId("rleg1_joint"), humanoid_model.getJointId("lleg2_joint")}; + test_mimic_against_full_model(humanoid_model, primaries, secondaries); +} + BOOST_AUTO_TEST_CASE(test_centroidal_mapping) { pinocchio::Model model; diff --git a/unittest/cppad/joints.cpp b/unittest/cppad/joints.cpp index 023067f249..28f53678db 100644 --- a/unittest/cppad/joints.cpp +++ b/unittest/cppad/joints.cpp @@ -190,8 +190,9 @@ struct TestADOnJoints } // TODO: implement it - template - void operator()(const pinocchio::JointModelMimic & /*jmodel*/) const + template class JointCollection> + void operator()( + const pinocchio::JointModelMimicTpl & /*jmodel*/) const { /* do nothing */ } diff --git a/unittest/crba.cpp b/unittest/crba.cpp index 19da9b9e35..ef8b6b0293 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" @@ -30,6 +31,8 @@ #include #include +#include "utils/model-generator.hpp" + template static void addJointAndBody( pinocchio::Model & model, @@ -125,6 +128,105 @@ BOOST_AUTO_TEST_CASE(test_crba) #endif // ifndef NDEBUG } +void test_mimic_against_full_model( + const pinocchio::Model & model_full, + const std::vector & primary_ids, + const std::vector & secondary_ids) +{ + const double ratio = 2.5; + const double offset = 0.75; + pinocchio::Model model_mimic; + Eigen::MatrixXd G; + buildMimicModel(model_full, primary_ids, secondary_ids, ratio, offset, model_mimic, G); + + Eigen::VectorXd q_mimic = pinocchio::randomConfiguration(model_mimic); + Eigen::VectorXd v_mimic = Eigen::VectorXd::Random(model_mimic.nv); + Eigen::VectorXd as_mimic = Eigen::VectorXd::Random(model_mimic.nv); + + Eigen::VectorXd q_full = Eigen::VectorXd::Zero(model_full.nq); + Eigen::VectorXd v_full = G * v_mimic; + Eigen::VectorXd a_full = G * as_mimic; + + toFull(model_full, model_mimic, secondary_ids, ratio, offset, q_mimic, q_full); + + pinocchio::Data data_full(model_full); + pinocchio::Data data_mimic(model_mimic); + pinocchio::Data data_ref_mimic(model_mimic); + + pinocchio::crba(model_full, data_full, q_full); + // 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)); + + // 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); + + // Direct parent/Child + std::vector primaries = {humanoid_model.getJointId("rleg1_joint")}; + std::vector secondaries = {humanoid_model.getJointId("rleg2_joint")}; + test_mimic_against_full_model(humanoid_model, primaries, secondaries); + + // Spaced Parent/Child + primaries.clear(); + secondaries.clear(); + + primaries = {humanoid_model.getJointId("rleg1_joint")}; + + secondaries = {humanoid_model.getJointId("rleg4_joint")}; + + test_mimic_against_full_model(humanoid_model, primaries, secondaries); + + // // Parallel + primaries.clear(); + secondaries.clear(); + + primaries = {humanoid_model.getJointId("lleg1_joint")}; + + secondaries = {humanoid_model.getJointId("rleg1_joint")}; + + test_mimic_against_full_model(humanoid_model, primaries, secondaries); + + // // Double mimic, not same primary + primaries.clear(); + secondaries.clear(); + primaries = {humanoid_model.getJointId("lleg1_joint"), humanoid_model.getJointId("rarm1_joint")}; + + secondaries = { + humanoid_model.getJointId("rleg1_joint"), humanoid_model.getJointId("larm1_joint")}; + test_mimic_against_full_model(humanoid_model, primaries, secondaries); + + // // Double Mimic, Same Primary + primaries.clear(); + secondaries.clear(); + primaries = {humanoid_model.getJointId("lleg1_joint"), humanoid_model.getJointId("lleg1_joint")}; + + secondaries = { + humanoid_model.getJointId("rleg1_joint"), humanoid_model.getJointId("lleg2_joint")}; + test_mimic_against_full_model(humanoid_model, primaries, secondaries); + + // test mimic terminal + primaries.clear(); + secondaries.clear(); + primaries = {humanoid_model.getJointId("larm5_joint")}; + + secondaries = {humanoid_model.getJointId("larm6_joint")}; + test_mimic_against_full_model(humanoid_model, primaries, secondaries); +} + BOOST_AUTO_TEST_CASE(test_minimal_crba) { pinocchio::Model model; @@ -194,7 +296,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()), @@ -203,7 +305,8 @@ BOOST_AUTO_TEST_CASE(test_crba_malloc) const Eigen::VectorXd q = pinocchio::neutral(model); Eigen::internal::set_is_malloc_allowed(false); - crba(model, data, q); + crba(model, data, q, pinocchio::Convention::WORLD); + crba(model, data, q, pinocchio::Convention::LOCAL); Eigen::internal::set_is_malloc_allowed(true); } diff --git a/unittest/data.cpp b/unittest/data.cpp index e20d74dc05..1e336018e9 100644 --- a/unittest/data.cpp +++ b/unittest/data.cpp @@ -11,6 +11,8 @@ #include #include +#include "utils/model-generator.hpp" + using namespace pinocchio; BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) @@ -107,4 +109,36 @@ BOOST_AUTO_TEST_CASE(test_std_vector_of_Data) datas.push_back(Data(model)); } +BOOST_AUTO_TEST_CASE(test_mimic_subtree) +{ + Model model; + buildModels::manipulator(model); + // Direct parent/Child + std::vector primaries = {model.getJointId("shoulder1_joint")}; + std::vector secondaries = {model.getJointId("shoulder2_joint")}; + + const double ratio = 2.5; + const double offset = 0.75; + pinocchio::Model model_mimic; + Eigen::MatrixXd G; + buildMimicModel(model, primaries, secondaries, ratio, offset, model_mimic, G); + + Data data_mimic(model_mimic); + Data data(model); + + // No mimic so should not be filled + BOOST_CHECK(data.mimic_subtree_joint.size() == 0); + + // it's a linear model with RX and RY so the joint after shoulder2 is not a mimic and is in its + // subtree + BOOST_CHECK(data_mimic.mimic_subtree_joint[0] == model_mimic.getJointId("shoulder3_joint")); + + // Test when mimic is terminal + Model man_mimic; + buildModels::manipulator(man_mimic, true); + + Data data_man_mimic(man_mimic); + BOOST_CHECK(data_man_mimic.mimic_subtree_joint[0] == 0); +} + BOOST_AUTO_TEST_SUITE_END() diff --git a/unittest/finite-differences.cpp b/unittest/finite-differences.cpp index 95ade70874..9b3958ef41 100644 --- a/unittest/finite-differences.cpp +++ b/unittest/finite-differences.cpp @@ -195,17 +195,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); + 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..d48eb1733c 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_vExtended()); typedef typename JointModel::ConfigVector_t ConfigVector_t; typedef typename JointModel::TangentVector_t TangentVector_t; @@ -142,6 +142,12 @@ void test_joint_methods( Model::ConfigVectorType vec(Model::ConfigVectorType::Ones(m)); const Model::ConfigVectorType vec_const(Model::ConfigVectorType::Ones(m)); + BOOST_CHECK( + jmodel.JointMappedConfigSelector(vec) == jmodel_composite.JointMappedConfigSelector(vec)); + BOOST_CHECK( + jmodel.JointMappedConfigSelector(vec_const) + == jmodel_composite.JointMappedConfigSelector(vec_const)); + BOOST_CHECK(jmodel.jointConfigSelector(vec) == jmodel_composite.jointConfigSelector(vec)); BOOST_CHECK( jmodel.jointConfigSelector(vec_const) == jmodel_composite.jointConfigSelector(vec_const)); @@ -150,8 +156,17 @@ void test_joint_methods( BOOST_CHECK( jmodel.jointVelocitySelector(vec_const) == jmodel_composite.jointVelocitySelector(vec_const)); + BOOST_CHECK( + jmodel.JointMappedVelocitySelector(vec) == jmodel_composite.JointMappedVelocitySelector(vec)); + BOOST_CHECK( + jmodel.JointMappedVelocitySelector(vec_const) + == jmodel_composite.JointMappedVelocitySelector(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.jointExtendedModelCols(mat) == jmodel_composite.jointExtendedModelCols(mat)); + BOOST_CHECK( + jmodel.jointExtendedModelCols(mat_const) == jmodel_composite.jointExtendedModelCols(mat_const)); } struct TestJointComposite @@ -259,7 +274,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..1f993b7bb7 100644 --- a/unittest/joint-generic.cpp +++ b/unittest/joint-generic.cpp @@ -218,18 +218,18 @@ struct init> 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; } @@ -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, + NVExtended = 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> @@ -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..c75aa4e435 100644 --- a/unittest/joint-jacobian.cpp +++ b/unittest/joint-jacobian.cpp @@ -12,10 +12,9 @@ #include "pinocchio/utils/timer.hpp" #include "pinocchio/algorithm/joint-configuration.hpp" -#include - #include #include +#include "utils/model-generator.hpp" template inline bool isFinite(const Eigen::MatrixBase & x) @@ -23,15 +22,11 @@ inline bool isFinite(const Eigen::MatrixBase & x) return ((x - x).array() == (x - x).array()).all(); } -BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) - -BOOST_AUTO_TEST_CASE(test_jacobian) +void test_jacobian_impl(const pinocchio::Model & model) { using namespace Eigen; using namespace pinocchio; - pinocchio::Model model; - pinocchio::buildModels::humanoidRandom(model); pinocchio::Data data(model); VectorXd q = VectorXd::Zero(model.nq); @@ -71,16 +66,77 @@ BOOST_AUTO_TEST_CASE(test_jacobian) BOOST_CHECK(data_fk.J.isApprox(data.J)); } +BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) + +BOOST_AUTO_TEST_CASE(test_jacobian) +{ + pinocchio::Model model; + pinocchio::buildModels::humanoidRandom(model, false); + + test_jacobian_impl(model); +} + +BOOST_AUTO_TEST_CASE(test_jacobian_mimic) +{ + using namespace Eigen; + using namespace pinocchio; + + // Full model and mimic var + pinocchio::Model model_full; + pinocchio::buildModels::humanoidRandom(model_full, false); + const double ratio = 2.5; + const double offset = 0.75; + pinocchio::Model model_mimic; + Eigen::MatrixXd G; + + // Direct parent/Child + std::vector primary_ids = {model_full.getJointId("rleg1_joint")}; + std::vector secondary_ids = {model_full.getJointId("rleg2_joint")}; + pinocchio::buildMimicModel(model_full, primary_ids, secondary_ids, ratio, offset, model_mimic, G); + test_jacobian_impl(model_mimic); + + // Spaced Parent/Child + primary_ids.clear(); + secondary_ids.clear(); + primary_ids = {model_full.getJointId("rleg1_joint")}; + secondary_ids = {model_full.getJointId("rleg4_joint")}; + pinocchio::buildMimicModel(model_full, primary_ids, secondary_ids, ratio, offset, model_mimic, G); + test_jacobian_impl(model_mimic); + + // Parallel + primary_ids.clear(); + secondary_ids.clear(); + primary_ids = {model_full.getJointId("lleg1_joint")}; + secondary_ids = {model_full.getJointId("rleg1_joint")}; + pinocchio::buildMimicModel(model_full, primary_ids, secondary_ids, ratio, offset, model_mimic, G); + test_jacobian_impl(model_mimic); + + // Double mimic, not same primary + primary_ids.clear(); + secondary_ids.clear(); + primary_ids = {model_full.getJointId("lleg1_joint"), model_full.getJointId("rarm1_joint")}; + secondary_ids = {model_full.getJointId("rleg1_joint"), model_full.getJointId("larm1_joint")}; + pinocchio::buildMimicModel(model_full, primary_ids, secondary_ids, ratio, offset, model_mimic, G); + test_jacobian_impl(model_mimic); + + // Double Mimic, Same Primary + primary_ids.clear(); + secondary_ids.clear(); + primary_ids = {model_full.getJointId("lleg1_joint"), model_full.getJointId("lleg1_joint")}; + secondary_ids = {model_full.getJointId("rleg1_joint"), model_full.getJointId("lleg2_joint")}; + pinocchio::buildMimicModel(model_full, primary_ids, secondary_ids, ratio, offset, model_mimic, G); + test_jacobian_impl(model_mimic); +} + BOOST_AUTO_TEST_CASE(test_jacobian_time_variation) { using namespace Eigen; 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 +159,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 +223,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..59b82b200e 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(1, 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(1, 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(1, 0, 0, 0); test_constraint_mimic(jmodel); } @@ -122,14 +120,13 @@ 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> Variant; boost::mpl::for_each(TestJointConstraint()); } -template +template void test_joint_mimic(const JointModelBase & jmodel) { typedef typename traits::JointDerived Joint; @@ -137,16 +134,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 +150,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 +185,27 @@ struct TestJointMimic void operator()(const JointModelBase &) const { JointModel jmodel; - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(1, 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(1, 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(1, 0, 0, 0); - test_joint_mimic(jmodel); + test_joint_mimic( + jmodel); } }; @@ -214,11 +214,15 @@ 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 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 +235,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,26 +254,29 @@ 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_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(2, -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(2, -2, -2, 3); - BOOST_CHECK(jmodel_generic.id() == jmodel_ref.id()); + BOOST_CHECK(jmodel_generic.id() == jmodel.id()); } BOOST_AUTO_TEST_SUITE_END() diff --git a/unittest/joint-motion-subspace.cpp b/unittest/joint-motion-subspace.cpp index ba1b90bc38..2940a930bc 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,21 +114,6 @@ struct buildModel } }; -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"); - - return model; - } -}; - template void test_constraint_operations(const JointModelBase & jmodel) { @@ -268,6 +258,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; @@ -377,17 +373,17 @@ 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); + jmodel.setIndexes(1, 0, 0, 0); return jmodel; } @@ -429,8 +425,6 @@ struct TestJointConstraint void operator()(const JointModelBase &) const { JointModel jmodel = init::run(); - jmodel.setIndexes(0, 0, 0); - test_constraint_operations(jmodel); } }; diff --git a/unittest/kinematics.cpp b/unittest/kinematics.cpp index afde400324..2f2e6b5210 100644 --- a/unittest/kinematics.cpp +++ b/unittest/kinematics.cpp @@ -14,6 +14,8 @@ #include #include +#include "utils/model-generator.hpp" + BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) BOOST_AUTO_TEST_CASE(test_kinematics_constant_vector_input) @@ -81,6 +83,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; @@ -283,4 +336,82 @@ BOOST_AUTO_TEST_CASE(test_kinematic_getters) ac_align.isApprox(getClassicalAcceleration(model, data, jointId, LOCAL_WORLD_ALIGNED))); } +void test_mimic_against_full_model( + const pinocchio::Model & model_full, + const std::vector & primary_ids, + const std::vector & secondary_ids) +{ + const double ratio = 2.5; + const double offset = 0.75; + pinocchio::Model model_mimic; + Eigen::MatrixXd G; + buildMimicModel(model_full, primary_ids, secondary_ids, ratio, offset, model_mimic, G); + + 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; + + toFull(model_full, model_mimic, secondary_ids, ratio, offset, q, q_full); + + pinocchio::Data dataFKFull(model_full); + pinocchio::forwardKinematics(model_full, dataFKFull, q_full, v_full, a_full); + + pinocchio::Data dataFKRed(model_mimic); + pinocchio::forwardKinematics(model_mimic, dataFKRed, q, v, a); + + for (int i = 0; i < model_full.njoints; i++) + { + BOOST_CHECK(dataFKRed.oMi[i].isApprox(dataFKFull.oMi[i])); + BOOST_CHECK(dataFKRed.liMi[i].isApprox(dataFKFull.liMi[i])); + BOOST_CHECK(model_full.inertias[i].isApprox(model_mimic.inertias[i])); + } +} + +BOOST_AUTO_TEST_CASE(test_kinematics_mimic) +{ + pinocchio::Model humanoid_model; + pinocchio::buildModels::humanoidRandom(humanoid_model); + + // Direct parent/Child + std::vector primaries = {humanoid_model.getJointId("rleg1_joint")}; + std::vector secondaries = {humanoid_model.getJointId("rleg2_joint")}; + test_mimic_against_full_model(humanoid_model, primaries, secondaries); + + // Spaced Parent/Child + primaries.clear(); + secondaries.clear(); + primaries = {humanoid_model.getJointId("rleg1_joint")}; + secondaries = {humanoid_model.getJointId("rleg4_joint")}; + test_mimic_against_full_model(humanoid_model, primaries, secondaries); + + // Parallel + primaries.clear(); + secondaries.clear(); + primaries = {humanoid_model.getJointId("lleg1_joint")}; + secondaries = {humanoid_model.getJointId("rleg1_joint")}; + test_mimic_against_full_model(humanoid_model, primaries, secondaries); + + // Double mimic, not same primary + primaries.clear(); + secondaries.clear(); + primaries = {humanoid_model.getJointId("lleg1_joint"), humanoid_model.getJointId("rarm1_joint")}; + + secondaries = { + humanoid_model.getJointId("rleg1_joint"), humanoid_model.getJointId("larm1_joint")}; + test_mimic_against_full_model(humanoid_model, primaries, secondaries); + + // Double Mimic, Same Primary + primaries.clear(); + secondaries.clear(); + primaries = {humanoid_model.getJointId("lleg1_joint"), humanoid_model.getJointId("lleg1_joint")}; + + secondaries = { + humanoid_model.getJointId("rleg1_joint"), humanoid_model.getJointId("lleg2_joint")}; + test_mimic_against_full_model(humanoid_model, primaries, secondaries); +} + BOOST_AUTO_TEST_SUITE_END() diff --git a/unittest/model.cpp b/unittest/model.cpp index 7e254fbb04..e398e2d904 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 @@ -252,6 +252,9 @@ BOOST_AUTO_TEST_CASE(append) BOOST_CHECK( joint_model_humanoid.jointConfigSelector(humanoid_config->second) == joint_model1.jointConfigSelector(config_vector)); + BOOST_CHECK( + joint_model_humanoid.JointMappedConfigSelector(humanoid_config->second) + == joint_model1.JointMappedConfigSelector(config_vector)); // std::cerr<<"humanoid "<second) == joint_model1.jointConfigSelector(config_vector)); + BOOST_CHECK( + joint_model_manipulator.JointMappedConfigSelector(manipulator_config->second) + == joint_model1.JointMappedConfigSelector(config_vector)); // std::cerr<<"manipulator "<second) == joint_model.jointConfigSelector(config_vector)); + BOOST_CHECK( + joint_model_humanoid.JointMappedConfigSelector(humanoid_config->second) + == joint_model.JointMappedConfigSelector(config_vector)); // std::cerr<<"humanoid "<second) == joint_model.jointConfigSelector(config_vector)); + BOOST_CHECK( + joint_model_manipulator.JointMappedConfigSelector(manipulator_config->second) + == joint_model.JointMappedConfigSelector(config_vector)); // std::cerr<<"manipulator "<(); + data = Data(humanoid_mimic); + BOOST_CHECK(humanoid_mimic.check(data)); + index_s = humanoid_mimic.getJointId("lleg_shoulder3_joint"); + BOOST_CHECK_EQUAL(humanoid_mimic.idx_qs[index_s], humanoid_mimic.joints[index_s].idx_q()); + BOOST_CHECK_EQUAL(humanoid_mimic.idx_vs[index_s], humanoid_mimic.joints[index_s].idx_v()); + + ModelTpl humanoid_mimic_f = humanoid_mimic.cast(); + DataTpl data_f(humanoid_mimic_f); + BOOST_CHECK(humanoid_mimic_f.check(data_f)); + index_s = humanoid_mimic_f.getJointId("lleg_shoulder3_joint"); + BOOST_CHECK_EQUAL(humanoid_mimic_f.idx_qs[index_s], humanoid_mimic_f.joints[index_s].idx_q()); + BOOST_CHECK_EQUAL(humanoid_mimic_f.idx_vs[index_s], humanoid_mimic_f.joints[index_s].idx_v()); +} + +BOOST_AUTO_TEST_CASE(test_build_reduced_model_mimic) +{ + typedef Model::JointCollection JC; + typedef Model::JointIndex JointIndex; + typedef JC::JointModelRX JointModelRX; + typedef JC::JointModelRUBX JointModelRUBX; + Model model; + + JointIndex ffidx = model.addJoint(0, JC::JointModelFreeFlyer(), SE3::Identity(), "root_joint"); + model.addJointFrame(ffidx); + JointIndex idx = model.addJoint(ffidx, JointModelRX(), SE3::Identity(), "joint1"); + model.addJointFrame(idx); + idx = model.addJoint(idx, JointModelRUBX(), SE3::Identity(), "joint2"); + model.addJointFrame(idx); + idx = model.addJoint(idx, JointModelRX(), SE3::Identity(), "joint3"); + model.addJointFrame(idx); + + auto const mimic = JointModelMimic( + boost::get(model.joints[model.getJointId("joint3")].toVariant()), 1., 0.); + idx = model.addJoint(idx, mimic, SE3::Identity(), "joint4"); + model.addJointFrame(idx); + + // We lock a 2 DoF joint that is before the mimicked joint. + const std::vector joints_to_lock = {model.getJointId("joint2")}; + Model reduced_model; + BOOST_CHECK_NO_THROW(buildReducedModel(model, joints_to_lock, neutral(model), reduced_model)); +} + BOOST_AUTO_TEST_SUITE_END() diff --git a/unittest/python/bindings_data.py b/unittest/python/bindings_data.py index 7a2c961916..3fc7b24c19 100644 --- a/unittest/python/bindings_data.py +++ b/unittest/python/bindings_data.py @@ -7,7 +7,7 @@ class TestData(TestCase): def setUp(self): - self.model = pin.buildSampleModelHumanoidRandom() + self.model = pin.buildSampleModelHumanoidRandom(True, True) self.data = self.model.createData() def test_copy(self): diff --git a/unittest/python/bindings_kinematics.py b/unittest/python/bindings_kinematics.py index 402e712f1b..721dfa4fde 100644 --- a/unittest/python/bindings_kinematics.py +++ b/unittest/python/bindings_kinematics.py @@ -7,7 +7,7 @@ class TestKinematicsBindings(PinocchioTestCase): def setUp(self): - self.model = pin.buildSampleModelHumanoidRandom() + self.model = pin.buildSampleModelHumanoidRandom(True, True) self.joint_idx = ( self.model.getJointId("rarm2_joint") if self.model.existJointName("rarm2_joint") diff --git a/unittest/reachable-workspace.cpp b/unittest/reachable-workspace.cpp index c4b56f83dc..1f19dbedd1 100644 --- a/unittest/reachable-workspace.cpp +++ b/unittest/reachable-workspace.cpp @@ -296,7 +296,7 @@ BOOST_AUTO_TEST_CASE(test_reachable_workspace) // Load the urdf model Model model; - pinocchio::buildModels::manipulator(model); + pinocchio::buildModels::manipulator(model, true); Eigen::VectorXd q = (model.upperPositionLimit + model.lowerPositionLimit) / 2; ReachableSetResults res; diff --git a/unittest/rnea.cpp b/unittest/rnea.cpp index 51b7842c63..3375608dee 100644 --- a/unittest/rnea.cpp +++ b/unittest/rnea.cpp @@ -9,9 +9,10 @@ * */ -#include "pinocchio/spatial/fwd.hpp" #include "pinocchio/multibody/model.hpp" #include "pinocchio/multibody/data.hpp" +#include "pinocchio/algorithm/model.hpp" +#include "pinocchio/spatial/fwd.hpp" #include "pinocchio/algorithm/rnea.hpp" #include "pinocchio/algorithm/jacobian.hpp" #include "pinocchio/algorithm/center-of-mass.hpp" @@ -33,6 +34,8 @@ #include #include +#include "utils/model-generator.hpp" + BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) BOOST_AUTO_TEST_CASE(test_rnea) @@ -44,7 +47,7 @@ BOOST_AUTO_TEST_CASE(test_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.); @@ -79,7 +82,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 +139,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 +182,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 +212,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 +242,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 +288,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 +347,114 @@ BOOST_AUTO_TEST_CASE(test_compute_coriolis) } } +void test_mimic_against_full_model( + const pinocchio::Model & model_full, + const std::vector & primary_ids, + const std::vector & secondary_ids) +{ + const double ratio = 2.5; + const double offset = 0.75; + pinocchio::Model model_mimic; + Eigen::MatrixXd G; + buildMimicModel(model_full, primary_ids, secondary_ids, ratio, offset, model_mimic, G); + + pinocchio::Data data_nle_mimic(model_mimic); + pinocchio::Data data_rnea_mimic(model_mimic); + pinocchio::Data data_full(model_full); + + 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; + + toFull(model_full, model_mimic, secondary_ids, ratio, offset, q, q_full); + + 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_ref_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); + + // Direct parent/Child + std::vector primaries = {humanoid_model.getJointId("rleg1_joint")}; + std::vector secondaries = {humanoid_model.getJointId("rleg2_joint")}; + test_mimic_against_full_model(humanoid_model, primaries, secondaries); + + // Spaced Parent/Child + primaries.clear(); + secondaries.clear(); + primaries = {humanoid_model.getJointId("rleg1_joint")}; + secondaries = {humanoid_model.getJointId("rleg4_joint")}; + test_mimic_against_full_model(humanoid_model, primaries, secondaries); + + // Parallel + primaries.clear(); + secondaries.clear(); + primaries = {humanoid_model.getJointId("lleg1_joint")}; + secondaries = {humanoid_model.getJointId("rleg1_joint")}; + test_mimic_against_full_model(humanoid_model, primaries, secondaries); + + // Double mimic, not same primary + primaries.clear(); + secondaries.clear(); + primaries = {humanoid_model.getJointId("lleg1_joint"), humanoid_model.getJointId("rarm1_joint")}; + + secondaries = { + humanoid_model.getJointId("rleg1_joint"), humanoid_model.getJointId("larm1_joint")}; + test_mimic_against_full_model(humanoid_model, primaries, secondaries); + + // Double Mimic, Same Primary + primaries.clear(); + secondaries.clear(); + primaries = {humanoid_model.getJointId("lleg1_joint"), humanoid_model.getJointId("lleg1_joint")}; + + secondaries = { + humanoid_model.getJointId("rleg1_joint"), humanoid_model.getJointId("lleg2_joint")}; + test_mimic_against_full_model(humanoid_model, primaries, secondaries); +} + BOOST_AUTO_TEST_SUITE_END() diff --git a/unittest/sample-models.cpp b/unittest/sample-models.cpp index 4ad4815988..6e60d7ea60 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,6 +48,18 @@ 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) @@ -37,6 +70,12 @@ BOOST_AUTO_TEST_CASE(build_model_sample_manipulator) BOOST_CHECK(model.nq == 6); BOOST_CHECK(model.nv == 6); + pinocchio::Model model_m; + pinocchio::buildModels::manipulator(model_m, true); + + BOOST_CHECK(model_m.nq == 5); + BOOST_CHECK(model_m.nv == 5); + #ifdef PINOCCHIO_WITH_HPP_FCL pinocchio::Data data(model); pinocchio::GeometryModel geom; diff --git a/unittest/serialization.cpp b/unittest/serialization.cpp index 2ff9f8e57a..fc810abb67 100644 --- a/unittest/serialization.cpp +++ b/unittest/serialization.cpp @@ -132,7 +132,6 @@ void generic_test(const T & object, const std::string & filename, const std::str delete &object_loaded; } - // Load and save as XML const std::string xml_filename = filename + ".xml"; saveToXML(object, xml_filename, tag_name); @@ -140,7 +139,6 @@ void generic_test(const T & object, const std::string & filename, const std::str { T & object_loaded = *empty_contructor(); loadFromXML(object_loaded, xml_filename, tag_name); - // Check BOOST_CHECK(run_call_equality_op(object_loaded, object)); @@ -397,16 +395,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(1, 0, 0, 0); return jmodel; } @@ -471,35 +471,10 @@ 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(); - - JointDataMimic jdata_mimic = jmodel_mimic.createData(); - JointDataBase & jdata_mimic_base = static_cast(jdata_mimic); - - 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 q_random = lg.randomConfiguration(lb, ub); - - jmodel_mimic.calc(jdata_mimic, q_random); - Transform & m = jdata_mimic_base.M(); - test(m); - - Constraint & S = jdata_mimic_base.S(); - test(S); + // Do nothing } template @@ -554,37 +529,10 @@ 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(); - - JointDataMimic jdata_mimic = jmodel_mimic.createData(); - JointDataBase & jdata_mimic_base = static_cast(jdata_mimic); - - 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 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(); - - test(m); - - Bias & b = jdata_mimic_base.c(); - test(b); + // Do nothing } template @@ -654,30 +602,21 @@ 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(); - - JointDataMimic jdata_mimic = jmodel_mimic.createData(); - - typedef typename pinocchio::LieGroup::type LieGroupType; - LieGroupType lg; + typedef pinocchio::JointModelMimicTpl JointModel; + typedef typename JointModel::JointDerived JointDerived; + typedef typename pinocchio::traits::JointDataDerived JointData; - Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(), -1.)); - Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.)); + JointModel jmodel = init::run(); + JointData jdata = jmodel.createData(); - Eigen::VectorXd q_random = lg.randomConfiguration(lb, ub); - Eigen::VectorXd v_random = Eigen::VectorXd::Random(jmodel.nv()); + Eigen::VectorXd q_random = Eigen::VectorXd::Random(jmodel.jmodel().nq()); + Eigen::VectorXd v_random = Eigen::VectorXd::Random(jmodel.jmodel().nv()); + jmodel.calc(jdata, q_random, v_random); - 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); + test(jdata); } template diff --git a/unittest/utils/model-generator.hpp b/unittest/utils/model-generator.hpp index d5ad33a283..d40cf13528 100644 --- a/unittest/utils/model-generator.hpp +++ b/unittest/utils/model-generator.hpp @@ -3,6 +3,7 @@ // #include "pinocchio/multibody/model.hpp" +#include "pinocchio/algorithm/model.hpp" namespace pinocchio { @@ -65,4 +66,64 @@ namespace pinocchio "translation", Inertia::Random()); } + void toFull( + const Model & model_full, + const Model & model_mimic, + const std::vector & secondary_ids, + const double & ratio, + const double & offset, + const Eigen::VectorXd & q, + Eigen::VectorXd & q_full) + { + for (int n = 1; n < model_full.njoints; n++) + { + double joint_ratio = 1.0; + double joint_offset = 0.0; + if (std::find(secondary_ids.begin(), secondary_ids.end(), n) != secondary_ids.end()) + { + joint_ratio = ratio; + joint_offset = offset; + } + model_full.joints[n].JointMappedConfigSelector(q_full) = + joint_ratio * model_mimic.joints[n].JointMappedConfigSelector(q) + + joint_offset * Eigen::VectorXd::Ones(model_full.joints[n].nq()); + } + } + + void buildMimicModel( + const Model & model_full, + const std::vector & primary_ids, + const std::vector & secondary_ids, + const double & ratio, + const double & offset, + Model & model_mimic, + Eigen::MatrixXd & G) + { + Model model_temp; + model_temp = model_full; + for (int i = 0; i < primary_ids.size(); i++) + { + pinocchio::transformJointIntoMimic( + model_temp, primary_ids[i], secondary_ids[i], ratio, offset, model_mimic); + model_temp = model_mimic; + } + // Initialize G as a zero matrix + G.resize(model_full.nv, model_mimic.nv); + G.setZero(); + // Set ones on the pseudo-diagonal + for (int j = 1; j < model_full.njoints; ++j) + { + if (std::find(secondary_ids.begin(), secondary_ids.end(), j) == secondary_ids.end()) + G.block( + model_full.joints[j].idx_v(), model_mimic.joints[j].idx_v(), model_full.joints[j].nv(), + model_mimic.joints[j].nv()) + .setIdentity(); + } + + // Set specific values for primary-secondary joint pairs + for (size_t i = 0; i < secondary_ids.size(); ++i) + { + G(model_full.idx_vs[secondary_ids[i]], model_mimic.idx_vs[secondary_ids[i]]) = ratio; + } + } } // namespace pinocchio diff --git a/unittest/visitor.cpp b/unittest/visitor.cpp index 792cdfdf90..e03cd905f7 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(1, 0, 0, 0); return jmodel; }