diff --git a/core/include/jiminy/core/Types.h b/core/include/jiminy/core/Types.h index dfdb7b5d7..a9e0a5fd6 100644 --- a/core/include/jiminy/core/Types.h +++ b/core/include/jiminy/core/Types.h @@ -127,23 +127,27 @@ namespace jiminy struct flexibleJointData_t { std::string frameName; - vectorN_t stiffness; - vectorN_t damping; + vector3_t stiffness; + vector3_t damping; + vector3_t inertia; flexibleJointData_t(void) : frameName(), stiffness(), - damping() + damping(), + inertia() { // Empty. }; flexibleJointData_t(std::string const & frameNameIn, - vectorN_t const & stiffnessIn, - vectorN_t const & dampingIn) : + vector3_t const & stiffnessIn, + vector3_t const & dampingIn, + vector3_t const & inertiaIn) : frameName(frameNameIn), stiffness(stiffnessIn), - damping(dampingIn) + damping(dampingIn), + inertia(inertiaIn) { // Empty. }; @@ -152,7 +156,8 @@ namespace jiminy { return (this->frameName == other.frameName && this->stiffness == other.stiffness - && this->damping == other.damping); + && this->damping == other.damping + && this->inertia == other.inertia); }; }; diff --git a/core/include/jiminy/core/robot/Model.h b/core/include/jiminy/core/robot/Model.h index e3189edfe..e876ae35a 100644 --- a/core/include/jiminy/core/robot/Model.h +++ b/core/include/jiminy/core/robot/Model.h @@ -392,20 +392,22 @@ namespace jiminy hresult_t removeConstraints(std::vector const & constraintsNames, constraintsHolderType_t const & holderType); - hresult_t refreshCollisionsProxies(void); + hresult_t refreshGeometryProxies(void); hresult_t refreshContactsProxies(void); /// \brief Refresh the proxies of the kinematics constraints. hresult_t refreshConstraintsProxies(void); virtual hresult_t refreshProxies(void); public: - pinocchio::Model pncModelRigidOrig_; + pinocchio::Model pncModelOrig_; pinocchio::Model pncModel_; + pinocchio::GeometryModel collisionModelOrig_; pinocchio::GeometryModel collisionModel_; + pinocchio::GeometryModel visualModelOrig_; pinocchio::GeometryModel visualModel_; pinocchio::Data pncDataRigidOrig_; mutable pinocchio::Data pncData_; - mutable std::unique_ptr pncCollisionData_; // Using smart ptr to avoid having to initialize it with an empty GeometryModel, which causes Pinocchio segfault at least up to v2.5.6 + mutable std::unique_ptr collisionData_; // Using smart ptr to avoid having to initialize it with an empty GeometryModel, which causes Pinocchio segfault at least up to v2.5.6 std::unique_ptr mdlOptions_; forceVector_t contactForces_; ///< Buffer storing the contact forces diff --git a/core/include/jiminy/core/robot/PinocchioOverloadAlgorithms.h b/core/include/jiminy/core/robot/PinocchioOverloadAlgorithms.h index 71d3950c0..1526e127e 100644 --- a/core/include/jiminy/core/robot/PinocchioOverloadAlgorithms.h +++ b/core/include/jiminy/core/robot/PinocchioOverloadAlgorithms.h @@ -17,7 +17,7 @@ #include -#include "pinocchio/spatial/fwd.hpp" // `Pinocchio::Motion` +#include "pinocchio/spatial/fwd.hpp" // `Pinocchio::Inertia` #include "pinocchio/multibody/visitor.hpp" // `pinocchio::fusion::JointUnaryVisitorBase` #include "pinocchio/multibody/fwd.hpp" // `pinocchio::ModelTpl`, `pinocchio::DataTpl` #include "pinocchio/multibody/joint/fwd.hpp" // `pinocchio::JointModelBase`, `pinocchio::JointDataBase`, ... @@ -74,7 +74,7 @@ namespace pinocchio_overload Eigen::MatrixBase const & a) { (void) pinocchio::rnea(model, data, q, v, a); - data.tau += model.rotorInertia.asDiagonal() * a; + data.tau.array() += model.rotorInertia.array() * a.array(); return data.tau; } @@ -86,9 +86,7 @@ namespace pinocchio_overload Eigen::MatrixBase const & q) { (void) pinocchio::crba(model, data, q); - // data.M.triangularView() = - // data.M.transpose().triangularView(); - data.M += model.rotorInertia.asDiagonal(); + data.M.diagonal() += model.rotorInertia; return data.M; } @@ -120,8 +118,8 @@ namespace pinocchio_overload jmodel.jointVelocitySelector(data.u) -= jdata.S().transpose()*data.f[i]; // jmodel.calc_aba(jdata.derived(), Ia, parent > 0); - Scalar const & Im = model.rotorInertia[jmodel.idx_v()]; - calc_aba(jmodel.derived(), jdata.derived(), Ia, Im, parent > 0); + auto const Im = model.rotorInertia.segment(jmodel.idx_v(), jmodel.nv()); + calc_aba(jmodel.derived(), jdata.derived(), Im, Ia, parent > 0); if (parent > 0) { @@ -132,102 +130,243 @@ namespace pinocchio_overload } } - template + template static std::enable_if_t || is_pinocchio_joint_revolute_unbounded_v, void> calc_aba(JointModel const & model, typename JointModel::JointDataDerived & data, + Eigen::MatrixBase const & Im, Eigen::MatrixBase & Ia, - Scalar const & Im, bool const & update_I) { - using Motion = pinocchio::Motion; + using Inertia = pinocchio::Inertia; - data.U.noalias() = Ia.col(Motion::ANGULAR + getAxis(model)); - data.Dinv[0] = Scalar(1.0) / (data.U[Motion::ANGULAR + getAxis(model)] + Im); + data.U = Ia.col(Inertia::ANGULAR + getAxis(model)); + data.Dinv[0] = Scalar(1) / (data.U[Inertia::ANGULAR + getAxis(model)] + Im[0]); data.UDinv.noalias() = data.U * data.Dinv[0]; if (update_I) { - Ia -= data.UDinv * data.U.transpose(); + Ia.noalias() -= data.UDinv * data.U.transpose(); } } - template + template static std::enable_if_t || is_pinocchio_joint_revolute_unbounded_unaligned_v, void> calc_aba(JointModel const & model, typename JointModel::JointDataDerived & data, + Eigen::MatrixBase const & Im, Eigen::MatrixBase & Ia, - Scalar const & Im, bool const & update_I) { - using Motion = pinocchio::Motion; + using Inertia = pinocchio::Inertia; - data.U.noalias() = Ia.template middleCols<3>(Motion::ANGULAR) * model.axis; - data.Dinv[0] = Scalar(1.0) / (model.axis.dot(data.U.template segment<3>(Motion::ANGULAR)) + Im); + data.U.noalias() = Ia.template middleCols<3>(Inertia::ANGULAR) * model.axis; + data.Dinv[0] = Scalar(1) / (model.axis.dot(data.U.template segment<3>(Inertia::ANGULAR)) + Im[0]); data.UDinv.noalias() = data.U * data.Dinv[0]; if (update_I) { - Ia -= data.UDinv * data.U.transpose(); + Ia.noalias() -= data.UDinv * data.U.transpose(); } } - template + template static std::enable_if_t, void> calc_aba(JointModel const & model, typename JointModel::JointDataDerived & data, + Eigen::MatrixBase const & Im, Eigen::MatrixBase & Ia, - Scalar const & Im, bool const & update_I) { - using Motion = pinocchio::Motion; + using Inertia = pinocchio::Inertia; - data.U.noalias() = Ia.col(Motion::LINEAR + getAxis(model)); - data.Dinv[0] = Scalar(1.0) / (data.U[Motion::LINEAR + getAxis(model)] + Im); + data.U = Ia.col(Inertia::LINEAR + getAxis(model)); + data.Dinv[0] = Scalar(1) / (data.U[Inertia::LINEAR + getAxis(model)] + Im[0]); data.UDinv.noalias() = data.U * data.Dinv[0]; if (update_I) { - Ia -= data.UDinv * data.U.transpose(); + Ia.noalias() -= data.UDinv * data.U.transpose(); } } - template + template static std::enable_if_t, void> calc_aba(JointModel const & model, typename JointModel::JointDataDerived & data, + Eigen::MatrixBase const & Im, Eigen::MatrixBase & Ia, - Scalar const & Im, bool const & update_I) { - using Motion = pinocchio::Motion; + using Inertia = pinocchio::Inertia; - data.U.noalias() = Ia.template middleCols<3>(Motion::LINEAR) * model.axis; - data.Dinv[0] = Scalar(1.0) / (model.axis.dot(data.U.template segment<3>(Motion::LINEAR)) + Im); + data.U.noalias() = Ia.template middleCols<3>(Inertia::LINEAR) * model.axis; + data.Dinv[0] = Scalar(1) / (model.axis.dot(data.U.template segment<3>(Inertia::LINEAR)) + Im[0]); data.UDinv.noalias() = data.U * data.Dinv[0]; if (update_I) { - Ia -= data.UDinv * data.U.transpose(); + Ia.noalias() -= data.UDinv * data.U.transpose(); + } + } + + template + static std::enable_if_t, void> + calc_aba(JointModel const & /* model */, + typename JointModel::JointDataDerived & data, + Eigen::MatrixBase const & Im, + Eigen::MatrixBase & Ia, + bool const & update_I) + { + static Eigen::Matrix StU; + + data.U.template leftCols<2>() = Ia.template leftCols<2>(); + data.U.template rightCols<1>() = Ia.template rightCols<1>(); + StU.template leftCols<2>() = data.U.template topRows<2>().transpose(); + StU.template rightCols<1>() = data.U.template bottomRows<1>(); + + StU.diagonal() += Im; + pinocchio::internal::PerformStYSInversion::run(StU, data.Dinv); + data.UDinv.noalias() = data.U * data.Dinv; + + if(update_I) + { + Ia.noalias() -= data.UDinv * data.U.transpose(); + } + } + + template + static std::enable_if_t, void> + calc_aba(JointModel const & /* model */, + typename JointModel::JointDataDerived & data, + Eigen::MatrixBase const & Im, + Eigen::MatrixBase & Ia, + bool const & update_I) + { + static Eigen::Matrix StU; + + using Inertia = pinocchio::Inertia; + + data.U = Ia.template middleCols<3>(Inertia::LINEAR); + StU = data.U.template middleRows<3>(Inertia::LINEAR); + StU.diagonal() += Im; + + pinocchio::internal::PerformStYSInversion::run(StU, data.Dinv); + data.UDinv.noalias() = data.U * data.Dinv; + + if(update_I) + { + Ia.noalias() -= data.UDinv * data.U.transpose(); + } + } + + template + static std::enable_if_t, void> + calc_aba(JointModel const & /* model */, + typename JointModel::JointDataDerived & data, + Eigen::MatrixBase const & Im, + Eigen::MatrixBase & Ia, + bool const & update_I) + { + static Eigen::Matrix StU; + + using Inertia = pinocchio::Inertia; + + data.U = Ia.template block<6,3>(0, Inertia::ANGULAR); + StU = data.U.template middleRows<3>(Inertia::ANGULAR); + StU.diagonal() += Im; + + pinocchio::internal::PerformStYSInversion::run(StU, data.Dinv); + data.UDinv.noalias() = data.U * data.Dinv; + + if (update_I) + { + Ia.noalias() -= data.UDinv * data.U.transpose(); + } + } + + template + static std::enable_if_t, void> + calc_aba(JointModel const & /* model */, + typename JointModel::JointDataDerived & data, + Eigen::MatrixBase const & Im, + Eigen::MatrixBase & Ia, + bool const & update_I) + { + static Eigen::Matrix StU; + + using Inertia = pinocchio::Inertia; + + data.U.noalias() = Ia.template middleCols<3>(Inertia::ANGULAR) * data.S.angularSubspace(); + StU.noalias() = data.S.angularSubspace().transpose() * data.U.template middleRows<3>(Inertia::ANGULAR); + StU.diagonal() += Im; + + pinocchio::internal::PerformStYSInversion::run(StU, data.Dinv); + data.UDinv.noalias() = data.U * data.Dinv; + + if (update_I) + { + Ia.noalias() -= data.UDinv * data.U.transpose(); + } + } + + template + static std::enable_if_t, void> + calc_aba(JointModel const & /* model */, + typename JointModel::JointDataDerived & data, + Eigen::MatrixBase const & Im, + Eigen::MatrixBase & Ia, + bool const & update_I) + { + static Eigen::Matrix StU; + + data.U = Ia; + StU = Ia; + StU.diagonal() += Im; + + pinocchio::internal::PerformStYSInversion::run(StU, data.Dinv); + data.UDinv.noalias() = data.U * data.Dinv; + + if(update_I) + { + Ia.noalias() -= data.UDinv * data.U.transpose(); + } + } + + template + static std::enable_if_t, void> + calc_aba(JointModel const & /* model */, + typename JointModel::JointDataDerived & data, + Eigen::MatrixBase const & Im, + Eigen::MatrixBase & Ia, + bool const & update_I) + { + static Eigen::Matrix StU; + + data.U.noalias() = Ia * data.S.matrix(); + StU.noalias() = data.S.matrix().transpose() * data.U; + StU.diagonal() += Im; + + pinocchio::internal::PerformStYSInversion::run(StU, data.Dinv); + data.UDinv.noalias() = data.U * data.Dinv; + + if(update_I) + { + Ia.noalias() -= data.UDinv * data.U.transpose(); } } - template - static std::enable_if_t - || is_pinocchio_joint_translation_v - || is_pinocchio_joint_spherical_v - || is_pinocchio_joint_spherical_zyx_v - || is_pinocchio_joint_freeflyer_v - || is_pinocchio_joint_mimic_v - || is_pinocchio_joint_composite_v, void> + template + static std::enable_if_t< is_pinocchio_joint_mimic_v, void> calc_aba(JointModel const & model, typename JointModel::JointDataDerived & data, + Eigen::MatrixBase const & /* Im */, Eigen::MatrixBase & Ia, - Scalar const & /* Im */, bool const & update_I) { + // TODO: Not implemented model.calc_aba(data.derived(), Ia, update_I); } diff --git a/core/include/jiminy/core/utilities/Json.tpp b/core/include/jiminy/core/utilities/Json.tpp index b8f28b4c4..3cae90718 100644 --- a/core/include/jiminy/core/utilities/Json.tpp +++ b/core/include/jiminy/core/utilities/Json.tpp @@ -15,11 +15,39 @@ namespace jiminy return {value}; } - template<> - Json::Value convertToJson(vectorN_t const & value); + template + Json::Value convertToJson(Eigen::Matrix const & value) + { + Json::Value row(Json::arrayValue); + for (Eigen::Index i = 0; i < value.size(); ++i) + { + row.append(value[i]); + } + return row; + } - template<> - Json::Value convertToJson(matrixN_t const & value); + template + Json::Value convertToJson(Eigen::Matrix const & value) + { + Json::Value mat(Json::arrayValue); + if (value.rows() > 0) + { + for (Eigen::Index i = 0; i Json::Value convertToJson(flexibleJointData_t const & value); diff --git a/core/src/engine/EngineMultiRobot.cc b/core/src/engine/EngineMultiRobot.cc index c6a660c72..58989ab00 100644 --- a/core/src/engine/EngineMultiRobot.cc +++ b/core/src/engine/EngineMultiRobot.cc @@ -2795,7 +2795,7 @@ namespace jiminy pinocchio::Model const & model = system.robot->pncModel_; pinocchio::Data & data = system.robot->pncData_; pinocchio::GeometryModel const & geomModel = system.robot->collisionModel_; - pinocchio::GeometryData & geomData = *system.robot->pncCollisionData_; + pinocchio::GeometryData & geomData = *system.robot->collisionData_; // Update forward kinematics pinocchio::forwardKinematics(model, data, q, v, a); @@ -2874,7 +2874,7 @@ namespace jiminy jointIndex_t const & parentJointIdx = system.robot->collisionModel_.geometryObjects[geometryIdx].parentJoint; // Extract collision and distance results - hpp::fcl::CollisionResult const & collisionResult = system.robot->pncCollisionData_->collisionResults[collisionPairIdx]; + hpp::fcl::CollisionResult const & collisionResult = system.robot->collisionData_->collisionResults[collisionPairIdx]; fextLocal.setZero(); @@ -3384,11 +3384,11 @@ namespace jiminy jointIndex_t const & jointIdx = flexibilityIdx[i]; uint32_t const & positionIdx = pncModel.joints[jointIdx].idx_q(); uint32_t const & velocityIdx = pncModel.joints[jointIdx].idx_v(); - vectorN_t const & stiffness = mdlDynOptions.flexibilityConfig[i].stiffness; - vectorN_t const & damping = mdlDynOptions.flexibilityConfig[i].damping; + vector3_t const & stiffness = mdlDynOptions.flexibilityConfig[i].stiffness; + vector3_t const & damping = mdlDynOptions.flexibilityConfig[i].damping; quaternion_t const quat(q.segment<4>(positionIdx)); // Only way to initialize with [x,y,z,w] order - vectorN_t const axis = pinocchio::quaternion::log3(quat); + vector3_t const axis = pinocchio::quaternion::log3(quat); uInternal.segment<3>(velocityIdx).array() += - stiffness.array() * axis.array() - damping.array() * v.segment<3>(velocityIdx).array(); diff --git a/core/src/robot/Model.cc b/core/src/robot/Model.cc index ba4dee805..7f0863e1a 100644 --- a/core/src/robot/Model.cc +++ b/core/src/robot/Model.cc @@ -189,13 +189,15 @@ namespace jiminy } Model::Model(void) : - pncModelRigidOrig_(), + pncModelOrig_(), pncModel_(), + collisionModelOrig_(), collisionModel_(), + visualModelOrig_(), visualModel_(), pncDataRigidOrig_(), pncData_(), - pncCollisionData_(nullptr), + collisionData_(nullptr), mdlOptions_(nullptr), contactForces_(), isInitialized_(false), @@ -265,12 +267,12 @@ namespace jiminy meshPackageDirs_.clear(); // Set the models - pncModelRigidOrig_ = pncModel; - collisionModel_ = collisionModel; - visualModel_ = visualModel; + pncModelOrig_ = pncModel; + collisionModelOrig_ = collisionModel; + visualModelOrig_ = visualModel; // Add ground geometry object to collision model is not already available - if (!collisionModel_.existGeometryName("ground")) + if (!collisionModelOrig_.existGeometryName("ground")) { // Instantiate ground FCL box geometry, wrapped as a pinocchio collision geometry. // Note that half-space cannot be used for Shape-Shape collision because it has no @@ -286,26 +288,26 @@ namespace jiminy pinocchio::GeometryObject groundPlane("ground", 0, 0, groudBox, groundPose); // Add the ground plane pinocchio to the robot model - collisionModel_.addGeometryObject(groundPlane, pncModelRigidOrig_); + collisionModelOrig_.addGeometryObject(groundPlane, pncModelOrig_); } // Backup the original model and data - pncDataRigidOrig_ = pinocchio::Data(pncModelRigidOrig_); + pncDataRigidOrig_ = pinocchio::Data(pncModelOrig_); // Initialize Pinocchio data internal state, including basic // attributes such as the mass of each body. - pinocchio::forwardKinematics(pncModelRigidOrig_, + pinocchio::forwardKinematics(pncModelOrig_, pncDataRigidOrig_, - pinocchio::neutral(pncModelRigidOrig_), - vectorN_t::Zero(pncModelRigidOrig_.nv)); - pinocchio::updateFramePlacements(pncModelRigidOrig_, pncDataRigidOrig_); - pinocchio::centerOfMass(pncModelRigidOrig_, + pinocchio::neutral(pncModelOrig_), + vectorN_t::Zero(pncModelOrig_.nv)); + pinocchio::updateFramePlacements(pncModelOrig_, pncDataRigidOrig_); + pinocchio::centerOfMass(pncModelOrig_, pncDataRigidOrig_, - pinocchio::neutral(pncModelRigidOrig_)); + pinocchio::neutral(pncModelOrig_)); /* Get the list of joint names of the rigid model and remove the 'universe' and 'root_joint' if any, since they are not actual joints. */ - rigidJointsNames_ = pncModelRigidOrig_.names; + rigidJointsNames_ = pncModelOrig_.names; rigidJointsNames_.erase(rigidJointsNames_.begin()); // remove 'universe' if (hasFreeflyer_) { @@ -401,7 +403,7 @@ namespace jiminy hresult_t returnCode = hresult_t::SUCCESS; // Check that no frame with the same name already exists. - if (pncModelRigidOrig_.existFrame(frameName)) + if (pncModelOrig_.existFrame(frameName)) { PRINT_ERROR("A frame with the same name already exists."); returnCode = hresult_t::ERROR_BAD_INPUT; @@ -411,19 +413,19 @@ namespace jiminy frameIndex_t parentFrameId = 0; if (returnCode == hresult_t::SUCCESS) { - returnCode = getFrameIdx(pncModelRigidOrig_, parentBodyName, parentFrameId); + returnCode = getFrameIdx(pncModelOrig_, parentBodyName, parentFrameId); } if (returnCode == hresult_t::SUCCESS) { // Add the frame to the the original rigid model { - jointIndex_t const & parentJointId = pncModelRigidOrig_.frames[parentFrameId].parent; - pinocchio::SE3 const & parentFramePlacement = pncModelRigidOrig_.frames[parentFrameId].placement; + jointIndex_t const & parentJointId = pncModelOrig_.frames[parentFrameId].parent; + pinocchio::SE3 const & parentFramePlacement = pncModelOrig_.frames[parentFrameId].placement; pinocchio::SE3 const jointFramePlacement = parentFramePlacement.act(framePlacement); pinocchio::Frame const frame(frameName, parentJointId, parentFrameId, jointFramePlacement, frameType); - pncModelRigidOrig_.addFrame(frame); - pncDataRigidOrig_ = pinocchio::Data(pncModelRigidOrig_); + pncModelOrig_.addFrame(frame); + pncDataRigidOrig_ = pinocchio::Data(pncModelOrig_); } // Add the frame to the the original flexible model @@ -464,10 +466,10 @@ namespace jiminy { frameIndex_t frameId; pinocchio::FrameType const frameType = pinocchio::FrameType::OP_FRAME; - returnCode = getFrameIdx(pncModelRigidOrig_, frameName, frameId); + returnCode = getFrameIdx(pncModelOrig_, frameName, frameId); if (returnCode == hresult_t::SUCCESS) { - if (pncModelRigidOrig_.frames[frameId].type != frameType) + if (pncModelOrig_.frames[frameId].type != frameType) { PRINT_ERROR("Impossible to remove this frame. One should only remove frames added manually."); returnCode = hresult_t::ERROR_BAD_INPUT; @@ -481,11 +483,11 @@ namespace jiminy { // Get the frame idx frameIndex_t frameId; - getFrameIdx(pncModelRigidOrig_, frameName, frameId); // It cannot fail + getFrameIdx(pncModelOrig_, frameName, frameId); // It cannot fail // Remove the frame from the the original rigid model - pncModelRigidOrig_.frames.erase(pncModelRigidOrig_.frames.begin() + frameId); - pncModelRigidOrig_.nframes--; + pncModelOrig_.frames.erase(pncModelOrig_.frames.begin() + frameId); + pncModelOrig_.nframes--; // Remove the frame from the the original flexible model getFrameIdx(pncModelFlexibleOrig_, frameName, frameId); @@ -494,7 +496,7 @@ namespace jiminy } // Regenerate rigid data - pncDataRigidOrig_ = pinocchio::Data(pncModelRigidOrig_); + pncDataRigidOrig_ = pinocchio::Data(pncModelOrig_); // One must reset the model after removing a frame reset(); @@ -524,7 +526,7 @@ namespace jiminy return hresult_t::SUCCESS; // Nothing to do. Returning early. } - if (collisionModel_.ngeoms == 0) // If successfully loaded, the ground should be available + if (collisionModelOrig_.ngeoms == 0) // If successfully loaded, the ground should be available { PRINT_ERROR("Collision geometry not available. Some collision meshes were probably not found."); return hresult_t::ERROR_INIT_FAILED; @@ -558,7 +560,7 @@ namespace jiminy for (std::string const & name : bodyNames) { bool_t hasGeometry = false; - for (pinocchio::GeometryObject const & geom : collisionModel_.geometryObjects) + for (pinocchio::GeometryObject const & geom : collisionModelOrig_.geometryObjects) { bool_t const isGeomMesh = (geom.meshPath.find('/') != std::string::npos || geom.meshPath.find('\\') != std::string::npos); @@ -580,14 +582,14 @@ namespace jiminy collisionBodiesNames_.insert(collisionBodiesNames_.end(), bodyNames.begin(), bodyNames.end()); // Create the collision pairs and add them to the geometry model of the robot - pinocchio::GeomIndex const & groundId = collisionModel_.getGeometryId("ground"); + pinocchio::GeomIndex const & groundId = collisionModelOrig_.getGeometryId("ground"); for (std::string const & name : bodyNames) { // Find the geometries having the body for parent, and add a collision pair for each of them constraintsMap_t collisionConstraintsMap; - for (std::size_t i = 0; i < collisionModel_.geometryObjects.size(); ++i) + for (std::size_t i = 0; i < collisionModelOrig_.geometryObjects.size(); ++i) { - pinocchio::GeometryObject const & geom = collisionModel_.geometryObjects[i]; + pinocchio::GeometryObject const & geom = collisionModelOrig_.geometryObjects[i]; bool_t const isGeomMesh = (geom.meshPath.find('/') != std::string::npos || geom.meshPath.find('\\') != std::string::npos); std::string const & frameName = pncModel_.frames[geom.parentFrame].name; @@ -597,7 +599,7 @@ namespace jiminy Note that the ground always comes second for the normal to be consistently compute wrt the ground instead of the body. */ pinocchio::CollisionPair const collisionPair(i, groundId); - collisionModel_.addCollisionPair(collisionPair); + collisionModelOrig_.addCollisionPair(collisionPair); if (returnCode == hresult_t::SUCCESS) { @@ -634,11 +636,10 @@ namespace jiminy } } - // Refresh proxies associated with the collisions only if (returnCode == hresult_t::SUCCESS) { - refreshCollisionsProxies(); + refreshGeometryProxies(); } return returnCode; @@ -689,18 +690,18 @@ namespace jiminy // Get the indices of the corresponding collision pairs in the geometry model of the robot and remove them std::vector collisionConstraintsNames; - pinocchio::GeomIndex const & groundId = collisionModel_.getGeometryId("ground"); + pinocchio::GeomIndex const & groundId = collisionModelOrig_.getGeometryId("ground"); for (std::string const & name : bodyNames) { // Find the geometries having the body for parent, and remove the collision pair for each of them - for (std::size_t i = 0; i < collisionModel_.geometryObjects.size(); ++i) + for (std::size_t i = 0; i < collisionModelOrig_.geometryObjects.size(); ++i) { - pinocchio::GeometryObject const & geom = collisionModel_.geometryObjects[i]; + pinocchio::GeometryObject const & geom = collisionModelOrig_.geometryObjects[i]; if (pncModel_.frames[geom.parentFrame].name == name) { // Remove the collision pair with the ground pinocchio::CollisionPair const collisionPair(i, groundId); - collisionModel_.removeCollisionPair(collisionPair); + collisionModelOrig_.removeCollisionPair(collisionPair); // Append collision geometry to the list of constraints to remove if (constraintsHolder_.exist(geom.name, constraintsHolderType_t::COLLISION_BODIES)) @@ -716,7 +717,7 @@ namespace jiminy removeFrames(collisionConstraintsNames); // Refresh proxies associated with the collisions only - refreshCollisionsProxies(); + refreshGeometryProxies(); return hresult_t::SUCCESS; } @@ -1031,7 +1032,7 @@ namespace jiminy { flexibleJointsNames_.clear(); flexibleJointsModelIdx_.clear(); - pncModelFlexibleOrig_ = pncModelRigidOrig_; + pncModelFlexibleOrig_ = pncModelOrig_; for(flexibleJointData_t const & flexibleJoint : mdlOptions_->dynamics.flexibilityConfig) { // Check if joint name exists @@ -1069,6 +1070,16 @@ namespace jiminy flexibleJointsNames_.emplace_back(flexName); } + // Add flexibility armuture-like inertia to the model + for(flexibleJointData_t const & flexibleJoint : mdlOptions_->dynamics.flexibilityConfig) + { + std::string const & frameName = flexibleJoint.frameName; + std::string flexName = frameName + FLEXIBLE_JOINT_SUFFIX; + int32_t jointVelocityIdx; + ::jiminy::getJointVelocityIdx(pncModelFlexibleOrig_, flexName, jointVelocityIdx); + pncModelFlexibleOrig_.rotorInertia.segment<3>(jointVelocityIdx) = flexibleJoint.inertia; + } + // Compute flexible joint indices getJointsModelIdx(pncModelFlexibleOrig_, flexibleJointsNames_, flexibleJointsModelIdx_); @@ -1095,7 +1106,7 @@ namespace jiminy } else { - pncModel_ = pncModelRigidOrig_; + pncModel_ = pncModelOrig_; } for (std::string const & jointName : rigidJointsNames_) @@ -1428,7 +1439,7 @@ namespace jiminy if (returnCode == hresult_t::SUCCESS) { - returnCode = refreshCollisionsProxies(); + returnCode = refreshGeometryProxies(); } if (returnCode == hresult_t::SUCCESS) @@ -1444,7 +1455,7 @@ namespace jiminy return returnCode; } - hresult_t Model::refreshCollisionsProxies(void) + hresult_t Model::refreshGeometryProxies(void) { hresult_t returnCode = hresult_t::SUCCESS; @@ -1456,25 +1467,39 @@ namespace jiminy if (returnCode == hresult_t::SUCCESS) { + // Restore collision and visual models + collisionModel_ = collisionModelOrig_; + visualModel_ = visualModelOrig_; + + // Update joint/frame fix for every geometry objects + if (mdlOptions_->dynamics.enableFlexibleModel) + { + for (auto model : std::array{{&collisionModel_, &visualModel_}}) + { + for (pinocchio::GeometryObject & geom : model->geometryObjects) + { + geom.parentFrame = pncModel_.getFrameId(pncModelOrig_.frames[geom.parentFrame].name); + geom.parentJoint = pncModel_.getJointId(pncModelOrig_.names[geom.parentJoint]); + } + } + } + // Update geometry data object after changing the collision pairs - if (pncCollisionData_.get()) + if (collisionData_.get()) { // No object stored at this point, so created a new one - *pncCollisionData_ = pinocchio::GeometryData(collisionModel_); + *collisionData_ = pinocchio::GeometryData(collisionModel_); } else { - /* Use copy assignment to avoid changing memory pointers, to - avoid dangling reference at Python-side. */ - pncCollisionData_ = std::make_unique(collisionModel_); + /* Use copy assignment to avoid changing memory pointers, which would + result in dangling reference at Python-side. */ + collisionData_ = std::make_unique(collisionModel_); } - pinocchio::updateGeometryPlacements(pncModel_, - pncData_, - collisionModel_, - *pncCollisionData_); + pinocchio::updateGeometryPlacements(pncModel_, pncData_, collisionModel_, *collisionData_); // Set the max number of contact points per collision pairs - for (hpp::fcl::CollisionRequest & collisionRequest : pncCollisionData_->collisionRequests) + for (hpp::fcl::CollisionRequest & collisionRequest : collisionData_->collisionRequests) { collisionRequest.num_max_contacts = mdlOptions_->collisions.maxContactPointsPerBody; } @@ -1653,7 +1678,6 @@ namespace jiminy bool_t const & enableFlexibleModel = boost::get(dynOptionsHolder.at("enableFlexibleModel")); flexibilityConfig_t const & flexibilityConfig = boost::get(dynOptionsHolder.at("flexibilityConfig")); - if (mdlOptions_ && (flexibilityConfig.size() != mdlOptions_->dynamics.flexibilityConfig.size() || !std::equal(flexibilityConfig.begin(), @@ -1683,11 +1707,11 @@ namespace jiminy // Check that the model randomization parameters are valid configHolder_t & dynOptionsHolder = boost::get(modelOptions.at("dynamics")); - for (std::string const & field : std::vector{ - "inertiaBodiesBiasStd", - "massBodiesBiasStd", - "centerOfMassPositionBodiesBiasStd", - "relativePositionBodiesBiasStd"}) + for (auto const & field : std::array{{ + "inertiaBodiesBiasStd", + "massBodiesBiasStd", + "centerOfMassPositionBodiesBiasStd", + "relativePositionBodiesBiasStd"}}) { float64_t const & value = boost::get(dynOptionsHolder.at(field)); if (0.9 < value || value < 0.0) @@ -1721,8 +1745,8 @@ namespace jiminy } else if (isCollisionDataInvalid) { - // Update the collision data - refreshCollisionsProxies(); + // Update the visual and collision data + refreshGeometryProxies(); } return hresult_t::SUCCESS; @@ -1740,7 +1764,7 @@ namespace jiminy std::string const & Model::getName(void) const { - return pncModelRigidOrig_.name; + return pncModelOrig_.name; } std::string const & Model::getUrdfPath(void) const @@ -1762,7 +1786,7 @@ namespace jiminy vectorN_t & qFlex) const { // Define some proxies - uint32_t const & nqRigid = pncModelRigidOrig_.nq; + uint32_t const & nqRigid = pncModelOrig_.nq; // Check the size of the input state if (qRigid.size() != nqRigid) @@ -1777,13 +1801,13 @@ namespace jiminy // Compute the flexible state based on the rigid state int32_t idxRigid = 0; int32_t idxFlex = 0; - for (; idxRigid < pncModelRigidOrig_.njoints; ++idxFlex) + for (; idxRigid < pncModelOrig_.njoints; ++idxFlex) { - std::string const & jointRigidName = pncModelRigidOrig_.names[idxRigid]; + std::string const & jointRigidName = pncModelOrig_.names[idxRigid]; std::string const & jointFlexName = pncModelFlexibleOrig_.names[idxFlex]; if (jointRigidName == jointFlexName) { - auto const & jointRigid = pncModelRigidOrig_.joints[idxRigid]; + auto const & jointRigid = pncModelOrig_.joints[idxRigid]; auto const & jointFlex = pncModelFlexibleOrig_.joints[idxFlex]; if (jointRigid.idx_q() >= 0) { @@ -1811,18 +1835,18 @@ namespace jiminy } // Initialize the rigid state - qRigid = pinocchio::neutral(pncModelRigidOrig_); + qRigid = pinocchio::neutral(pncModelOrig_); // Compute the rigid state based on the flexible state int32_t idxRigid = 0; int32_t idxFlex = 0; for (; idxFlex < pncModelFlexibleOrig_.njoints; ++idxFlex) { - std::string const & jointRigidName = pncModelRigidOrig_.names[idxRigid]; + std::string const & jointRigidName = pncModelOrig_.names[idxRigid]; std::string const & jointFlexName = pncModelFlexibleOrig_.names[idxFlex]; if (jointRigidName == jointFlexName) { - auto const & jointRigid = pncModelRigidOrig_.joints[idxRigid]; + auto const & jointRigid = pncModelOrig_.joints[idxRigid]; auto const & jointFlex = pncModelFlexibleOrig_.joints[idxFlex]; if (jointRigid.idx_q() >= 0) { @@ -1840,7 +1864,7 @@ namespace jiminy vectorN_t & vFlex) const { // Define some proxies - uint32_t const & nvRigid = pncModelRigidOrig_.nv; + uint32_t const & nvRigid = pncModelOrig_.nv; uint32_t const & nvFlex = pncModelFlexibleOrig_.nv; // Check the size of the input state @@ -1856,13 +1880,13 @@ namespace jiminy // Compute the flexible state based on the rigid state int32_t idxRigid = 0; int32_t idxFlex = 0; - for (; idxRigid < pncModelRigidOrig_.njoints; ++idxFlex) + for (; idxRigid < pncModelOrig_.njoints; ++idxFlex) { - std::string const & jointRigidName = pncModelRigidOrig_.names[idxRigid]; + std::string const & jointRigidName = pncModelOrig_.names[idxRigid]; std::string const & jointFlexName = pncModelFlexibleOrig_.names[idxFlex]; if (jointRigidName == jointFlexName) { - auto const & jointRigid = pncModelRigidOrig_.joints[idxRigid]; + auto const & jointRigid = pncModelOrig_.joints[idxRigid]; auto const & jointFlex = pncModelFlexibleOrig_.joints[idxFlex]; if (jointRigid.idx_q() >= 0) { @@ -1880,7 +1904,7 @@ namespace jiminy vectorN_t & vRigid) const { // Define some proxies - uint32_t const & nvRigid = pncModelRigidOrig_.nv; + uint32_t const & nvRigid = pncModelOrig_.nv; uint32_t const & nvFlex = pncModelFlexibleOrig_.nv; // Check the size of the input state @@ -1898,11 +1922,11 @@ namespace jiminy int32_t idxFlex = 0; for (; idxFlex < pncModelFlexibleOrig_.njoints; ++idxRigid, ++idxFlex) { - std::string const & jointRigidName = pncModelRigidOrig_.names[idxRigid]; + std::string const & jointRigidName = pncModelOrig_.names[idxRigid]; std::string const & jointFlexName = pncModelFlexibleOrig_.names[idxFlex]; if (jointRigidName == jointFlexName) { - auto const & jointRigid = pncModelRigidOrig_.joints[idxRigid]; + auto const & jointRigid = pncModelOrig_.joints[idxRigid]; auto const & jointFlex = pncModelFlexibleOrig_.joints[idxFlex]; if (jointRigid.idx_q() >= 0) { diff --git a/core/src/robot/Robot.cc b/core/src/robot/Robot.cc index 6975308d6..b67199dc6 100644 --- a/core/src/robot/Robot.cc +++ b/core/src/robot/Robot.cc @@ -191,8 +191,8 @@ namespace jiminy int32_t jointVelocityIdx; ::jiminy::getJointVelocityIdx(robot->pncModel_, jointName, jointVelocityIdx); robot->pncModel_.rotorInertia[jointVelocityIdx] = armature; - ::jiminy::getJointVelocityIdx(robot->pncModelRigidOrig_, jointName, jointVelocityIdx); - robot->pncModelRigidOrig_.rotorInertia[jointVelocityIdx] = armature; + ::jiminy::getJointVelocityIdx(robot->pncModelOrig_, jointName, jointVelocityIdx); + robot->pncModelOrig_.rotorInertia[jointVelocityIdx] = armature; return hresult_t::SUCCESS; }; @@ -859,8 +859,8 @@ namespace jiminy } // Propagate the user-defined motor inertia at Pinocchio model level - pncModelRigidOrig_.rotorInertia = getArmatures(); - pncModel_.rotorInertia = pncModelRigidOrig_.rotorInertia; + pncModelOrig_.rotorInertia = getArmatures(); + pncModel_.rotorInertia = pncModelOrig_.rotorInertia; return returnCode; } diff --git a/core/src/utilities/Json.cc b/core/src/utilities/Json.cc index f6cd58974..bc35b3f78 100644 --- a/core/src/utilities/Json.cc +++ b/core/src/utilities/Json.cc @@ -13,40 +13,6 @@ namespace jiminy { // *************** Convertion to JSON utilities ***************** - template<> - Json::Value convertToJson(vectorN_t const & value) - { - Json::Value row(Json::arrayValue); - for (Eigen::Index i = 0; i < value.size(); ++i) - { - row.append(value[i]); - } - return row; - } - - template<> - Json::Value convertToJson(matrixN_t const & value) - { - Json::Value mat(Json::arrayValue); - if (value.rows() > 0) - { - for (Eigen::Index i = 0; i Json::Value convertToJson(flexibleJointData_t const & value) { @@ -54,6 +20,7 @@ namespace jiminy flex["frameName"] = convertToJson(value.frameName); flex["stiffness"] = convertToJson(value.stiffness); flex["damping"] = convertToJson(value.damping); + flex["inertia"] = convertToJson(value.inertia); return flex; } @@ -196,7 +163,8 @@ namespace jiminy return { convertFromJson(value["frameName"]), convertFromJson(value["stiffness"]), - convertFromJson(value["damping"]) + convertFromJson(value["damping"]), + convertFromJson(value["inertia"]) }; } diff --git a/core/src/utilities/Pinocchio.cc b/core/src/utilities/Pinocchio.cc index 82de56a4f..de503f91d 100644 --- a/core/src/utilities/Pinocchio.cc +++ b/core/src/utilities/Pinocchio.cc @@ -691,12 +691,12 @@ namespace jiminy } /* Add weightless body. - In practice having a zero inertia makes some of pinocchio algorithm crash, - so we set a very small value instead: 1.0g. Anything below that creates - numerical instability. */ + In practice having a zero inertia makes some of pinocchio algorithm + crash, so we set a very small value instead: 1g. Anything below + creates numerical instability. */ float64_t const mass = 1.0e-3; float64_t const lengthSemiAxis = 1.0; - pinocchio::Inertia inertia = pinocchio::Inertia::FromEllipsoid( + pinocchio::Inertia const inertia = pinocchio::Inertia::FromEllipsoid( mass, lengthSemiAxis, lengthSemiAxis, lengthSemiAxis); modelInOut.appendBodyToJoint(newJointIdx, inertia, SE3::Identity()); diff --git a/python/jiminy_py/src/jiminy_py/viewer/viewer.py b/python/jiminy_py/src/jiminy_py/viewer/viewer.py index 317cd6e20..1e7763937 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/viewer.py +++ b/python/jiminy_py/src/jiminy_py/viewer/viewer.py @@ -721,7 +721,7 @@ def get_dcm_scale() -> Tuple3FType: # Add contact frame markers for frame_name, frame_idx in zip( robot.contact_frames_names, robot.contact_frames_idx): - frame_pose = robot.pinocchio_data.oMf[frame_idx] + frame_pose = self._client.data.oMf[frame_idx] self.add_marker(name='_'.join(("ContactFrame", frame_name)), shape="sphere", color="yellow", @@ -764,31 +764,25 @@ def get_force_pose( joint_idx: int) -> Tuple[Tuple3FType, Tuple4FType]: joint_pose = self._client.data.oMi[joint_idx] joint_position = joint_pose.translation - if self.f_external: - f_ext = self.f_external[joint_idx - 1].linear - joint_rotation = pin.Quaternion(joint_pose.rotation) - frame_rotation = (joint_rotation * pin.Quaternion( - np.array([0.0, 0.0, 1.0]), f_ext)).coeffs() - else: - frame_rotation = pin.Quaternion.Identity().coeffs() + f_ext = self.f_external[joint_idx - 1].linear + joint_rotation = pin.Quaternion(joint_pose.rotation) + frame_rotation = (joint_rotation * pin.Quaternion( + np.array([0.0, 0.0, 1.0]), f_ext)).coeffs() return (joint_position, frame_rotation) def get_force_scale(joint_idx: int) -> Tuple[float, float, float]: - if self.f_external: - f_ext = self.f_external[joint_idx - 1].linear - f_ext_norm = np.linalg.norm(f_ext, 2) - length = min(f_ext_norm / EXTERNAL_FORCE_SCALE, 1.0) - else: - length = 0.0 + f_ext = self.f_external[joint_idx - 1].linear + f_ext_norm = np.linalg.norm(f_ext, 2) + length = min(f_ext_norm / EXTERNAL_FORCE_SCALE, 1.0) return (1.0, 1.0, length) - for i in range(1, pinocchio_model.njoints): - frame_name = self._client.model.names[i] - self.add_marker(name=f"ForceExternal_{frame_name}", + for joint_name in pinocchio_model.names[1:]: + joint_idx = self._client.model.getJointId(joint_name) + self.add_marker(name=f"ForceExternal_{joint_name}", shape="arrow", color="red", - pose=partial(get_force_pose, i), - scale=partial(get_force_scale, i), + pose=partial(get_force_pose, joint_idx), + scale=partial(get_force_scale, joint_idx), remove_if_exists=True, auto_refresh=False, radius=0.015, diff --git a/python/jiminy_pywrap/include/jiminy/python/Utilities.h b/python/jiminy_pywrap/include/jiminy/python/Utilities.h index b964a64f9..dbd1024e1 100644 --- a/python/jiminy_pywrap/include/jiminy/python/Utilities.h +++ b/python/jiminy_pywrap/include/jiminy/python/Utilities.h @@ -316,6 +316,7 @@ namespace python flexibilityJointDataPy["frameName"] = flexibleJointData.frameName; flexibilityJointDataPy["stiffness"] = flexibleJointData.stiffness; flexibilityJointDataPy["damping"] = flexibleJointData.damping; + flexibilityJointDataPy["inertia"] = flexibleJointData.inertia; return flexibilityJointDataPy; } @@ -606,6 +607,7 @@ namespace python flexData.frameName = convertFromPython(flexDataPy["frameName"]); flexData.stiffness = convertFromPython(flexDataPy["stiffness"]); flexData.damping = convertFromPython(flexDataPy["damping"]); + flexData.inertia = convertFromPython(flexDataPy["inertia"]); return flexData; } diff --git a/python/jiminy_pywrap/src/Robot.cc b/python/jiminy_pywrap/src/Robot.cc index 1edaf7e60..f0e704992 100644 --- a/python/jiminy_pywrap/src/Robot.cc +++ b/python/jiminy_pywrap/src/Robot.cc @@ -78,7 +78,7 @@ namespace python .def("get_rigid_velocity_from_flexible", &PyModelVisitor::getRigidVelocityFromFlexible, (bp::arg("self"), "flexible_velocity")) - .add_property("pinocchio_model_th", bp::make_getter(&Model::pncModelRigidOrig_, + .add_property("pinocchio_model_th", bp::make_getter(&Model::pncModelOrig_, bp::return_internal_reference<>())) .add_property("pinocchio_model", bp::make_getter(&Model::pncModel_, bp::return_internal_reference<>())) @@ -154,7 +154,7 @@ namespace python static pinocchio::GeometryData & getCollisionData(Model & self) { - return *(self.pncCollisionData_); + return *(self.collisionData_); } static hresult_t addCollisionBodies(Model & self, diff --git a/unit_py/test_simple_pendulum.py b/unit_py/test_simple_pendulum.py index 3380062b3..d8a38b7a5 100644 --- a/unit_py/test_simple_pendulum.py +++ b/unit_py/test_simple_pendulum.py @@ -509,7 +509,8 @@ def test_flexibility_armature(self): model_options["dynamics"]["flexibilityConfig"] = [{ 'frameName': "PendulumJoint", 'stiffness': k * np.ones(3), - 'damping': nu * np.ones(3) + 'damping': nu * np.ones(3), + 'inertia': np.zeros(3) }] self.robot.set_model_options(model_options)