From e48cbabe1ec7fc8aa1b25c7f423496f7eb6616e8 Mon Sep 17 00:00:00 2001 From: Benjamin Navarro Date: Wed, 29 Apr 2020 17:41:57 +0200 Subject: [PATCH 01/15] [RBDyn] Add possibility to compute a jacobian with given reference body The original implementation was made by Sonny Tarbouriech (@sonny-tarbouriech) --- src/RBDyn/Jacobian.cpp | 99 ++++++++++++++++++++++++++++++++++--- src/RBDyn/RBDyn/Jacobian.h | 20 ++++++++ src/RBDyn/RBDyn/MultiBody.h | 8 +++ 3 files changed, 121 insertions(+), 6 deletions(-) diff --git a/src/RBDyn/Jacobian.cpp b/src/RBDyn/Jacobian.cpp index 3a2a782c..64d808a1 100644 --- a/src/RBDyn/Jacobian.cpp +++ b/src/RBDyn/Jacobian.cpp @@ -21,13 +21,17 @@ Jacobian::Jacobian() {} Jacobian::Jacobian(const MultiBody & mb, const std::string & bodyName, const Eigen::Vector3d & point) : jointsPath_(), point_(point), jac_(), jacDot_() { - int index = mb.sBodyIndexByName(bodyName); + body_index_ = mb.sBodyIndexByName(bodyName); + ref_index_ = -1; + + int index = body_index_; int dof = 0; while(index != -1) { jointsPath_.insert(jointsPath_.begin(), index); dof += mb.joint(index).dof(); + reverseJoints_.insert(reverseJoints_.begin(), false); index = mb.parent(index); } @@ -36,6 +40,62 @@ Jacobian::Jacobian(const MultiBody & mb, const std::string & bodyName, const Eig jacDot_.resize(6, dof); } +Jacobian::Jacobian(const MultiBody & mb, + const std::string & bodyName, + const std::string & refBodyName, + const Eigen::Vector3d & point) +: jointsPath_(), point_(point), jac_(), jacDot_() +{ + body_index_ = mb.sBodyIndexByName(bodyName); + ref_index_ = mb.sBodyIndexByName(refBodyName); + + int index = body_index_; + + int dof = 0; + bool refindexFound = false; + while(not refindexFound && index != -1) + { + jointsPath_.insert(jointsPath_.begin(), index); + dof += mb.joint(index).dof(); + reverseJoints_.insert(reverseJoints_.begin(), false); + + if(index == ref_index_) + refindexFound = true; + else + index = mb.parent(index); + } + + // The two bodies don't belong to the same branch -> start from ref body to the root of the tree + if(index != ref_index_) + { + index = ref_index_; + int count = 0; + // Add joints until reaching the common node + do + { + jointsPath_.insert(jointsPath_.begin() + count, index); + dof += mb.joint(index).dof(); + reverseJoints_.insert(reverseJoints_.begin(), true); + + index = mb.parent(index); + count++; + } while(std::find(jointsPath_.begin(), jointsPath_.end(), index) == jointsPath_.end()); + + // Delete common joints previously added + bool commonJoints = true; + while(commonJoints) + { + if(jointsPath_[count] == index) commonJoints = false; + dof -= mb.joint(jointsPath_[count]).dof(); + jointsPath_.erase(jointsPath_.begin() + count); + reverseJoints_.erase(reverseJoints_.begin() + count); + } + } + + jac_.resize(6, dof); + jacDot_.resize(6, dof); +} + MultiBody Jacobian::subMultiBody(const MultiBody & mb) const { std::vector bodies; @@ -64,6 +124,16 @@ MultiBody Jacobian::subMultiBody(const MultiBody & mb) const std::move(Xt)); } +std::vector Jacobian::dofPath(const MultiBody & mb) const +{ + std::vector dof_path; + for(size_t i = 0; i < jointsPath_.size(); ++i) + { + for(size_t j = 0; j < mb.joint(jointsPath_[i]).dof(); ++j) dof_path.push_back(jointsPath_[i]); + } + return dof_path; +} + /// private implementation of the Jacobian computation /// We use the Transform template allow Eigen3 to /// remove the Matrix3d from the computation. @@ -72,7 +142,9 @@ static inline const Eigen::MatrixXd & jacobian_(const MultiBody & mb, const MultiBodyConfig & mbc, const Transform & Trans_0_p, const std::vector & jointsPath, - Eigen::MatrixXd & jac) + const std::vector & reverseJoints, + Eigen::MatrixXd & jac, + int ref_index) { const std::vector & joints = mb.joints(); int curJ = 0; @@ -87,11 +159,26 @@ static inline const Eigen::MatrixXd & jacobian_(const MultiBody & mb, for(int dof = 0; dof < joints[i].dof(); ++dof) { - jac.col(curJ + dof).noalias() = (X_i_N * (sva::MotionVecd(mbc.motionSubspace[i].col(dof)))).vector(); + if(not reverseJoints[index]) + { + jac.col(curJ + dof).noalias() = (X_i_N * (sva::MotionVecd(mbc.motionSubspace[i].col(dof)))).vector(); + } + else + { + // The joint motion is seen from child body to parent body, so has inverted effect + jac.col(curJ + dof).noalias() = (X_i_N * (-sva::MotionVecd(mbc.motionSubspace[i].col(dof)))).vector(); + } } curJ += joints[i].dof(); } + // Change Jacobian base : root of the tree --> reference body + if(ref_index != -1) + { + auto dof_count = jac.cols(); + jac.block(0, 0, 3, dof_count) = mbc.bodyPosW[jointsPath[0]].rotation() * jac.block(0, 0, 3, dof_count); + jac.block(3, 0, 3, dof_count) = mbc.bodyPosW[jointsPath[0]].rotation() * jac.block(3, 0, 3, dof_count); + } return jac; } @@ -100,7 +187,7 @@ const Eigen::MatrixXd & Jacobian::jacobian(const MultiBody & mb, const MultiBodyConfig & mbc, const sva::PTransformd & X_0_p) { - return jacobian_(mb, mbc, X_0_p, jointsPath_, jac_); + return jacobian_(mb, mbc, X_0_p, jointsPath_, reverseJoints_, jac_, ref_index_); } const Eigen::MatrixXd & Jacobian::jacobian(const MultiBody & mb, const MultiBodyConfig & mbc) @@ -109,7 +196,7 @@ const Eigen::MatrixXd & Jacobian::jacobian(const MultiBody & mb, const MultiBody // the transformation must be read {}^0E_p {}^pT_N {}^NX_0 Eigen::Vector3d T_0_Np((point_ * mbc.bodyPosW[N]).translation()); - return jacobian_(mb, mbc, T_0_Np, jointsPath_, jac_); + return jacobian_(mb, mbc, T_0_Np, jointsPath_, reverseJoints_, jac_, ref_index_); } const Eigen::MatrixXd & Jacobian::bodyJacobian(const MultiBody & mb, const MultiBodyConfig & mbc) @@ -117,7 +204,7 @@ const Eigen::MatrixXd & Jacobian::bodyJacobian(const MultiBody & mb, const Multi int N = jointsPath_.back(); sva::PTransformd X_0_Np = point_ * mbc.bodyPosW[N]; - return jacobian_(mb, mbc, X_0_Np, jointsPath_, jac_); + return jacobian_(mb, mbc, X_0_Np, jointsPath_, reverseJoints_, jac_, ref_index_); } const Eigen::MatrixXd & Jacobian::vectorJacobian(const MultiBody & mb, diff --git a/src/RBDyn/RBDyn/Jacobian.h b/src/RBDyn/RBDyn/Jacobian.h index 90b19052..2c7866d1 100644 --- a/src/RBDyn/RBDyn/Jacobian.h +++ b/src/RBDyn/RBDyn/Jacobian.h @@ -49,6 +49,19 @@ class RBDYN_DLLAPI Jacobian */ Jacobian(const MultiBody & mb, const std::string & bodyName, const Eigen::Vector3d & point = Eigen::Vector3d::Zero()); + /** + * Create a jacobian from a reference body to the specified body. + * @param mb Multibody where bodyId is in. + * @param bodyName Specified body. + * @param refBodyName Reference body. + * @param point Point in the body exprimed in body coordinate. + * @throw std::out_of_range If bodyId don't exist. + */ + Jacobian(const MultiBody & mb, + const std::string & bodyName, + const std::string & refBodyName, + const Eigen::Vector3d & point = Eigen::Vector3d::Zero()); + /** * Compute the jacobian at the point/frame specified by X_0_p. * @param mb MultiBody used has model. @@ -307,6 +320,9 @@ class RBDYN_DLLAPI Jacobian return jointsPath_; } + /// @return The dof path vector from the root to the specified body. + std::vector dofPath(const MultiBody & mb) const; + /// @return The number of degree of freedom in the joint path int dof() const { @@ -452,10 +468,14 @@ class RBDYN_DLLAPI Jacobian private: std::vector jointsPath_; + std::vector reverseJoints_; sva::PTransformd point_; Eigen::MatrixXd jac_; Eigen::MatrixXd jacDot_; + + int body_index_; + int ref_index_; }; } // namespace rbd diff --git a/src/RBDyn/RBDyn/MultiBody.h b/src/RBDyn/RBDyn/MultiBody.h index e765a88e..b3a8fe83 100644 --- a/src/RBDyn/RBDyn/MultiBody.h +++ b/src/RBDyn/RBDyn/MultiBody.h @@ -82,6 +82,14 @@ class RBDYN_DLLAPI MultiBody bodies_[num] = b; } + /// @return Index of the root body + int rootBodyIndex() const + { + int index = bodyIndexByName(bodies_[0].name()); + while(parent(index) != -1) index = parent(index); + return index; + } + /// @return Joints of the multibody system. const std::vector & joints() const { From dd1f83d8a470775796349da23993df625bdb7c5f Mon Sep 17 00:00:00 2001 From: Benjamin Navarro Date: Wed, 29 Apr 2020 18:10:13 +0200 Subject: [PATCH 02/15] [RBDyn] remove all warnings and enable Werror --- CMakeLists.txt | 3 - src/RBDyn/CoM.cpp | 79 ++++++++++---------- src/RBDyn/Coriolis.cpp | 24 ++++--- src/RBDyn/EulerIntegration.cpp | 3 +- src/RBDyn/FA.cpp | 7 +- src/RBDyn/FD.cpp | 53 +++++++------- src/RBDyn/FK.cpp | 7 +- src/RBDyn/FV.cpp | 12 ++-- src/RBDyn/ID.cpp | 18 ++--- src/RBDyn/IDIM.cpp | 14 ++-- src/RBDyn/IK.cpp | 6 +- src/RBDyn/IS.cpp | 34 +++++---- src/RBDyn/Jacobian.cpp | 60 ++++++++-------- src/RBDyn/Momentum.cpp | 40 ++++++----- src/RBDyn/MultiBody.cpp | 14 ++-- src/RBDyn/MultiBodyConfig.cpp | 112 +++++++++++++++++------------ src/RBDyn/MultiBodyGraph.cpp | 6 +- src/RBDyn/RBDyn/Body.h | 4 +- src/RBDyn/RBDyn/MultiBody.h | 40 +++++------ src/RBDyn/RBDyn/MultiBodyConfig.h | 6 +- src/RBDyn/RBDyn/VisServo.h | 4 +- src/RBDyn/VisServo.cpp | 2 +- src/parsers/RBDyn/parsers/common.h | 2 +- src/parsers/yaml.cpp | 15 +--- tests/AlgoTest.cpp | 21 +++--- tests/CoMTest.cpp | 68 +++++++++++------- tests/DynamicsTest.cpp | 8 ++- tests/IntegrationTest.cpp | 2 +- tests/JacobianTest.cpp | 58 ++++++++++----- tests/MomentumTest.cpp | 4 +- tests/MultiBodyTest.cpp | 42 ++++++----- 31 files changed, 436 insertions(+), 332 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 45bf1f75..f0800a47 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,9 +19,6 @@ include(cmake/base.cmake) include(cmake/cython/cython.cmake) include(cmake/msvc-specific.cmake) -# Disable -Werror on Unix for now. -set(CXX_DISABLE_WERROR True) - project(RBDyn CXX) option(BENCHMARKS "Generate benchmarks." OFF) diff --git a/src/RBDyn/CoM.cpp b/src/RBDyn/CoM.cpp index d97cfb10..b386f8dc 100644 --- a/src/RBDyn/CoM.cpp +++ b/src/RBDyn/CoM.cpp @@ -21,7 +21,7 @@ Eigen::Vector3d computeCoM(const MultiBody & mb, const MultiBodyConfig & mbc) Vector3d com = Vector3d::Zero(); double totalMass = 0.; - for(int i = 0; i < mb.nrBodies(); ++i) + for(size_t i = 0; i < static_cast(mb.nrBodies()); ++i) { double mass = bodies[i].inertia().mass(); @@ -43,7 +43,7 @@ Eigen::Vector3d computeCoMVelocity(const MultiBody & mb, const MultiBodyConfig & Vector3d comV = Vector3d::Zero(); double totalMass = 0.; - for(int i = 0; i < mb.nrBodies(); ++i) + for(size_t i = 0; i < static_cast(mb.nrBodies()); ++i) { double mass = bodies[i].inertia().mass(); totalMass += mass; @@ -68,7 +68,7 @@ Eigen::Vector3d computeCoMAcceleration(const MultiBody & mb, const MultiBodyConf Vector3d comA = Vector3d::Zero(); double totalMass = 0.; - for(int i = 0; i < mb.nrBodies(); ++i) + for(size_t i = 0; i < static_cast(mb.nrBodies()); ++i) { double mass = bodies[i].inertia().mass(); @@ -120,15 +120,15 @@ Eigen::Vector3d sComputeCoMAcceleration(const MultiBody & mb, const MultiBodyCon CoMJacobianDummy::CoMJacobianDummy() {} CoMJacobianDummy::CoMJacobianDummy(const MultiBody & mb) -: jac_(3, mb.nrDof()), jacDot_(3, mb.nrDof()), jacFull_(3, mb.nrDof()), jacVec_(mb.nrBodies()), totalMass_(0.), - bodiesWeight_(mb.nrBodies(), 1.) +: jac_(3, mb.nrDof()), jacDot_(3, mb.nrDof()), jacFull_(3, mb.nrDof()), jacVec_(static_cast(mb.nrBodies())), + totalMass_(0.), bodiesWeight_(static_cast(mb.nrBodies()), 1.) { init(mb); } CoMJacobianDummy::CoMJacobianDummy(const MultiBody & mb, std::vector weight) -: jac_(3, mb.nrDof()), jacDot_(3, mb.nrDof()), jacFull_(3, mb.nrDof()), jacVec_(mb.nrBodies()), totalMass_(0.), - bodiesWeight_(std::move(weight)) +: jac_(3, mb.nrDof()), jacDot_(3, mb.nrDof()), jacFull_(3, mb.nrDof()), jacVec_(static_cast(mb.nrBodies())), + totalMass_(0.), bodiesWeight_(std::move(weight)) { init(mb); @@ -150,7 +150,7 @@ const Eigen::MatrixXd & CoMJacobianDummy::jacobian(const MultiBody & mb, const M jac_.setZero(); - for(int i = 0; i < mb.nrBodies(); ++i) + for(size_t i = 0; i < static_cast(mb.nrBodies()); ++i) { const MatrixXd & jac = jacVec_[i].jacobian(mb, mbc); jacVec_[i].fullJacobian(mb, jac.block(3, 0, 3, jac.cols()), jacFull_); @@ -171,7 +171,7 @@ const Eigen::MatrixXd & CoMJacobianDummy::jacobianDot(const MultiBody & mb, cons jacDot_.setZero(); - for(int i = 0; i < mb.nrBodies(); ++i) + for(size_t i = 0; i < static_cast(mb.nrBodies()); ++i) { const MatrixXd & jac = jacVec_[i].jacobianDot(mb, mbc); jacVec_[i].fullJacobian(mb, jac.block(3, 0, 3, jac.cols()), jacFull_); @@ -210,7 +210,7 @@ void CoMJacobianDummy::init(const rbd::MultiBody & mb) Vector3d comT(0, 0, 0); if(bodyMass > 0) comT = mb.body(i).inertia().momentum() / bodyMass; - jacVec_[i] = Jacobian(mb, mb.body(i).name(), comT); + jacVec_[static_cast(i)] = Jacobian(mb, mb.body(i).name(), comT); totalMass_ += mb.body(i).inertia().mass(); } } @@ -222,17 +222,19 @@ void CoMJacobianDummy::init(const rbd::MultiBody & mb) CoMJacobian::CoMJacobian() {} CoMJacobian::CoMJacobian(const MultiBody & mb) -: jac_(3, mb.nrDof()), jacDot_(3, mb.nrDof()), bodiesCoeff_(mb.nrBodies()), bodiesCoM_(mb.nrBodies()), - jointsSubBodies_(mb.nrJoints()), bodiesCoMWorld_(mb.nrBodies()), bodiesCoMVelB_(mb.nrBodies()), - normalAcc_(mb.nrJoints()), weight_(mb.nrBodies(), 1.) +: jac_(3, mb.nrDof()), jacDot_(3, mb.nrDof()), bodiesCoeff_(static_cast(mb.nrBodies())), + bodiesCoM_(static_cast(mb.nrBodies())), jointsSubBodies_(static_cast(mb.nrJoints())), + bodiesCoMWorld_(static_cast(mb.nrBodies())), bodiesCoMVelB_(static_cast(mb.nrBodies())), + normalAcc_(static_cast(mb.nrJoints())), weight_(static_cast(mb.nrBodies()), 1.) { init(mb); } CoMJacobian::CoMJacobian(const MultiBody & mb, std::vector weight) -: jac_(3, mb.nrDof()), jacDot_(3, mb.nrDof()), bodiesCoeff_(mb.nrBodies()), bodiesCoM_(mb.nrBodies()), - jointsSubBodies_(mb.nrJoints()), bodiesCoMWorld_(mb.nrBodies()), bodiesCoMVelB_(mb.nrBodies()), - normalAcc_(mb.nrJoints()), weight_(std::move(weight)) +: jac_(3, mb.nrDof()), jacDot_(3, mb.nrDof()), bodiesCoeff_(static_cast(mb.nrBodies())), + bodiesCoM_(static_cast(mb.nrBodies())), jointsSubBodies_(static_cast(mb.nrJoints())), + bodiesCoMWorld_(static_cast(mb.nrBodies())), bodiesCoMVelB_(static_cast(mb.nrBodies())), + normalAcc_(static_cast(mb.nrJoints())), weight_(std::move(weight)) { if(int(weight_.size()) != mb.nrBodies()) { @@ -256,11 +258,11 @@ void CoMJacobian::updateInertialParameters(const MultiBody & mb) for(int i = 0; i < mb.nrBodies(); ++i) { double bodyMass = mb.body(i).inertia().mass(); - bodiesCoeff_[i] = (bodyMass * weight_[i]) / mass; + bodiesCoeff_[static_cast(i)] = (bodyMass * weight_[static_cast(i)]) / mass; if(bodyMass > 0) - bodiesCoM_[i] = sva::PTransformd((mb.body(i).inertia().momentum() / bodyMass).eval()); + bodiesCoM_[static_cast(i)] = sva::PTransformd((mb.body(i).inertia().momentum() / bodyMass).eval()); else - bodiesCoM_[i] = sva::PTransformd::Identity(); + bodiesCoM_[static_cast(i)] = sva::PTransformd::Identity(); } } @@ -282,7 +284,7 @@ const Eigen::MatrixXd & CoMJacobian::jacobian(const MultiBody & mb, const MultiB jac_.setZero(); // we pre compute the CoM position of each bodie in world frame - for(int i = 0; i < mb.nrBodies(); ++i) + for(size_t i = 0; i < static_cast(mb.nrBodies()); ++i) { // the transformation must be read {}^0E_p {}^pT_N {}^NX_0 sva::PTransformd X_0_com_w = bodiesCoM_[i] * mbc.bodyPosW[i]; @@ -290,17 +292,18 @@ const Eigen::MatrixXd & CoMJacobian::jacobian(const MultiBody & mb, const MultiB } int curJ = 0; - for(int i = 0; i < mb.nrJoints(); ++i) + for(size_t i = 0; i < static_cast(mb.nrJoints()); ++i) { std::vector & subBodies = jointsSubBodies_[i]; sva::PTransformd X_i_0 = mbc.bodyPosW[i].inv(); for(int b : subBodies) { - sva::PTransformd X_i_com = bodiesCoMWorld_[b] * X_i_0; + const auto body_index = static_cast(b); + sva::PTransformd X_i_com = bodiesCoMWorld_[body_index] * X_i_0; for(int dof = 0; dof < joints[i].dof(); ++dof) { jac_.col(curJ + dof).noalias() += - (X_i_com.linearMul(sva::MotionVecd(mbc.motionSubspace[i].col(dof)))) * bodiesCoeff_[b]; + (X_i_com.linearMul(sva::MotionVecd(mbc.motionSubspace[i].col(dof)))) * bodiesCoeff_[body_index]; } } curJ += joints[i].dof(); @@ -316,26 +319,27 @@ const Eigen::MatrixXd & CoMJacobian::jacobianDot(const MultiBody & mb, const Mul jacDot_.setZero(); // we pre compute the CoM position/velocity of each bodie - for(int i = 0; i < mb.nrBodies(); ++i) + for(size_t i = 0; i < static_cast(mb.nrBodies()); ++i) { bodiesCoMWorld_[i] = bodiesCoM_[i] * mbc.bodyPosW[i]; bodiesCoMVelB_[i] = bodiesCoM_[i] * mbc.bodyVelB[i]; } int curJ = 0; - for(int i = 0; i < mb.nrJoints(); ++i) + for(size_t i = 0; i < static_cast(mb.nrJoints()); ++i) { std::vector & subBodies = jointsSubBodies_[i]; sva::PTransformd X_i_0 = mbc.bodyPosW[i].inv(); for(int b : subBodies) { - sva::PTransformd X_i_com = bodiesCoMWorld_[b] * X_i_0; - sva::PTransformd E_b_0(Eigen::Matrix3d(mbc.bodyPosW[b].rotation().transpose())); + const auto body_index = static_cast(b); + sva::PTransformd X_i_com = bodiesCoMWorld_[body_index] * X_i_0; + sva::PTransformd E_b_0(Eigen::Matrix3d(mbc.bodyPosW[body_index].rotation().transpose())); // angular velocity of rotation N to O - sva::MotionVecd E_Vb(mbc.bodyVelW[b].angular(), Eigen::Vector3d::Zero()); - sva::MotionVecd X_Vcom_i_com = X_i_com * mbc.bodyVelB[i] - bodiesCoMVelB_[b]; + sva::MotionVecd E_Vb(mbc.bodyVelW[body_index].angular(), Eigen::Vector3d::Zero()); + sva::MotionVecd X_Vcom_i_com = X_i_com * mbc.bodyVelB[i] - bodiesCoMVelB_[body_index]; for(int dof = 0; dof < joints[i].dof(); ++dof) { @@ -346,7 +350,7 @@ const Eigen::MatrixXd & CoMJacobian::jacobianDot(const MultiBody & mb, const Mul // X_i_com_d = (Vi - Vcom)_com x X_i_com jacDot_.col(curJ + dof).noalias() += ((E_Vb.cross(E_b_0 * X_i_com * S_ij)).linear() + (E_b_0 * X_Vcom_i_com.cross(X_i_com * S_ij)).linear()) - * bodiesCoeff_[b]; + * bodiesCoeff_[body_index]; } } curJ += joints[i].dof(); @@ -358,7 +362,7 @@ const Eigen::MatrixXd & CoMJacobian::jacobianDot(const MultiBody & mb, const Mul Eigen::Vector3d CoMJacobian::velocity(const MultiBody & mb, const MultiBodyConfig & mbc) const { Eigen::Vector3d comV = Eigen::Vector3d::Zero(); - for(int i = 0; i < mb.nrBodies(); ++i) + for(size_t i = 0; i < static_cast(mb.nrBodies()); ++i) { const Eigen::Vector3d & comT = bodiesCoM_[i].translation(); @@ -376,16 +380,19 @@ Eigen::Vector3d CoMJacobian::normalAcceleration(const MultiBody & mb, const Mult const std::vector & pred = mb.predecessors(); const std::vector & succ = mb.successors(); - for(int i = 0; i < mb.nrJoints(); ++i) + for(size_t i = 0; i < static_cast(mb.nrJoints()); ++i) { const sva::PTransformd & X_p_i = mbc.parentToSon[i]; const sva::MotionVecd & vj_i = mbc.jointVelocity[i]; const sva::MotionVecd & vb_i = mbc.bodyVelB[i]; + const auto pred_index = static_cast(pred[i]); + const auto succ_index = static_cast(succ[i]); + if(pred[i] != -1) - normalAcc_[succ[i]] = X_p_i * normalAcc_[pred[i]] + vb_i.cross(vj_i); + normalAcc_[succ_index] = X_p_i * normalAcc_[pred_index] + vb_i.cross(vj_i); else - normalAcc_[succ[i]] = vb_i.cross(vj_i); + normalAcc_[succ_index] = vb_i.cross(vj_i); } return normalAcceleration(mb, mbc, normalAcc_); @@ -396,7 +403,7 @@ Eigen::Vector3d CoMJacobian::normalAcceleration(const MultiBody & mb, const std::vector & normalAccB) const { Eigen::Vector3d comA(Eigen::Vector3d::Zero()); - for(int i = 0; i < mb.nrBodies(); ++i) + for(size_t i = 0; i < static_cast(mb.nrBodies()); ++i) { const Eigen::Vector3d & comT = bodiesCoM_[i].translation(); @@ -510,7 +517,7 @@ void CoMJacobian::init(const MultiBody & mb) for(int i = 0; i < mb.nrJoints(); ++i) { - std::vector & subBodies = jointsSubBodies_[i]; + std::vector & subBodies = jointsSubBodies_[static_cast(i)]; jointBodiesSuccessors(mb, i, subBodies); } } diff --git a/src/RBDyn/Coriolis.cpp b/src/RBDyn/Coriolis.cpp index 4b81d485..c2dfd8cd 100644 --- a/src/RBDyn/Coriolis.cpp +++ b/src/RBDyn/Coriolis.cpp @@ -42,11 +42,14 @@ const Eigen::MatrixXd & Coriolis::coriolis(const rbd::MultiBody & mb, const rbd: for(int i = 0; i < mb.nrBodies(); ++i) { - const auto & jac = jacs_[i].jacobian(mb, mbc); - const auto & jacDot = jacs_[i].jacobianDot(mb, mbc); + const auto body_sindex = static_cast(i); + const auto body_uindex = static_cast(i); - rot = mbc.bodyPosW[i].rotation().transpose(); - rDot.noalias() = sva::vector3ToCrossMatrix(mbc.bodyVelW[i].angular()) * rot; + const auto & jac = jacs_[body_uindex].jacobian(mb, mbc); + const auto & jacDot = jacs_[body_uindex].jacobianDot(mb, mbc); + + rot = mbc.bodyPosW[body_uindex].rotation().transpose(); + rDot.noalias() = sva::vector3ToCrossMatrix(mbc.bodyVelW[body_uindex].angular()) * rot; auto jvi = jac.bottomRows<3>(); auto jDvi = jacDot.bottomRows<3>(); @@ -54,10 +57,10 @@ const Eigen::MatrixXd & Coriolis::coriolis(const rbd::MultiBody & mb, const rbd: auto jwi = jac.topRows<3>(); auto jDwi = jacDot.topRows<3>(); - double mass = mb.body(i).inertia().mass(); - inertia = mb.body(i).inertia().inertia() - - sva::vector3ToCrossMatrix(mass * jacs_[i].point()) - * sva::vector3ToCrossMatrix(jacs_[i].point()).transpose(); + double mass = mb.body(body_sindex).inertia().mass(); + inertia = mb.body(body_sindex).inertia().inertia() + - sva::vector3ToCrossMatrix(mass * jacs_[body_uindex].point()) + * sva::vector3ToCrossMatrix(jacs_[body_uindex].point()).transpose(); Eigen::Matrix3d ir = inertia * rot.transpose(); @@ -65,10 +68,11 @@ const Eigen::MatrixXd & Coriolis::coriolis(const rbd::MultiBody & mb, const rbd: * + J_{w_i}^T R_i I_i R_i^T \dot{J}_{w_i} * + J_{w_i}^T \dot{R}_i I_i R_i^T J_{w_i} */ - res_.topLeftCorner(jacs_[i].dof(), jacs_[i].dof()).noalias() = + res_.topLeftCorner(jacs_[body_uindex].dof(), jacs_[body_uindex].dof()).noalias() = mass * jvi.transpose() * jDvi + jwi.transpose() * (rot * ir * jDwi + rDot * ir * jwi); - jacs_[i].expandAdd(compactPaths_[i], res_.topLeftCorner(jacs_[i].dof(), jacs_[i].dof()), coriolis_); + jacs_[body_uindex].expandAdd(compactPaths_[body_uindex], + res_.topLeftCorner(jacs_[body_uindex].dof(), jacs_[body_uindex].dof()), coriolis_); } return coriolis_; diff --git a/src/RBDyn/EulerIntegration.cpp b/src/RBDyn/EulerIntegration.cpp index 34c38124..2b9ec589 100644 --- a/src/RBDyn/EulerIntegration.cpp +++ b/src/RBDyn/EulerIntegration.cpp @@ -89,6 +89,7 @@ void eulerJointIntegration(Joint::Type type, // don't break, we go in spherical } /// @todo manage reverse joint + /* fallthrough */ case Joint::Spherical: { Eigen::Quaterniond qi(q[0], q[1], q[2], q[3]); @@ -125,7 +126,7 @@ void eulerIntegration(const MultiBody & mb, MultiBodyConfig & mbc, double step) for(std::size_t i = 0; i < joints.size(); ++i) { eulerJointIntegration(joints[i].type(), mbc.alpha[i], mbc.alphaD[i], step, mbc.q[i]); - for(int j = 0; j < joints[i].dof(); ++j) + for(size_t j = 0; j < static_cast(joints[i].dof()); ++j) { mbc.alpha[i][j] += mbc.alphaD[i][j] * step; } diff --git a/src/RBDyn/FA.cpp b/src/RBDyn/FA.cpp index 2457ca37..6ad1623c 100644 --- a/src/RBDyn/FA.cpp +++ b/src/RBDyn/FA.cpp @@ -22,6 +22,9 @@ void forwardAcceleration(const MultiBody & mb, MultiBodyConfig & mbc, const sva: for(std::size_t i = 0; i < joints.size(); ++i) { + const auto pred_index = static_cast(pred[i]); + const auto succ_index = static_cast(succ[i]); + const sva::PTransformd & X_p_i = mbc.parentToSon[i]; const sva::MotionVecd & vj_i = mbc.jointVelocity[i]; @@ -30,9 +33,9 @@ void forwardAcceleration(const MultiBody & mb, MultiBodyConfig & mbc, const sva: const sva::MotionVecd & vb_i = mbc.bodyVelB[i]; if(pred[i] != -1) - mbc.bodyAccB[succ[i]] = X_p_i * mbc.bodyAccB[pred[i]] + ai_tan + vb_i.cross(vj_i); + mbc.bodyAccB[succ_index] = X_p_i * mbc.bodyAccB[pred_index] + ai_tan + vb_i.cross(vj_i); else - mbc.bodyAccB[succ[i]] = X_p_i * A_0 + ai_tan + vb_i.cross(vj_i); + mbc.bodyAccB[succ_index] = X_p_i * A_0 + ai_tan + vb_i.cross(vj_i); } } diff --git a/src/RBDyn/FD.cpp b/src/RBDyn/FD.cpp index 595e8a26..cad2df40 100644 --- a/src/RBDyn/FD.cpp +++ b/src/RBDyn/FD.cpp @@ -14,14 +14,17 @@ namespace rbd { ForwardDynamics::ForwardDynamics(const MultiBody & mb) -: H_(mb.nrDof(), mb.nrDof()), C_(mb.nrDof()), I_st_(mb.nrBodies()), F_(mb.nrJoints()), acc_(mb.nrBodies()), - f_(mb.nrBodies()), tmpFd_(mb.nrDof()), dofPos_(mb.nrJoints()), ldlt_(mb.nrDof()) +: H_(mb.nrDof(), mb.nrDof()), C_(mb.nrDof()), I_st_(static_cast(mb.nrBodies())), + F_(static_cast(mb.nrJoints())), acc_(static_cast(mb.nrBodies())), + f_(static_cast(mb.nrBodies())), tmpFd_(mb.nrDof()), dofPos_(static_cast(mb.nrJoints())), + ldlt_(mb.nrDof()) { int dofP = 0; for(int i = 0; i < mb.nrJoints(); ++i) { - F_[i].resize(6, mb.joint(i).dof()); - dofPos_[i] = dofP; + const auto ui = static_cast(i); + F_[ui].resize(6, mb.joint(i).dof()); + dofPos_[ui] = dofP; dofP += mb.joint(i).dof(); } } @@ -52,37 +55,38 @@ void ForwardDynamics::computeH(const MultiBody & mb, const MultiBodyConfig & mbc for(int i = static_cast(bodies.size()) - 1; i >= 0; --i) { - if(pred[i] != -1) + const auto ui = static_cast(i); + if(pred[ui] != -1) { - const sva::PTransformd & X_p_i = mbc.parentToSon[i]; - I_st_[pred[i]] += X_p_i.transMul(I_st_[i]); + const sva::PTransformd & X_p_i = mbc.parentToSon[ui]; + I_st_[static_cast(pred[ui])] += X_p_i.transMul(I_st_[ui]); } - for(int dof = 0; dof < joints[i].dof(); ++dof) + for(int dof = 0; dof < joints[ui].dof(); ++dof) { - F_[i].col(dof).noalias() = (I_st_[i] * sva::MotionVecd(mbc.motionSubspace[i].col(dof))).vector(); + F_[ui].col(dof).noalias() = (I_st_[ui] * sva::MotionVecd(mbc.motionSubspace[ui].col(dof))).vector(); } - H_.block(dofPos_[i], dofPos_[i], joints[i].dof(), joints[i].dof()).noalias() = - mbc.motionSubspace[i].transpose() * F_[i]; + H_.block(dofPos_[ui], dofPos_[ui], joints[ui].dof(), joints[ui].dof()).noalias() = + mbc.motionSubspace[ui].transpose() * F_[ui]; - int j = i; + size_t j = ui; while(pred[j] != -1) { const sva::PTransformd & X_p_j = mbc.parentToSon[j]; - for(int dof = 0; dof < joints[i].dof(); ++dof) + for(int dof = 0; dof < joints[ui].dof(); ++dof) { - F_[i].col(dof) = X_p_j.transMul(sva::ForceVecd(F_[i].col(dof))).vector(); + F_[ui].col(dof) = X_p_j.transMul(sva::ForceVecd(F_[ui].col(dof))).vector(); } - j = pred[j]; + j = static_cast(pred[j]); if(joints[j].dof() != 0) { - H_.block(dofPos_[i], dofPos_[j], joints[i].dof(), joints[j].dof()).noalias() = - F_[i].transpose() * mbc.motionSubspace[j]; + H_.block(dofPos_[ui], dofPos_[j], joints[ui].dof(), joints[j].dof()).noalias() = + F_[ui].transpose() * mbc.motionSubspace[j]; - H_.block(dofPos_[j], dofPos_[i], joints[j].dof(), joints[i].dof()).noalias() = - H_.block(dofPos_[i], dofPos_[j], joints[i].dof(), joints[j].dof()).transpose(); + H_.block(dofPos_[j], dofPos_[ui], joints[j].dof(), joints[ui].dof()).noalias() = + H_.block(dofPos_[ui], dofPos_[j], joints[ui].dof(), joints[j].dof()).transpose(); } } } @@ -105,7 +109,7 @@ void ForwardDynamics::computeC(const MultiBody & mb, const MultiBodyConfig & mbc const sva::MotionVecd & vb_i = mbc.bodyVelB[i]; if(pred[i] != -1) - acc_[i] = X_p_i * acc_[pred[i]] + vb_i.cross(vj_i); + acc_[i] = X_p_i * acc_[static_cast(pred[i])] + vb_i.cross(vj_i); else acc_[i] = X_p_i * a_0 + vb_i.cross(vj_i); @@ -115,12 +119,13 @@ void ForwardDynamics::computeC(const MultiBody & mb, const MultiBodyConfig & mbc for(int i = static_cast(bodies.size()) - 1; i >= 0; --i) { - C_.segment(dofPos_[i], joints[i].dof()).noalias() = mbc.motionSubspace[i].transpose() * f_[i].vector(); + const auto ui = static_cast(i); + C_.segment(dofPos_[ui], joints[ui].dof()).noalias() = mbc.motionSubspace[ui].transpose() * f_[ui].vector(); - if(pred[i] != -1) + if(pred[ui] != -1) { - const sva::PTransformd & X_p_i = mbc.parentToSon[i]; - f_[pred[i]] += X_p_i.transMul(f_[i]); + const sva::PTransformd & X_p_i = mbc.parentToSon[ui]; + f_[static_cast(pred[ui])] += X_p_i.transMul(f_[ui]); } } } diff --git a/src/RBDyn/FK.cpp b/src/RBDyn/FK.cpp index 29cc7086..b3af2b0a 100644 --- a/src/RBDyn/FK.cpp +++ b/src/RBDyn/FK.cpp @@ -23,14 +23,17 @@ void forwardKinematics(const MultiBody & mb, MultiBodyConfig & mbc) for(std::size_t i = 0; i < joints.size(); ++i) { + const auto pred_index = static_cast(pred[i]); + const auto succ_index = static_cast(succ[i]); + mbc.jointConfig[i] = joints[i].pose(mbc.q[i]); mbc.parentToSon[i] = mbc.jointConfig[i] * Xt[i]; mbc.motionSubspace[i] = joints[i].motionSubspace(); if(pred[i] != -1) - mbc.bodyPosW[succ[i]] = mbc.parentToSon[i] * mbc.bodyPosW[pred[i]]; + mbc.bodyPosW[succ_index] = mbc.parentToSon[i] * mbc.bodyPosW[pred_index]; else - mbc.bodyPosW[succ[i]] = mbc.parentToSon[i]; + mbc.bodyPosW[succ_index] = mbc.parentToSon[i]; } } diff --git a/src/RBDyn/FV.cpp b/src/RBDyn/FV.cpp index 58c9995f..58bf5b92 100644 --- a/src/RBDyn/FV.cpp +++ b/src/RBDyn/FV.cpp @@ -16,23 +16,27 @@ namespace rbd void forwardVelocity(const MultiBody & mb, MultiBodyConfig & mbc) { + const std::vector & joints = mb.joints(); const std::vector & pred = mb.predecessors(); const std::vector & succ = mb.successors(); for(std::size_t i = 0; i < joints.size(); ++i) { + const auto pred_index = static_cast(pred[i]); + const auto succ_index = static_cast(succ[i]); + const sva::PTransformd & X_p_i = mbc.parentToSon[i]; mbc.jointVelocity[i] = joints[i].motion(mbc.alpha[i]); if(pred[i] != -1) - mbc.bodyVelB[succ[i]] = X_p_i * mbc.bodyVelB[pred[i]] + mbc.jointVelocity[i]; + mbc.bodyVelB[succ_index] = X_p_i * mbc.bodyVelB[pred_index] + mbc.jointVelocity[i]; else - mbc.bodyVelB[succ[i]] = mbc.jointVelocity[i]; + mbc.bodyVelB[succ_index] = mbc.jointVelocity[i]; - sva::PTransformd E_0_i(mbc.bodyPosW[succ[i]].rotation()); - mbc.bodyVelW[succ[i]] = E_0_i.invMul(mbc.bodyVelB[succ[i]]); + sva::PTransformd E_0_i(mbc.bodyPosW[succ_index].rotation()); + mbc.bodyVelW[succ_index] = E_0_i.invMul(mbc.bodyVelB[succ_index]); } } diff --git a/src/RBDyn/ID.cpp b/src/RBDyn/ID.cpp index 10a5b261..e99fc721 100644 --- a/src/RBDyn/ID.cpp +++ b/src/RBDyn/ID.cpp @@ -13,7 +13,7 @@ namespace rbd { -InverseDynamics::InverseDynamics(const MultiBody & mb) : f_(mb.nrBodies()) {} +InverseDynamics::InverseDynamics(const MultiBody & mb) : f_(static_cast(mb.nrBodies())) {} void InverseDynamics::inverseDynamics(const MultiBody & mb, MultiBodyConfig & mbc) { @@ -33,7 +33,7 @@ void InverseDynamics::inverseDynamics(const MultiBody & mb, MultiBodyConfig & mb const sva::MotionVecd & vb_i = mbc.bodyVelB[i]; if(pred[i] != -1) - mbc.bodyAccB[i] = X_p_i * mbc.bodyAccB[pred[i]] + ai_tan + vb_i.cross(vj_i); + mbc.bodyAccB[i] = X_p_i * mbc.bodyAccB[static_cast(pred[i])] + ai_tan + vb_i.cross(vj_i); else mbc.bodyAccB[i] = X_p_i * a_0 + ai_tan + vb_i.cross(vj_i); @@ -46,7 +46,7 @@ void InverseDynamics::inverseDynamics(const MultiBody & mb, MultiBodyConfig & mb void InverseDynamics::inverseDynamicsNoInertia(const MultiBody & mb, MultiBodyConfig & mbc) { - for(int i = 0; i < mb.nrBodies(); ++i) + for(size_t i = 0; i < static_cast(mb.nrBodies()); ++i) { f_[i] = mbc.bodyPosW[i].dualMul(mbc.force[i]); } @@ -105,15 +105,17 @@ void InverseDynamics::computeJointTorques(const MultiBody & mb, MultiBodyConfig for(int i = static_cast(bodies.size()) - 1; i >= 0; --i) { - for(int j = 0; j < joints[i].dof(); ++j) + const auto ui = static_cast(i); + for(int j = 0; j < joints[ui].dof(); ++j) { - mbc.jointTorque[i][j] = mbc.motionSubspace[i].col(j).transpose() * f_[i].vector(); + const auto uj = static_cast(j); + mbc.jointTorque[ui][uj] = mbc.motionSubspace[ui].col(j).transpose() * f_[ui].vector(); } - if(pred[i] != -1) + if(pred[ui] != -1) { - const sva::PTransformd & X_p_i = mbc.parentToSon[i]; - f_[pred[i]] = f_[pred[i]] + X_p_i.transMul(f_[i]); + const sva::PTransformd & X_p_i = mbc.parentToSon[ui]; + f_[static_cast(pred[ui])] = f_[static_cast(pred[ui])] + X_p_i.transMul(f_[ui]); } } } diff --git a/src/RBDyn/IDIM.cpp b/src/RBDyn/IDIM.cpp index c8073614..0596738b 100644 --- a/src/RBDyn/IDIM.cpp +++ b/src/RBDyn/IDIM.cpp @@ -75,10 +75,12 @@ void IDIM::computeY(const MultiBody & mb, const MultiBodyConfig & mbc) Eigen::Matrix bodyFPhi; for(int i = static_cast(bodies.size()) - 1; i >= 0; --i) { - const sva::MotionVecd & vb_i = mbc.bodyVelB[i]; + const auto ui = static_cast(i); + + const sva::MotionVecd & vb_i = mbc.bodyVelB[ui]; Eigen::Matrix vb_i_Phi(IMPhi(vb_i)); - bodyFPhi.noalias() = IMPhi(mbc.bodyAccB[i]); + bodyFPhi.noalias() = IMPhi(mbc.bodyAccB[ui]); // bodyFPhi += vb_i x* IMPhi(vb_i) // is faster to convert each col in a ForceVecd // than using sva::vector6ToCrossDualMatrix @@ -89,9 +91,9 @@ void IDIM::computeY(const MultiBody & mb, const MultiBodyConfig & mbc) int iDofPos = mb.jointPosInDof(i); - Y_.block(iDofPos, i * 10, joints[i].dof(), 10).noalias() = mbc.motionSubspace[i].transpose() * bodyFPhi; + Y_.block(iDofPos, i * 10, joints[ui].dof(), 10).noalias() = mbc.motionSubspace[ui].transpose() * bodyFPhi; - int j = i; + auto j = static_cast(i); while(pred[j] != -1) { const sva::PTransformd & X_p_j = mbc.parentToSon[j]; @@ -102,9 +104,9 @@ void IDIM::computeY(const MultiBody & mb, const MultiBodyConfig & mbc) { bodyFPhi.col(c) = X_p_j.transMul(sva::ForceVecd(bodyFPhi.col(c))).vector(); } - j = pred[j]; + j = static_cast(pred[j]); - int jDofPos = mb.jointPosInDof(j); + int jDofPos = mb.jointPosInDof(static_cast(j)); if(joints[j].dof() != 0) { Y_.block(jDofPos, i * 10, joints[j].dof(), 10).noalias() = mbc.motionSubspace[j].transpose() * bodyFPhi; diff --git a/src/RBDyn/IK.cpp b/src/RBDyn/IK.cpp index 7ed3db5e..6883857d 100644 --- a/src/RBDyn/IK.cpp +++ b/src/RBDyn/IK.cpp @@ -57,15 +57,15 @@ bool InverseKinematics::inverseKinematics(const MultiBody & mb, // non-strict zeros in jacobian can be a problem... jacMat = jacMat.unaryExpr(CwiseRoundOp(-almost_zero_, almost_zero_)); svd_.compute(jacMat, Eigen::ComputeThinU | Eigen::ComputeThinV); - rotErr = sva::rotationError(mbc.bodyPosW[ef_index_].rotation(), ef_target.rotation()); - v << rotErr, ef_target.translation() - mbc.bodyPosW[ef_index_].translation(); + rotErr = sva::rotationError(mbc.bodyPosW[static_cast(ef_index_)].rotation(), ef_target.rotation()); + v << rotErr, ef_target.translation() - mbc.bodyPosW[static_cast(ef_index_)].translation(); converged = v.norm() < threshold_; res = svd_.solve(v); dof = 0; for(auto index : jac_.jointsPath()) { - std::vector & qi = mbc.q[index]; + std::vector & qi = mbc.q[static_cast(index)]; for(auto & qv : qi) { qv += lambda_ * res[dof]; diff --git a/src/RBDyn/IS.cpp b/src/RBDyn/IS.cpp index 2b1a6c7d..d87b839c 100644 --- a/src/RBDyn/IS.cpp +++ b/src/RBDyn/IS.cpp @@ -26,8 +26,9 @@ typedef Matrix Matrix6d; typedef Matrix Vector6d; InverseStatics::InverseStatics(const MultiBody & mb) -: f_(mb.nrBodies()), df_(mb.nrBodies()), jointTorqueDiff_(mb.nrJoints()), jacW_(mb.nrBodies()), fullJac_(6, mb.nrDof()), - jacobianSizeHasBeenSet_(false) +: f_(static_cast(mb.nrBodies())), df_(static_cast(mb.nrBodies())), + jointTorqueDiff_(static_cast(mb.nrJoints())), jacW_(static_cast(mb.nrBodies())), + fullJac_(6, mb.nrDof()), jacobianSizeHasBeenSet_(false) { fullJac_.setZero(); for(size_t i = 0; i < static_cast(mb.nrBodies()); ++i) @@ -94,9 +95,10 @@ void InverseStatics::inverseStatics(const MultiBody & mb, MultiBodyConfig & mbc) // $ mbc.jointTorque[i][j] = mbc.motionSubspace[i].col(j).transpose() * // f_[i].vector(); // - VectorXd::Map(mbc.jointTorque[i].data(), joints[i].dof()) = f_[i].vector().transpose() * mbc.motionSubspace[i]; + const auto ui = static_cast(i); + VectorXd::Map(mbc.jointTorque[ui].data(), joints[ui].dof()) = f_[ui].vector().transpose() * mbc.motionSubspace[ui]; - if(pred[i] != -1) f_[pred[i]] += mbc.parentToSon[i].transMul(f_[i]); + if(pred[ui] != -1) f_[static_cast(pred[ui])] += mbc.parentToSon[ui].transMul(f_[ui]); } } @@ -176,19 +178,21 @@ void InverseStatics::computeTorqueJacobianJoint(const MultiBody & mb, for(int i = static_cast(joints.size()) - 1; i >= 0; --i) { - jointTorqueDiff_[i] = mbc.motionSubspace[i].transpose() * df_[i]; + const auto ui = static_cast(i); + jointTorqueDiff_[ui] = mbc.motionSubspace[ui].transpose() * df_[ui]; - if(pred[i] != -1) + if(pred[ui] != -1) { - Matrix6d transPtS = transMat(mbc.parentToSon[i]); + const auto pred_index = static_cast(pred[ui]); + Matrix6d transPtS = transMat(mbc.parentToSon[ui]); - f_[pred[i]] += mbc.parentToSon[i].transMul(f_[i]); - df_[pred[i]] += transPtS * df_[i]; + f_[pred_index] += mbc.parentToSon[ui].transMul(f_[ui]); + df_[pred_index] += transPtS * df_[ui]; - Matrix3d & R = mbc.jointConfig[i].rotation(); - Vector3d & t = mbc.jointConfig[i].translation(); - Vector3d RfC = R.transpose() * f_[i].couple(); - Vector3d RfF = R.transpose() * f_[i].force(); + Matrix3d & R = mbc.jointConfig[ui].rotation(); + Vector3d & t = mbc.jointConfig[ui].translation(); + Vector3d RfC = R.transpose() * f_[ui].couple(); + Vector3d RfF = R.transpose() * f_[ui].force(); Matrix3d hatRfC = vector3ToCrossMatrix(RfC); Matrix3d hatRfF = vector3ToCrossMatrix(RfF); Matrix6d MJ; @@ -198,8 +202,8 @@ void InverseStatics::computeTorqueJacobianJoint(const MultiBody & mb, MJ.block(3, 0, 3, 3) = hatRfF; MJ.block(3, 3, 3, 3).setZero(); - df_[pred[i]].block(0, mb.jointPosInDof(i), 6, joints[i].dof()) -= - transMat(mb.transforms()[i]) * MJ * mbc.motionSubspace[i]; + df_[pred_index].block(0, mb.jointPosInDof(i), 6, joints[ui].dof()) -= + transMat(mb.transforms()[ui]) * MJ * mbc.motionSubspace[ui]; } } } diff --git a/src/RBDyn/Jacobian.cpp b/src/RBDyn/Jacobian.cpp index 64d808a1..53aa3196 100644 --- a/src/RBDyn/Jacobian.cpp +++ b/src/RBDyn/Jacobian.cpp @@ -53,7 +53,7 @@ Jacobian::Jacobian(const MultiBody & mb, int dof = 0; bool refindexFound = false; - while(not refindexFound && index != -1) + while(!refindexFound && index != -1) { jointsPath_.insert(jointsPath_.begin(), index); dof += mb.joint(index).dof(); @@ -85,8 +85,9 @@ Jacobian::Jacobian(const MultiBody & mb, bool commonJoints = true; while(commonJoints) { - if(jointsPath_[count] == index) commonJoints = false; - dof -= mb.joint(jointsPath_[count]).dof(); + const auto joint_index = jointsPath_[static_cast(count)]; + if(joint_index == index) commonJoints = false; + dof -= mb.joint(joint_index).dof(); jointsPath_.erase(jointsPath_.begin() + count); reverseJoints_.erase(reverseJoints_.begin() + count); } @@ -108,7 +109,7 @@ MultiBody Jacobian::subMultiBody(const MultiBody & mb) const for(int index = 0; index < static_cast(jointsPath_.size()); ++index) { - int i = jointsPath_[index]; + int i = jointsPath_[static_cast(index)]; // body info bodies.push_back(mb.body(i)); parent.push_back(index - 1); @@ -129,7 +130,7 @@ std::vector Jacobian::dofPath(const MultiBody & mb) const std::vector dof_path; for(size_t i = 0; i < jointsPath_.size(); ++i) { - for(size_t j = 0; j < mb.joint(jointsPath_[i]).dof(); ++j) dof_path.push_back(jointsPath_[i]); + for(int j = 0; j < mb.joint(jointsPath_[i]).dof(); ++j) dof_path.push_back(jointsPath_[i]); } return dof_path; } @@ -153,13 +154,13 @@ static inline const Eigen::MatrixXd & jacobian_(const MultiBody & mb, for(std::size_t index = 0; index < jointsPath.size(); ++index) { - int i = jointsPath[index]; + const auto i = static_cast(jointsPath[index]); sva::PTransformd X_i_N = X_0_p * mbc.bodyPosW[i].inv(); for(int dof = 0; dof < joints[i].dof(); ++dof) { - if(not reverseJoints[index]) + if(!reverseJoints[index]) { jac.col(curJ + dof).noalias() = (X_i_N * (sva::MotionVecd(mbc.motionSubspace[i].col(dof)))).vector(); } @@ -176,8 +177,9 @@ static inline const Eigen::MatrixXd & jacobian_(const MultiBody & mb, if(ref_index != -1) { auto dof_count = jac.cols(); - jac.block(0, 0, 3, dof_count) = mbc.bodyPosW[jointsPath[0]].rotation() * jac.block(0, 0, 3, dof_count); - jac.block(3, 0, 3, dof_count) = mbc.bodyPosW[jointsPath[0]].rotation() * jac.block(3, 0, 3, dof_count); + const auto joint_index = static_cast(jointsPath[0]); + jac.block(0, 0, 3, dof_count) = mbc.bodyPosW[joint_index].rotation() * jac.block(0, 0, 3, dof_count); + jac.block(3, 0, 3, dof_count) = mbc.bodyPosW[joint_index].rotation() * jac.block(3, 0, 3, dof_count); } return jac; @@ -192,7 +194,7 @@ const Eigen::MatrixXd & Jacobian::jacobian(const MultiBody & mb, const Eigen::MatrixXd & Jacobian::jacobian(const MultiBody & mb, const MultiBodyConfig & mbc) { - int N = jointsPath_.back(); + auto N = static_cast(jointsPath_.back()); // the transformation must be read {}^0E_p {}^pT_N {}^NX_0 Eigen::Vector3d T_0_Np((point_ * mbc.bodyPosW[N]).translation()); @@ -201,7 +203,7 @@ const Eigen::MatrixXd & Jacobian::jacobian(const MultiBody & mb, const MultiBody const Eigen::MatrixXd & Jacobian::bodyJacobian(const MultiBody & mb, const MultiBodyConfig & mbc) { - int N = jointsPath_.back(); + auto N = static_cast(jointsPath_.back()); sva::PTransformd X_0_Np = point_ * mbc.bodyPosW[N]; return jacobian_(mb, mbc, X_0_Np, jointsPath_, reverseJoints_, jac_, ref_index_); @@ -214,14 +216,14 @@ const Eigen::MatrixXd & Jacobian::vectorJacobian(const MultiBody & mb, const std::vector & joints = mb.joints(); int curJ = 0; - int N = jointsPath_.back(); + const auto N = static_cast(jointsPath_.back()); Eigen::Matrix3d E_N_0(mbc.bodyPosW[N].rotation().transpose()); sva::PTransformd X_Np = point_ * mbc.bodyPosW[N]; sva::PTransformd vec = sva::PTransformd(vector); for(std::size_t index = 0; index < jointsPath_.size(); ++index) { - int i = jointsPath_[index]; + const auto i = static_cast(jointsPath_[index]); sva::PTransformd X_i_N = X_Np * mbc.bodyPosW[i].inv(); Eigen::Matrix3d E_i_0(E_N_0 * X_i_N.rotation()); @@ -250,13 +252,13 @@ const Eigen::MatrixXd & Jacobian::vectorBodyJacobian(const MultiBody & mb, const std::vector & joints = mb.joints(); int curJ = 0; - int N = jointsPath_.back(); + const auto N = static_cast(jointsPath_.back()); sva::PTransformd X_Np = point_ * mbc.bodyPosW[N]; sva::PTransformd vec = sva::PTransformd(vector); for(std::size_t index = 0; index < jointsPath_.size(); ++index) { - int i = jointsPath_[index]; + const auto i = static_cast(jointsPath_[index]); sva::PTransformd X_i_N = X_Np * mbc.bodyPosW[i].inv(); sva::PTransformd X_i_Nv = vec * X_i_N; @@ -283,7 +285,7 @@ const Eigen::MatrixXd & Jacobian::jacobianDot(const MultiBody & mb, const MultiB const std::vector & joints = mb.joints(); int curJ = 0; - int N = jointsPath_.back(); + const auto N = static_cast(jointsPath_.back()); sva::PTransformd X_0_Np = point_ * mbc.bodyPosW[N]; // speed of point in body N @@ -295,7 +297,7 @@ const Eigen::MatrixXd & Jacobian::jacobianDot(const MultiBody & mb, const MultiB for(std::size_t index = 0; index < jointsPath_.size(); ++index) { - int i = jointsPath_[index]; + const auto i = static_cast(jointsPath_[index]); sva::PTransformd X_i_Np = X_0_Np * mbc.bodyPosW[i].inv(); // speed of X_i_N in Np coordinate @@ -323,7 +325,7 @@ const Eigen::MatrixXd & Jacobian::bodyJacobianDot(const MultiBody & mb, const Mu const std::vector & joints = mb.joints(); int curJ = 0; - int N = jointsPath_.back(); + const auto N = static_cast(jointsPath_.back()); sva::PTransformd X_0_Np = point_ * mbc.bodyPosW[N]; // speed of point in body N @@ -331,7 +333,7 @@ const Eigen::MatrixXd & Jacobian::bodyJacobianDot(const MultiBody & mb, const Mu for(std::size_t index = 0; index < jointsPath_.size(); ++index) { - int i = jointsPath_[index]; + const auto i = static_cast(jointsPath_[index]); sva::PTransformd X_i_Np = X_0_Np * mbc.bodyPosW[i].inv(); // speed of X_i_N in Np coordinate @@ -356,13 +358,13 @@ sva::MotionVecd Jacobian::velocity(const MultiBody & /* mb */, const MultiBodyConfig & mbc, const sva::PTransformd & X_b_p) const { - int N = jointsPath_.back(); + const auto N = static_cast(jointsPath_.back()); return X_b_p * mbc.bodyVelB[N]; } sva::MotionVecd Jacobian::velocity(const MultiBody & /* mb */, const MultiBodyConfig & mbc) const { - int N = jointsPath_.back(); + const auto N = static_cast(jointsPath_.back()); // equivalent of E_N_0*X_N_Np sva::PTransformd X_Np_w(mbc.bodyPosW[N].rotation().transpose(), point_.translation()); return X_Np_w * mbc.bodyVelB[N]; @@ -370,7 +372,7 @@ sva::MotionVecd Jacobian::velocity(const MultiBody & /* mb */, const MultiBodyCo sva::MotionVecd Jacobian::bodyVelocity(const MultiBody & /* mb */, const MultiBodyConfig & mbc) const { - int N = jointsPath_.back(); + const auto N = static_cast(jointsPath_.back()); return point_ * mbc.bodyVelB[N]; } @@ -379,7 +381,7 @@ static inline sva::MotionVecd normalAccB_(const rbd::MultiBodyConfig & mbc, cons sva::MotionVecd accel(Eigen::Vector6d::Zero()); for(std::size_t index = 0; index < jointsPath.size(); ++index) { - int i = jointsPath[index]; + const auto i = static_cast(jointsPath[index]); const sva::PTransformd & X_p_i = mbc.parentToSon[i]; const sva::MotionVecd & vj_i = mbc.jointVelocity[i]; const sva::MotionVecd & vb_i = mbc.bodyVelB[i]; @@ -413,21 +415,21 @@ sva::MotionVecd Jacobian::normalAcceleration(const MultiBody & /* mb */, const sva::PTransformd & X_b_p, const sva::MotionVecd & V_b_p) const { - return normalAcceleration(mbc, normalAccB[jointsPath_.back()], X_b_p, V_b_p); + return normalAcceleration(mbc, normalAccB[static_cast(jointsPath_.back())], X_b_p, V_b_p); } sva::MotionVecd Jacobian::normalAcceleration(const MultiBody & /* mb */, const MultiBodyConfig & mbc, const std::vector & normalAccB) const { - return normalAcceleration(mbc, normalAccB[jointsPath_.back()]); + return normalAcceleration(mbc, normalAccB[static_cast(jointsPath_.back())]); } sva::MotionVecd Jacobian::bodyNormalAcceleration(const MultiBody & /* mb */, const MultiBodyConfig & mbc, const std::vector & normalAccB) const { - return bodyNormalAcceleration(mbc, normalAccB[jointsPath_.back()]); + return bodyNormalAcceleration(mbc, normalAccB[static_cast(jointsPath_.back())]); } void Jacobian::translateJacobian(const Eigen::Ref & jac, @@ -435,7 +437,7 @@ void Jacobian::translateJacobian(const Eigen::Ref & jac, const Eigen::Vector3d & point, Eigen::MatrixXd & res) { - int N = jointsPath_.back(); + const auto N = static_cast(jointsPath_.back()); sva::PTransformd E_N_0(Eigen::Matrix3d(mbc.bodyPosW[N].rotation().transpose())); sva::PTransformd t(point); @@ -830,13 +832,13 @@ sva::MotionVecd Jacobian::normalAcceleration(const MultiBodyConfig & mbc, const sva::PTransformd & X_b_p, const sva::MotionVecd & V_b_p) const { - int N = jointsPath_.back(); + const auto N = static_cast(jointsPath_.back()); return X_b_p * bodyNNormalAcc + V_b_p.cross(X_b_p * mbc.bodyVelB[N]); } sva::MotionVecd Jacobian::normalAcceleration(const MultiBodyConfig & mbc, const sva::MotionVecd & bodyNNormalAcc) const { - int N = jointsPath_.back(); + const auto N = static_cast(jointsPath_.back()); // equivalent of E_N_0*X_N_Np sva::PTransformd X_Np_w(mbc.bodyPosW[N].rotation().transpose(), point_.translation()); sva::MotionVecd E_VN(mbc.bodyVelW[N].angular(), Eigen::Vector3d::Zero()); diff --git a/src/RBDyn/Momentum.cpp b/src/RBDyn/Momentum.cpp index 6226c6b3..e08f7739 100644 --- a/src/RBDyn/Momentum.cpp +++ b/src/RBDyn/Momentum.cpp @@ -22,7 +22,7 @@ sva::ForceVecd computeCentroidalMomentum(const MultiBody & mb, const MultiBodyCo Vector6d cm(Vector6d::Zero()); sva::PTransformd X_com_0(Vector3d(-com)); - for(int i = 0; i < mb.nrBodies(); ++i) + for(size_t i = 0; i < static_cast(mb.nrBodies()); ++i) { // body inertia in body coordinate sva::ForceVecd hi = bodies[i].inertia() * mbc.bodyVelB[i]; @@ -47,7 +47,7 @@ sva::ForceVecd computeCentroidalMomentumDot(const MultiBody & mb, sva::PTransformd X_com_0(Vector3d(-com)); sva::MotionVecd com_Vel(Vector3d::Zero(), comDot); - for(int i = 0; i < mb.nrBodies(); ++i) + for(size_t i = 0; i < static_cast(mb.nrBodies()); ++i) { sva::MotionVecd body_i_Vel(mbc.bodyVelB[i]); sva::PTransformd X_com_i(mbc.bodyPosW[i] * X_com_0); @@ -116,15 +116,17 @@ CentroidalMomentumMatrix::CentroidalMomentumMatrix() } CentroidalMomentumMatrix::CentroidalMomentumMatrix(const MultiBody & mb) -: cmMat_(6, mb.nrDof()), cmMatDot_(6, mb.nrDof()), jacVec_(mb.nrBodies()), blocksVec_(mb.nrBodies()), - jacWork_(mb.nrBodies()), bodiesWeight_(mb.nrBodies(), 1.), normalAcc_(mb.nrBodies()) +: cmMat_(6, mb.nrDof()), cmMatDot_(6, mb.nrDof()), jacVec_(static_cast(mb.nrBodies())), + blocksVec_(static_cast(mb.nrBodies())), jacWork_(static_cast(mb.nrBodies())), + bodiesWeight_(static_cast(mb.nrBodies()), 1.), normalAcc_(static_cast(mb.nrBodies())) { init(mb); } CentroidalMomentumMatrix::CentroidalMomentumMatrix(const MultiBody & mb, std::vector weight) -: cmMat_(6, mb.nrDof()), cmMatDot_(6, mb.nrDof()), jacVec_(mb.nrBodies()), blocksVec_(mb.nrBodies()), - jacWork_(mb.nrBodies()), bodiesWeight_(std::move(weight)), normalAcc_(mb.nrBodies()) +: cmMat_(6, mb.nrDof()), cmMatDot_(6, mb.nrDof()), jacVec_(static_cast(mb.nrBodies())), + blocksVec_(static_cast(mb.nrBodies())), jacWork_(static_cast(mb.nrBodies())), + bodiesWeight_(std::move(weight)), normalAcc_(static_cast(mb.nrBodies())) { init(mb); @@ -145,7 +147,7 @@ void CentroidalMomentumMatrix::computeMatrix(const MultiBody & mb, cmMat_.setZero(); sva::PTransformd X_0_com(com); - for(int i = 0; i < mb.nrBodies(); ++i) + for(size_t i = 0; i < static_cast(mb.nrBodies()); ++i) { const MatrixXd & jac = jacVec_[i].bodyJacobian(mb, mbc); sva::PTransformd X_i_com(X_0_com * (mbc.bodyPosW[i].inv())); @@ -167,7 +169,7 @@ void CentroidalMomentumMatrix::computeMatrixDot(const MultiBody & mb, sva::PTransformd X_0_com(com); sva::MotionVecd com_Vel(Vector3d::Zero(), comDot); - for(int i = 0; i < mb.nrBodies(); ++i) + for(size_t i = 0; i < static_cast(mb.nrBodies()); ++i) { const MatrixXd & jac = jacVec_[i].bodyJacobian(mb, mbc); const MatrixXd & jacDot = jacVec_[i].bodyJacobianDot(mb, mbc); @@ -193,7 +195,7 @@ void CentroidalMomentumMatrix::computeMatrixAndMatrixDot(const MultiBody & mb, sva::PTransformd X_0_com(com); sva::MotionVecd com_Vel(Vector3d::Zero(), comDot); - for(int i = 0; i < mb.nrBodies(); ++i) + for(size_t i = 0; i < static_cast(mb.nrBodies()); ++i) { const MatrixXd & jac = jacVec_[i].bodyJacobian(mb, mbc); const MatrixXd & jacDot = jacVec_[i].bodyJacobianDot(mb, mbc); @@ -230,7 +232,7 @@ sva::ForceVecd CentroidalMomentumMatrix::momentum(const MultiBody & mb, Vector6d cm(Vector6d::Zero()); sva::PTransformd X_com_0(Vector3d(-com)); - for(int i = 0; i < mb.nrBodies(); ++i) + for(size_t i = 0; i < static_cast(mb.nrBodies()); ++i) { // body inertia in body coordinate sva::ForceVecd hi = bodies[i].inertia() * mbc.bodyVelB[i]; @@ -252,16 +254,19 @@ sva::ForceVecd CentroidalMomentumMatrix::normalMomentumDot(const MultiBody & mb, const std::vector & pred = mb.predecessors(); const std::vector & succ = mb.successors(); - for(int i = 0; i < mb.nrJoints(); ++i) + for(size_t i = 0; i < static_cast(mb.nrJoints()); ++i) { + const auto pred_index = static_cast(pred[i]); + const auto succ_index = static_cast(succ[i]); + const sva::PTransformd & X_p_i = mbc.parentToSon[i]; const sva::MotionVecd & vj_i = mbc.jointVelocity[i]; const sva::MotionVecd & vb_i = mbc.bodyVelB[i]; if(pred[i] != -1) - normalAcc_[succ[i]] = X_p_i * normalAcc_[pred[i]] + vb_i.cross(vj_i); + normalAcc_[succ_index] = X_p_i * normalAcc_[pred_index] + vb_i.cross(vj_i); else - normalAcc_[succ[i]] = vb_i.cross(vj_i); + normalAcc_[succ_index] = vb_i.cross(vj_i); } return normalMomentumDot(mb, mbc, com, comDot, normalAcc_); @@ -281,7 +286,7 @@ sva::ForceVecd CentroidalMomentumMatrix::normalMomentumDot(const MultiBody & mb, sva::PTransformd X_com_0(Vector3d(-com)); sva::MotionVecd com_Vel(Vector3d::Zero(), comDot); - for(int i = 0; i < mb.nrBodies(); ++i) + for(size_t i = 0; i < static_cast(mb.nrBodies()); ++i) { sva::MotionVecd body_i_Vel(mbc.bodyVelB[i]); sva::PTransformd X_com_i(mbc.bodyPosW[i] * X_com_0); @@ -382,9 +387,10 @@ void CentroidalMomentumMatrix::init(const rbd::MultiBody & mb) using namespace Eigen; for(int i = 0; i < mb.nrBodies(); ++i) { - jacVec_[i] = Jacobian(mb, mb.body(i).name()); - blocksVec_[i] = jacVec_[i].compactPath(mb); - jacWork_[i].resize(6, jacVec_[i].dof()); + const auto ui = static_cast(i); + jacVec_[ui] = Jacobian(mb, mb.body(i).name()); + blocksVec_[ui] = jacVec_[ui].compactPath(mb); + jacWork_[ui].resize(6, jacVec_[ui].dof()); } } diff --git a/src/RBDyn/MultiBody.cpp b/src/RBDyn/MultiBody.cpp index 795a1010..fb2a25b6 100644 --- a/src/RBDyn/MultiBody.cpp +++ b/src/RBDyn/MultiBody.cpp @@ -27,14 +27,16 @@ MultiBody::MultiBody(std::vector bodies, { for(int i = 0; i < static_cast(bodies_.size()); ++i) { - bodyNameToInd_[bodies_[i].name()] = i; - jointNameToInd_[joints_[i].name()] = i; + const auto ui = static_cast(i); - jointPosInParam_[i] = nrParams_; - jointPosInDof_[i] = nrDof_; + bodyNameToInd_[bodies_[ui].name()] = i; + jointNameToInd_[joints_[ui].name()] = i; - nrParams_ += joints_[i].params(); - nrDof_ += joints_[i].dof(); + jointPosInParam_[ui] = nrParams_; + jointPosInDof_[ui] = nrDof_; + + nrParams_ += joints_[ui].params(); + nrDof_ += joints_[ui].dof(); } } diff --git a/src/RBDyn/MultiBodyConfig.cpp b/src/RBDyn/MultiBodyConfig.cpp index 9b343e86..6dfefaa1 100644 --- a/src/RBDyn/MultiBodyConfig.cpp +++ b/src/RBDyn/MultiBodyConfig.cpp @@ -16,19 +16,24 @@ namespace rbd { MultiBodyConfig::MultiBodyConfig(const MultiBody & mb) -: q(mb.nrJoints()), alpha(mb.nrJoints()), alphaD(mb.nrJoints()), force(mb.nrBodies()), jointConfig(mb.nrJoints()), - jointVelocity(mb.nrJoints()), jointTorque(mb.nrJoints()), motionSubspace(mb.nrJoints()), bodyPosW(mb.nrBodies()), - parentToSon(mb.nrBodies()), bodyVelW(mb.nrBodies()), bodyVelB(mb.nrBodies()), bodyAccB(mb.nrBodies()), - gravity(0., 9.81, 0.) +: q(static_cast(mb.nrJoints())), alpha(static_cast(mb.nrJoints())), + alphaD(static_cast(mb.nrJoints())), force(static_cast(mb.nrBodies())), + jointConfig(static_cast(mb.nrJoints())), jointVelocity(static_cast(mb.nrJoints())), + jointTorque(static_cast(mb.nrJoints())), motionSubspace(static_cast(mb.nrJoints())), + bodyPosW(static_cast(mb.nrBodies())), parentToSon(static_cast(mb.nrBodies())), + bodyVelW(static_cast(mb.nrBodies())), bodyVelB(static_cast(mb.nrBodies())), + bodyAccB(static_cast(mb.nrBodies())), gravity(0., 9.81, 0.) { for(int i = 0; i < static_cast(q.size()); ++i) { - q[i].resize(mb.joint(i).params()); - alpha[i].resize(mb.joint(i).dof()); - alphaD[i].resize(mb.joint(i).dof()); + const auto ui = static_cast(i); - jointTorque[i].resize(mb.joint(i).dof()); - motionSubspace[i].resize(6, mb.joint(i).dof()); + q[ui].resize(static_cast(mb.joint(i).params())); + alpha[ui].resize(static_cast(mb.joint(i).dof())); + alphaD[ui].resize(static_cast(mb.joint(i).dof())); + + jointTorque[ui].resize(static_cast(mb.joint(i).dof())); + motionSubspace[ui].resize(6, mb.joint(i).dof()); } } @@ -36,11 +41,13 @@ void MultiBodyConfig::zero(const MultiBody & mb) { for(int i = 0; i < static_cast(q.size()); ++i) { - q[i] = mb.joint(i).zeroParam(); - alpha[i] = mb.joint(i).zeroDof(); - alphaD[i] = mb.joint(i).zeroDof(); + const auto ui = static_cast(i); + + q[ui] = mb.joint(i).zeroParam(); + alpha[ui] = mb.joint(i).zeroDof(); + alphaD[ui] = mb.joint(i).zeroDof(); - jointTorque[i] = mb.joint(i).zeroDof(); + jointTorque[ui] = mb.joint(i).zeroDof(); } for(std::size_t i = 0; i < force.size(); ++i) @@ -58,7 +65,7 @@ std::vector MultiBodyConfig::python_motionSubspace() ret[i] = motionSubspace[i]; } - return std::move(ret); + return ret; } void MultiBodyConfig::python_motionSubspace(const std::vector & v) @@ -75,7 +82,7 @@ void MultiBodyConfig::python_motionSubspace(const std::vector & */ ConfigConverter::ConfigConverter(const MultiBody & from, const MultiBody & to) -: jInd_(from.nrJoints() - 1), bInd_(from.nrBodies()) +: jInd_(static_cast(from.nrJoints() - 1)), bInd_(static_cast(from.nrBodies())) { using namespace Eigen; @@ -97,14 +104,16 @@ void ConfigConverter::convert(const MultiBodyConfig & from, MultiBodyConfig & to { for(std::size_t i = 0; i < jInd_.size(); ++i) { - to.q[jInd_[i]] = from.q[i + 1]; - to.alpha[jInd_[i]] = from.alpha[i + 1]; - to.alphaD[jInd_[i]] = from.alphaD[i + 1]; + const auto ui = static_cast(i); + const auto joint_index = static_cast(jInd_[ui]); + to.q[joint_index] = from.q[i + 1]; + to.alpha[joint_index] = from.alpha[i + 1]; + to.alphaD[joint_index] = from.alphaD[i + 1]; } for(std::size_t i = 0; i < bInd_.size(); ++i) { - to.force[bInd_[i]] = from.force[i]; + to.force[static_cast(bInd_[i])] = from.force[i]; } } @@ -194,7 +203,7 @@ void paramToVector(const std::vector> & v, Eigen::Ref> & v, Eigen::Ref e) { int nb = 0; - for(int i = 0; i < static_cast(v.size()); ++i) + for(size_t i = 0; i < v.size(); ++i) { nb += static_cast(v[i].size()); } @@ -214,7 +223,7 @@ Eigen::VectorXd paramToVector(const MultiBody & mb, const std::vector> & v) @@ -228,16 +237,17 @@ Eigen::VectorXd sParamToVector(const MultiBody & mb, const std::vector(v[i].size())) + const auto ui = static_cast(i); + if(mb.joint(i).params() != static_cast(v[ui].size())) { std::ostringstream str; str << "Parameters of joint " << i << " mismatch: expected size " << mb.joint(i).params() << " gived " - << v[i].size(); + << v[ui].size(); throw std::out_of_range(str.str()); } } - return std::move(paramToVector(mb, v)); + return paramToVector(mb, v); } Eigen::VectorXd dofToVector(const MultiBody & mb, const std::vector> & v) @@ -245,7 +255,7 @@ Eigen::VectorXd dofToVector(const MultiBody & mb, const std::vector> & v) @@ -259,15 +269,16 @@ Eigen::VectorXd sDofToVector(const MultiBody & mb, const std::vector(v[i].size())) + const auto ui = static_cast(i); + if(mb.joint(i).dof() != static_cast(v[ui].size())) { std::ostringstream str; - str << "Dof of joint " << i << " mismatch: expected size " << mb.joint(i).dof() << " gived " << v[i].size(); + str << "Dof of joint " << i << " mismatch: expected size " << mb.joint(i).dof() << " gived " << v[ui].size(); throw std::out_of_range(str.str()); } } - return std::move(dofToVector(mb, v)); + return dofToVector(mb, v); } void vectorToParam(const Eigen::Ref & e, std::vector> & v) @@ -303,15 +314,17 @@ void sVectorToParam(const Eigen::Ref & e, std::vector> vectorToParam(const MultiBody & mb, const Eigen::VectorXd & e) { - std::vector> ret(mb.nrJoints()); + std::vector> ret(static_cast(mb.nrJoints())); for(int i = 0; i < mb.nrJoints(); ++i) { - ret[i].resize(mb.joint(i).params()); + const auto ui = static_cast(i); + const auto nparams = static_cast(mb.joint(i).params()); + ret[ui].resize(nparams); } vectorToParam(e, ret); - return std::move(ret); + return ret; } std::vector> sVectorToParam(const MultiBody & mb, const Eigen::VectorXd & e) @@ -322,20 +335,22 @@ std::vector> sVectorToParam(const MultiBody & mb, const Eige str << "Parameter vector size mismatch: expected size " << mb.nrParams() << " gived " << e.rows(); throw std::out_of_range(str.str()); } - return std::move(vectorToParam(mb, e)); + return vectorToParam(mb, e); } std::vector> vectorToDof(const MultiBody & mb, const Eigen::VectorXd & e) { - std::vector> ret(mb.nrJoints()); + std::vector> ret(static_cast(mb.nrJoints())); for(int i = 0; i < mb.nrJoints(); ++i) { - ret[i].resize(mb.joint(i).dof()); + const auto ui = static_cast(i); + const auto ndofs = static_cast(mb.joint(i).dof()); + ret[ui].resize(ndofs); } vectorToParam(e, ret); - return std::move(ret); + return ret; } std::vector> sVectorToDof(const MultiBody & mb, const Eigen::VectorXd & e) @@ -346,7 +361,7 @@ std::vector> sVectorToDof(const MultiBody & mb, const Eigen: str << "Dof vector size mismatch: expected size " << mb.nrDof() << " gived " << e.rows(); throw std::out_of_range(str.str()); } - return std::move(vectorToDof(mb, e)); + return vectorToDof(mb, e); } void checkMatchBodyPos(const MultiBody & mb, const MultiBodyConfig & mbc) @@ -386,11 +401,12 @@ void checkMatchJointTorque(const MultiBody & mb, const MultiBodyConfig & mbc) for(int i = 0; i < static_cast(mbc.jointTorque.size()); ++i) { - if(mbc.jointTorque[i].size() != static_cast(mb.joint(i).dof())) + const auto ui = static_cast(i); + if(mbc.jointTorque[ui].size() != static_cast(mb.joint(i).dof())) { std::ostringstream str; str << "Bad number of torque variable for Joint " << mb.joint(i) << " at position " << i << ": expected size " - << mb.joint(i).dof() << " gived " << mbc.jointTorque[i].size(); + << mb.joint(i).dof() << " gived " << mbc.jointTorque[ui].size(); throw std::domain_error(str.str()); } } @@ -402,11 +418,12 @@ void checkMatchMotionSubspace(const MultiBody & mb, const MultiBodyConfig & mbc) for(int i = 0; i < static_cast(mbc.motionSubspace.size()); ++i) { - if(mbc.motionSubspace[i].cols() != static_cast(mb.joint(i).dof())) + const auto ui = static_cast(i); + if(mbc.motionSubspace[ui].cols() != static_cast(mb.joint(i).dof())) { std::ostringstream str; str << "Bad motionSubspace matrix size for Joint " << mb.joint(i) << " at position " << i - << ": expected column number " << mb.joint(i).dof() << " gived " << mbc.motionSubspace[i].cols(); + << ": expected column number " << mb.joint(i).dof() << " gived " << mbc.motionSubspace[ui].cols(); throw std::domain_error(str.str()); } } @@ -418,11 +435,12 @@ void checkMatchQ(const MultiBody & mb, const MultiBodyConfig & mbc) for(int i = 0; i < static_cast(mbc.q.size()); ++i) { - if(mbc.q[i].size() != static_cast(mb.joint(i).params())) + const auto ui = static_cast(i); + if(mbc.q[ui].size() != static_cast(mb.joint(i).params())) { std::ostringstream str; str << "Bad number of generalized position variable for Joint " << mb.joint(i) << " at position " << i - << ": expected size " << mb.joint(i).params() << " gived " << mbc.q[i].size(); + << ": expected size " << mb.joint(i).params() << " gived " << mbc.q[ui].size(); throw std::domain_error(str.str()); } } @@ -434,11 +452,12 @@ void checkMatchAlpha(const MultiBody & mb, const MultiBodyConfig & mbc) for(int i = 0; i < static_cast(mbc.alpha.size()); ++i) { - if(mbc.alpha[i].size() != static_cast(mb.joint(i).dof())) + const auto ui = static_cast(i); + if(mbc.alpha[ui].size() != static_cast(mb.joint(i).dof())) { std::ostringstream str; str << "Bad number of generalized velocity variable for Joint " << mb.joint(i) << " at position " << i - << ": expected size " << mb.joint(i).dof() << " gived " << mbc.alpha[i].size(); + << ": expected size " << mb.joint(i).dof() << " gived " << mbc.alpha[ui].size(); throw std::domain_error(str.str()); } } @@ -450,11 +469,12 @@ void checkMatchAlphaD(const MultiBody & mb, const MultiBodyConfig & mbc) for(int i = 0; i < static_cast(mbc.alphaD.size()); ++i) { - if(mbc.alphaD[i].size() != static_cast(mb.joint(i).dof())) + const auto ui = static_cast(i); + if(mbc.alphaD[ui].size() != static_cast(mb.joint(i).dof())) { std::ostringstream str; str << "Bad number of generalized acceleration variable for Joint " << mb.joint(i) << " at position " << i - << ": expected size " << mb.joint(i).dof() << " gived " << mbc.alphaD[i].size(); + << ": expected size " << mb.joint(i).dof() << " gived " << mbc.alphaD[ui].size(); throw std::domain_error(str.str()); } } diff --git a/src/RBDyn/MultiBodyGraph.cpp b/src/RBDyn/MultiBodyGraph.cpp index 7dc4b738..cff30e11 100644 --- a/src/RBDyn/MultiBodyGraph.cpp +++ b/src/RBDyn/MultiBodyGraph.cpp @@ -268,7 +268,7 @@ std::map MultiBodyGraph::bodiesBaseTransform(cons std::shared_ptr rootNode = bodyNameToNode_.at(rootBodyName); computeTransform(rootNode, nullptr, X_b0_j0); - return std::move(X_nb_b); + return X_nb_b; } std::map> MultiBodyGraph::successorJoints(const std::string & rootBodyName) @@ -291,7 +291,7 @@ std::map> MultiBodyGraph::successorJoints( std::shared_ptr rootNode = bodyNameToNode_.at(rootBodyName); computeSuccesors(rootNode, nullptr); - return std::move(successorJoints); + return successorJoints; } std::map MultiBodyGraph::predecessorJoint(const std::string & rootBodyName) @@ -317,7 +317,7 @@ std::map MultiBodyGraph::predecessorJoint(const std::s std::shared_ptr rootNode = bodyNameToNode_.at(rootBodyName); computePredecessor(rootNode, nullptr, rootJointName_); - return std::move(predJoint); + return predJoint; } MultiBodyGraph MultiBodyGraph::fixJoints(const MultiBodyGraph & other, diff --git a/src/RBDyn/RBDyn/Body.h b/src/RBDyn/RBDyn/Body.h index fedbf1f9..82eb5971 100644 --- a/src/RBDyn/RBDyn/Body.h +++ b/src/RBDyn/RBDyn/Body.h @@ -26,7 +26,7 @@ class Body * @param rbInertia Body spatial rigid body inertia. * @param name Body name, must be unique in a multibody. */ - Body(const sva::RBInertiad & rbInertia, std::string name) : inertia_(rbInertia), name_(name) {} + Body(const sva::RBInertiad & rbInertia, std::string name) : inertia_(rbInertia), name_(std::move(name)) {} /** * @param mass Body mass. @@ -35,7 +35,7 @@ class Body * @param name Body name, must be unique in a multibody. */ Body(double mass, const Eigen::Vector3d & com, const Eigen::Matrix3d & inertia, std::string name) - : inertia_(mass, mass * com, inertia), name_(name) + : inertia_(mass, mass * com, inertia), name_(std::move(name)) { } diff --git a/src/RBDyn/RBDyn/MultiBody.h b/src/RBDyn/RBDyn/MultiBody.h index b3a8fe83..edf7bc9d 100644 --- a/src/RBDyn/RBDyn/MultiBody.h +++ b/src/RBDyn/RBDyn/MultiBody.h @@ -73,13 +73,13 @@ class RBDYN_DLLAPI MultiBody /// @return Body at num position in bodies list. const Body & body(int num) const { - return bodies_[num]; + return bodies_[static_cast(num)]; } /// Set the body num in bodies list. void body(int num, const Body & b) { - bodies_[num] = b; + bodies_[static_cast(num)] = b; } /// @return Index of the root body @@ -99,7 +99,7 @@ class RBDYN_DLLAPI MultiBody /// @return Joint at num position in joint list. const Joint & joint(int num) const { - return joints_[num]; + return joints_[static_cast(num)]; } /// @return Predeccesor body index of each joint. @@ -111,7 +111,7 @@ class RBDYN_DLLAPI MultiBody /// @return Predecessor body of joint num. int predecessor(int num) const { - return pred_[num]; + return pred_[static_cast(num)]; } /// @return Successor body index of each joint. @@ -123,7 +123,7 @@ class RBDYN_DLLAPI MultiBody /// @return Successor body of joint num. int successor(int num) const { - return succ_[num]; + return succ_[static_cast(num)]; } /// @return Parent body index of each body. @@ -135,7 +135,7 @@ class RBDYN_DLLAPI MultiBody /// @return Parent body of body num. int parent(int num) const { - return parent_[num]; + return parent_[static_cast(num)]; } /// @return Transformation from the body base to joint i @@ -153,13 +153,13 @@ class RBDYN_DLLAPI MultiBody /// @return Transformation from the body base to joint num const sva::PTransformd & transform(int num) const { - return Xt_[num]; + return Xt_[static_cast(num)]; } /// Set the transformation from the body base to joint num void transform(int num, const sva::PTransformd & Xt) { - Xt_[num] = Xt; + Xt_[static_cast(num)] = Xt; } /// @return Index of the body with name 'name'. @@ -189,13 +189,13 @@ class RBDYN_DLLAPI MultiBody /// @return the joint i position in parameter vector (q). int jointPosInParam(int i) const { - return jointPosInParam_[i]; + return jointPosInParam_[static_cast(i)]; } /// @return the joint i position in dof vector (alpha, alphaD…). int jointPosInDof(int i) const { - return jointPosInDof_[i]; + return jointPosInDof_[static_cast(i)]; } /// @return the joint position in parameter vector (q). @@ -243,7 +243,7 @@ class RBDYN_DLLAPI MultiBody */ const Body & sBody(int num) const { - return bodies_.at(num); + return bodies_.at(static_cast(num)); } /** Safe version of @see body. @@ -251,7 +251,7 @@ class RBDYN_DLLAPI MultiBody */ void sBody(int num, const Body & b) { - bodies_.at(num) = b; + bodies_.at(static_cast(num)) = b; } /** Safe version of @see joint. @@ -259,7 +259,7 @@ class RBDYN_DLLAPI MultiBody */ const Joint & sJoint(int num) const { - return joints_.at(num); + return joints_.at(static_cast(num)); } /** Safe version of @see predecessor. @@ -267,7 +267,7 @@ class RBDYN_DLLAPI MultiBody */ int sPredecessor(int num) const { - return pred_.at(num); + return pred_.at(static_cast(num)); } /** Safe version of @see successor. @@ -275,7 +275,7 @@ class RBDYN_DLLAPI MultiBody */ int sSuccessor(int num) const { - return succ_.at(num); + return succ_.at(static_cast(num)); } /** Safe version of @see parent. @@ -283,7 +283,7 @@ class RBDYN_DLLAPI MultiBody */ int sParent(int num) const { - return parent_.at(num); + return parent_.at(static_cast(num)); } /** Safe version of @see transforms. @@ -305,7 +305,7 @@ class RBDYN_DLLAPI MultiBody */ const sva::PTransformd & sTransform(int num) const { - return Xt_.at(num); + return Xt_.at(static_cast(num)); } /** Safe version of @see transform. @@ -313,7 +313,7 @@ class RBDYN_DLLAPI MultiBody */ void sTransform(int num, const sva::PTransformd & Xt) { - Xt_.at(num) = Xt; + Xt_.at(static_cast(num)) = Xt; } /** Safe version of @see jointPosInParam. @@ -321,7 +321,7 @@ class RBDYN_DLLAPI MultiBody */ int sJointPosInParam(int i) const { - return jointPosInParam_.at(i); + return jointPosInParam_.at(static_cast(i)); } /** Safe version of @see jointPosInDof. @@ -329,7 +329,7 @@ class RBDYN_DLLAPI MultiBody */ int sJointPosInDof(int i) const { - return jointPosInDof_.at(i); + return jointPosInDof_.at(static_cast(i)); } /** Safe version of @see bodyIndexByName. diff --git a/src/RBDyn/RBDyn/MultiBodyConfig.h b/src/RBDyn/RBDyn/MultiBodyConfig.h index 0dfee321..0774396d 100644 --- a/src/RBDyn/RBDyn/MultiBodyConfig.h +++ b/src/RBDyn/RBDyn/MultiBodyConfig.h @@ -274,7 +274,7 @@ inline void ConfigConverter::convertJoint(const std::vector & from, std::vect { for(std::size_t i = 0; i < jInd_.size(); ++i) { - to[jInd_[i]] = from[i + 1]; + to[static_cast(jInd_[i])] = from[i + 1]; } } @@ -295,10 +295,10 @@ inline std::vector ConfigConverter::convertJoint(const std::vector & from) std::vector to(from.size()); for(std::size_t i = 0; i < jInd_.size(); ++i) { - to[jInd_[i]] = from[i + 1]; + to[static_cast(jInd_[i])] = from[i + 1]; } - return std::move(to); + return to; } } // namespace rbd diff --git a/src/RBDyn/RBDyn/VisServo.h b/src/RBDyn/RBDyn/VisServo.h index 4c9cfb71..04432905 100644 --- a/src/RBDyn/RBDyn/VisServo.h +++ b/src/RBDyn/RBDyn/VisServo.h @@ -54,9 +54,7 @@ RBDYN_DLLAPI void imagePointJacobianDot(const Eigen::Vector2d & imagePoint, * @param rot_angle_threshold is the minimum angle of an axis angle representation where the angle * is considered as zero */ -RBDYN_DLLAPI void poseJacobian(const Eigen::Matrix3d & rotation, - Eigen::Matrix & jac, - const double rot_angle_threshold = 1.0e-8); +RBDYN_DLLAPI void poseJacobian(const Eigen::Matrix3d & rotation, Eigen::Matrix & jac); /** * Compute the interaction matrix of the depth derivative diff --git a/src/RBDyn/VisServo.cpp b/src/RBDyn/VisServo.cpp index a4c22065..eded7e7c 100644 --- a/src/RBDyn/VisServo.cpp +++ b/src/RBDyn/VisServo.cpp @@ -39,7 +39,7 @@ void imagePointJacobianDot(const Eigen::Vector2d & imagePoint, depthDot / Z_sq, (imagePointSpeed(1) * depth - imagePoint(1) * depthDot) / Z_sq; } -void poseJacobian(const Eigen::Matrix3d & rotation, Eigen::Matrix & jac, const double rot_angle_threshold) +void poseJacobian(const Eigen::Matrix3d & rotation, Eigen::Matrix & jac) { Eigen::Matrix3d i3 = Eigen::Matrix3d::Identity(); diff --git a/src/parsers/RBDyn/parsers/common.h b/src/parsers/RBDyn/parsers/common.h index 77d8b2ee..8b1230d0 100644 --- a/src/parsers/RBDyn/parsers/common.h +++ b/src/parsers/RBDyn/parsers/common.h @@ -23,7 +23,7 @@ namespace rbd namespace parsers { -enum class RBDYN_PARSERS_DLLAPI ParserInput +enum class ParserInput { File, Description diff --git a/src/parsers/yaml.cpp b/src/parsers/yaml.cpp index eab0fdb6..82e90722 100644 --- a/src/parsers/yaml.cpp +++ b/src/parsers/yaml.cpp @@ -162,10 +162,7 @@ void RBDynFromYAML::parseFrame(const YAML::Node & frame, { throw std::runtime_error("YAML: Invalid array size (" + name + "->intertial->frame->xyz"); } - for(size_t i = 0; i < 3; ++i) - { - xyz[i] = xyz_data[i]; - } + std::copy_n(std::begin(xyz_data), 3, xyz.data()); } auto rpy_node = frame["rpy"]; if(rpy_node) @@ -175,10 +172,7 @@ void RBDynFromYAML::parseFrame(const YAML::Node & frame, { throw std::runtime_error("YAML: Invalid array size (" + name + "->intertial->frame->rpy"); } - for(size_t i = 0; i < 3; ++i) - { - rpy[i] = rpy_data[i]; - } + std::copy_n(std::begin(rpy_data), 3, rpy.data()); if(frame["anglesInDegrees"].as(angles_in_degrees_)) { rpy *= M_PI / 180.; @@ -492,10 +486,7 @@ void RBDynFromYAML::parseJointAxis(const YAML::Node & axis, const std::string & { throw std::runtime_error("YAML: Invalid array size (" + name + "->intertial->frame->axis"); } - for(size_t i = 0; i < 3; ++i) - { - joint_axis[i] = axis_data[i]; - } + std::copy_n(std::begin(axis_data), 3, joint_axis.data()); } else { diff --git a/tests/AlgoTest.cpp b/tests/AlgoTest.cpp index 4f2aaf4c..fcf0b9fb 100644 --- a/tests/AlgoTest.cpp +++ b/tests/AlgoTest.cpp @@ -402,15 +402,19 @@ double testEulerInteg(rbd::Joint::Type jType, using namespace rbd; Joint j(jType, axis, true, std::string("0")); - std::vector qVec(j.params()), alphaVec(j.dof()), alphaDVec(j.dof()); + const auto nparams = static_cast(j.params()); + const auto ndofs = static_cast(j.dof()); + std::vector qVec(nparams), alphaVec(ndofs), alphaDVec(ndofs); for(int i = 0; i < j.params(); ++i) { - qVec[i] = q[i]; + const auto ui = static_cast(i); + qVec[ui] = q(i); } for(int i = 0; i < j.dof(); ++i) { - alphaVec[i] = alpha[i]; - alphaDVec[i] = 0.; + const auto ui = static_cast(i); + alphaVec[ui] = alpha(i); + alphaDVec[ui] = 0.; } sva::PTransformd initPos(j.pose(qVec)); @@ -484,15 +488,16 @@ BOOST_AUTO_TEST_CASE(FATest) forwardVelocity(mb, mbc); forwardAcceleration(mb, mbc); - for(int i = 0; i < mb.nrBodies(); ++i) + for(size_t i = 0; i < static_cast(mb.nrBodies()); ++i) { BOOST_CHECK_SMALL(mbc.bodyAccB[i].vector().norm(), TOL); } - std::vector jacs(mb.nrBodies()); + std::vector jacs(static_cast(mb.nrBodies())); for(int i = 0; i < mb.nrBodies(); ++i) { - jacs[i] = rbd::Jacobian(mb, mb.body(i).name()); + const auto ui = static_cast(i); + jacs[ui] = rbd::Jacobian(mb, mb.body(i).name()); } Eigen::MatrixXd fullJac(6, mb.nrDof()); @@ -514,7 +519,7 @@ BOOST_AUTO_TEST_CASE(FATest) forwardVelocity(mb, mbc); forwardAcceleration(mb, mbc); - for(int j = 0; j < mb.nrBodies(); ++j) + for(size_t j = 0; j < static_cast(mb.nrBodies()); ++j) { const Eigen::MatrixXd & jac = jacs[j].bodyJacobian(mb, mbc); const Eigen::MatrixXd & jacDot = jacs[j].bodyJacobianDot(mb, mbc); diff --git a/tests/CoMTest.cpp b/tests/CoMTest.cpp index ba536600..b303845d 100644 --- a/tests/CoMTest.cpp +++ b/tests/CoMTest.cpp @@ -279,24 +279,28 @@ BOOST_AUTO_TEST_CASE(CoMJacobianDummyTest) for(int i = 0; i < mb.nrJoints(); ++i) { + const auto ui = static_cast(i); for(int j = 0; j < mb.joint(i).dof(); ++j) { - mbc.alpha[i][j] = 1.; - mbc.alphaD[i][j] = 1.; + const auto uj = static_cast(j); + mbc.alpha[ui][uj] = 1.; + mbc.alphaD[ui][uj] = 1.; testJacCoMVelAcc(mb, mbc, comJac); - mbc.alpha[i][j] = 0.; - mbc.alphaD[i][j] = 0.; + mbc.alpha[ui][uj] = 0.; + mbc.alphaD[ui][uj] = 0.; } } for(int i = 0; i < mb.nrJoints(); ++i) { + const auto ui = static_cast(i); for(int j = 0; j < mb.joint(i).dof(); ++j) { - mbc.alpha[i][j] = 1.; - mbc.alphaD[i][j] = 1.; + const auto uj = static_cast(j); + mbc.alpha[ui][uj] = 1.; + mbc.alphaD[ui][uj] = 1.; testJacCoMVelAcc(mb, mbc, comJac); } @@ -315,24 +319,28 @@ BOOST_AUTO_TEST_CASE(CoMJacobianDummyTest) for(int i = 0; i < mb.nrJoints(); ++i) { + const auto ui = static_cast(i); for(int j = 0; j < mb.joint(i).dof(); ++j) { - mbc.alpha[i][j] = 1.; - mbc.alphaD[i][j] = 1.; + const auto uj = static_cast(j); + mbc.alpha[ui][uj] = 1.; + mbc.alphaD[ui][uj] = 1.; testJacCoMVelAcc(mb, mbc, comJac); - mbc.alpha[i][j] = 0.; - mbc.alphaD[i][j] = 0.; + mbc.alpha[ui][uj] = 0.; + mbc.alphaD[ui][uj] = 0.; } } for(int i = 0; i < mb.nrJoints(); ++i) { + const auto ui = static_cast(i); for(int j = 0; j < mb.joint(i).dof(); ++j) { - mbc.alpha[i][j] = 1.; - mbc.alphaD[i][j] = 1.; + const auto uj = static_cast(j); + mbc.alpha[ui][uj] = 1.; + mbc.alphaD[ui][uj] = 1.; testJacCoMVelAcc(mb, mbc, comJac); } @@ -357,9 +365,11 @@ BOOST_AUTO_TEST_CASE(CoMJacobianDummyTest) for(int i = 0; i < mb.nrJoints(); ++i) { + const auto ui = static_cast(i); for(int j = 0; j < mb.joint(i).dof(); ++j) { - mbc.alpha[i][j] = 1.; + const auto uj = static_cast(j); + mbc.alpha[ui][uj] = 1.; forwardVelocity(mb, mbc); forwardAcceleration(mb, mbc); @@ -370,15 +380,17 @@ BOOST_AUTO_TEST_CASE(CoMJacobianDummyTest) BOOST_CHECK_EQUAL(jacDot.cols(), mb.nrDof()); BOOST_CHECK_SMALL((jacDot_diff - jacDot).norm(), TOL); - mbc.alpha[i][j] = 0.; + mbc.alpha[ui][uj] = 0.; } } for(int i = 0; i < mb.nrJoints(); ++i) { + const auto ui = static_cast(i); for(int j = 0; j < mb.joint(i).dof(); ++j) { - mbc.alpha[i][j] = 1.; + const auto uj = static_cast(j); + mbc.alpha[ui][uj] = 1.; forwardVelocity(mb, mbc); forwardAcceleration(mb, mbc); @@ -389,7 +401,7 @@ BOOST_AUTO_TEST_CASE(CoMJacobianDummyTest) BOOST_CHECK_EQUAL(jacDot.cols(), mb.nrDof()); BOOST_CHECK_SMALL((jacDot_diff - jacDot).norm(), TOL); - mbc.alpha[i][j] = 0.; + mbc.alpha[ui][uj] = 0.; } } mbc.alpha = {{}, {0.}, {0.}, {0.}, {0., 0., 0.}}; @@ -404,9 +416,11 @@ BOOST_AUTO_TEST_CASE(CoMJacobianDummyTest) for(int i = 0; i < mb.nrJoints(); ++i) { + const auto ui = static_cast(i); for(int j = 0; j < mb.joint(i).dof(); ++j) { - mbc.alpha[i][j] = 1.; + const auto uj = static_cast(j); + mbc.alpha[ui][uj] = 1.; forwardVelocity(mb, mbc); forwardAcceleration(mb, mbc); @@ -417,15 +431,17 @@ BOOST_AUTO_TEST_CASE(CoMJacobianDummyTest) BOOST_CHECK_EQUAL(jacDot.cols(), mb.nrDof()); BOOST_CHECK_SMALL((jacDot_diff - jacDot).norm(), TOL); - mbc.alpha[i][j] = 0.; + mbc.alpha[ui][uj] = 0.; } } for(int i = 0; i < mb.nrJoints(); ++i) { + const auto ui = static_cast(i); for(int j = 0; j < mb.joint(i).dof(); ++j) { - mbc.alpha[i][j] = 1.; + const auto uj = static_cast(j); + mbc.alpha[ui][uj] = 1.; forwardVelocity(mb, mbc); forwardAcceleration(mb, mbc); @@ -436,7 +452,7 @@ BOOST_AUTO_TEST_CASE(CoMJacobianDummyTest) BOOST_CHECK_EQUAL(jacDot.cols(), mb.nrDof()); BOOST_CHECK_SMALL((jacDot_diff - jacDot).norm(), TOL); - mbc.alpha[i][j] = 0.; + mbc.alpha[ui][uj] = 0.; } } mbc.alpha = {{}, {0.}, {0.}, {0.}, {0., 0., 0.}}; @@ -470,7 +486,7 @@ BOOST_AUTO_TEST_CASE(CoMJacobianTest) std::tie(mb, mbc, mbg) = makeXYZSarmRandomCoM(); - std::vector weight(mb.nrBodies()); + std::vector weight(static_cast(mb.nrBodies())); for(std::size_t i = 0; i < weight.size(); ++i) { weight[i] = Eigen::Matrix::Random()(0); @@ -506,9 +522,11 @@ BOOST_AUTO_TEST_CASE(CoMJacobianTest) for(int i = 0; i < mb.nrJoints(); ++i) { + const auto ui = static_cast(i); for(int j = 0; j < mb.joint(i).dof(); ++j) { - mbc.alpha[i][j] = 1.; + const auto uj = static_cast(j); + mbc.alpha[ui][uj] = 1.; forwardVelocity(mb, mbc); MatrixXd jacDotMat = comJac.jacobianDot(mb, mbc); @@ -518,15 +536,17 @@ BOOST_AUTO_TEST_CASE(CoMJacobianTest) BOOST_CHECK_EQUAL(jacDotMat.cols(), mb.nrDof()); BOOST_CHECK_SMALL((jacDotMat - jacDotDummyMat).norm(), TOL); - mbc.alpha[i][j] = 0.; + mbc.alpha[ui][uj] = 0.; } } for(int i = 0; i < mb.nrJoints(); ++i) { + const auto ui = static_cast(i); for(int j = 0; j < mb.joint(i).dof(); ++j) { - mbc.alpha[i][j] = 1.; + const auto uj = static_cast(j); + mbc.alpha[ui][uj] = 1.; forwardVelocity(mb, mbc); MatrixXd jacDotMat = comJac.jacobianDot(mb, mbc); diff --git a/tests/DynamicsTest.cpp b/tests/DynamicsTest.cpp index dec0b6f2..c75c7e57 100644 --- a/tests/DynamicsTest.cpp +++ b/tests/DynamicsTest.cpp @@ -113,7 +113,7 @@ void normalizeQuat(std::vector & q) { Eigen::Vector4d qv(q[0], q[1], q[2], q[3]); double norm = qv.norm(); - for(int i = 0; i < 4; ++i) q[i] /= norm; + for(size_t i = 0; i < 4; ++i) q[i] /= norm; } void makeRandomConfig(rbd::MultiBodyConfig & mbc) @@ -156,9 +156,11 @@ Eigen::MatrixXd makeHFromID(const rbd::MultiBody & mb, int col = 0; for(int i = 0; i < mb.nrJoints(); ++i) { + const auto ui = static_cast(i); for(int j = 0; j < mb.joint(i).dof(); ++j) { - mbcd.alphaD[i][j] = 1.; + const auto uj = static_cast(j); + mbcd.alphaD[ui][uj] = 1.; id.inverseDynamics(mb, mbcd); @@ -174,7 +176,7 @@ Eigen::MatrixXd makeHFromID(const rbd::MultiBody & mb, H.col(col) = Hd - C; - mbcd.alphaD[i][j] = 0.; + mbcd.alphaD[ui][uj] = 0.; ++col; } } diff --git a/tests/IntegrationTest.cpp b/tests/IntegrationTest.cpp index eb938725..4f086039 100644 --- a/tests/IntegrationTest.cpp +++ b/tests/IntegrationTest.cpp @@ -63,7 +63,7 @@ std::tuple makeSingle std::vector randVec(int size, double rmin, double rmax, bool normed = false) { - std::vector v(size, 0); + std::vector v(static_cast(size), 0); Map r(&v[0], size, 1); r = (rmax - rmin) * VectorXd::Random(size).cwiseAbs() + VectorXd::Constant(size, rmin); diff --git a/tests/JacobianTest.cpp b/tests/JacobianTest.cpp index a932d8fa..54fffbc2 100644 --- a/tests/JacobianTest.cpp +++ b/tests/JacobianTest.cpp @@ -139,16 +139,18 @@ void checkJacobianMatrixFromVelocity(const rbd::MultiBody & subMb, int col = 0; for(int i = 0; i < subMb.nrJoints(); ++i) { + const auto ui = static_cast(i); for(int j = 0; j < subMb.joint(i).dof(); ++j) { - subMbc.alpha[i][j] = 1.; + const auto uj = static_cast(j); + subMbc.alpha[ui][uj] = 1.; forwardVelocity(subMb, subMbc); Eigen::Vector6d mv = velVec.back().vector(); BOOST_CHECK_SMALL((mv - jacMat.col(col)).norm(), TOL); - subMbc.alpha[i][j] = 0.; + subMbc.alpha[ui][uj] = 0.; ++col; } } @@ -177,7 +179,8 @@ void checkFullJacobianMatrix(const rbd::MultiBody & mb, for(int i = 0; i < subMb.nrJoints(); ++i) { - int joint = jac.jointsPath()[i]; + const auto ui = static_cast(i); + int joint = jac.jointsPath()[ui]; int dof = mb.joint(i).dof(); BOOST_CHECK_EQUAL(jacMat.block(0, subMb.jointPosInDof(i), 6, dof), fullJacMat.block(0, mb.jointPosInDof(joint), 6, dof)); @@ -195,11 +198,12 @@ void checkJacobian(const rbd::MultiBody & mb, const rbd::MultiBodyConfig & mbc, // fill subMbc MultiBodyConfig subMbc(subMb); - for(int i = 0; i < subMb.nrJoints(); ++i) + for(size_t i = 0; i < static_cast(subMb.nrJoints()); ++i) { - subMbc.bodyPosW[i] = mbc.bodyPosW[jac.jointsPath()[i]]; - subMbc.jointConfig[i] = mbc.jointConfig[jac.jointsPath()[i]]; - subMbc.parentToSon[i] = mbc.parentToSon[jac.jointsPath()[i]]; + const auto joint_index = static_cast(jac.jointsPath()[i]); + subMbc.bodyPosW[i] = mbc.bodyPosW[joint_index]; + subMbc.jointConfig[i] = mbc.jointConfig[joint_index]; + subMbc.parentToSon[i] = mbc.parentToSon[joint_index]; } // test fullJacobian @@ -426,14 +430,16 @@ BOOST_AUTO_TEST_CASE(JacobianDotComputeTest) forwardKinematics(mb, mbc); for(int i = 0; i < mb.nrJoints(); ++i) { + const auto ui = static_cast(i); for(int j = 0; j < mb.joint(i).dof(); ++j) { - mbc.alpha[i][j] = 1.; + const auto uj = static_cast(j); + mbc.alpha[ui][uj] = 1.; forwardVelocity(mb, mbc); testJacobianDot(mb, mbc, jac1); - mbc.alpha[i][j] = 0.; + mbc.alpha[ui][uj] = 0.; } } @@ -445,14 +451,16 @@ BOOST_AUTO_TEST_CASE(JacobianDotComputeTest) forwardKinematics(mb, mbc); for(int i = 0; i < mb.nrJoints(); ++i) { + const auto ui = static_cast(i); for(int j = 0; j < mb.joint(i).dof(); ++j) { - mbc.alpha[i][j] = 1.; + const auto uj = static_cast(j); + mbc.alpha[ui][uj] = 1.; forwardVelocity(mb, mbc); testJacobianDot(mb, mbc, jac1); - mbc.alpha[i][j] = 0.; + mbc.alpha[ui][uj] = 0.; } } @@ -461,14 +469,16 @@ BOOST_AUTO_TEST_CASE(JacobianDotComputeTest) forwardKinematics(mb, mbc); for(int i = 0; i < mb.nrJoints(); ++i) { + const auto ui = static_cast(i); for(int j = 0; j < mb.joint(i).dof(); ++j) { - mbc.alpha[i][j] = 1.; + const auto uj = static_cast(j); + mbc.alpha[ui][uj] = 1.; forwardVelocity(mb, mbc); testJacobianDot(mb, mbc, jac1); - mbc.alpha[i][j] = 0.; + mbc.alpha[ui][uj] = 0.; } } @@ -477,23 +487,27 @@ BOOST_AUTO_TEST_CASE(JacobianDotComputeTest) forwardKinematics(mb, mbc); for(int i = 0; i < mb.nrJoints(); ++i) { + const auto ui = static_cast(i); for(int j = 0; j < mb.joint(i).dof(); ++j) { - mbc.alpha[i][j] = 1.; + const auto uj = static_cast(j); + mbc.alpha[ui][uj] = 1.; forwardVelocity(mb, mbc); testJacobianDot(mb, mbc, jac1); - mbc.alpha[i][j] = 0.; + mbc.alpha[ui][uj] = 0.; } } // test with all joint velocity for(int i = 0; i < mb.nrJoints(); ++i) { + const auto ui = static_cast(i); for(int j = 0; j < mb.joint(i).dof(); ++j) { - mbc.alpha[i][j] = 1.; + const auto uj = static_cast(j); + mbc.alpha[ui][uj] = 1.; forwardVelocity(mb, mbc); testJacobianDot(mb, mbc, jac1); @@ -505,9 +519,11 @@ BOOST_AUTO_TEST_CASE(JacobianDotComputeTest) Jacobian jacP(mb, "b3", Vector3d::Random() * 10.); for(int i = 0; i < mb.nrJoints(); ++i) { + const auto ui = static_cast(i); for(int j = 0; j < mb.joint(i).dof(); ++j) { - mbc.alpha[i][j] = 1.; + const auto uj = static_cast(j); + mbc.alpha[ui][uj] = 1.; forwardVelocity(mb, mbc); testJacobianDot(mb, mbc, jacP); @@ -530,9 +546,11 @@ BOOST_AUTO_TEST_CASE(JacobianDotComputeTest) for(int i = 0; i < mbF.nrJoints(); ++i) { + const auto ui = static_cast(i); for(int j = 0; j < mbF.joint(i).dof(); ++j) { - mbcF.alpha[i][j] = 1.; + const auto uj = static_cast(j); + mbcF.alpha[ui][uj] = 1.; forwardVelocity(mbF, mbcF); testJacobianDot(mbF, mbcF, jacF); @@ -549,9 +567,11 @@ BOOST_AUTO_TEST_CASE(JacobianDotComputeTest) for(int i = 0; i < mbF.nrJoints(); ++i) { + const auto ui = static_cast(i); for(int j = 0; j < mbF.joint(i).dof(); ++j) { - mbcF.alpha[i][j] = 1.; + const auto uj = static_cast(j); + mbcF.alpha[ui][uj] = 1.; forwardVelocity(mbF, mbcF); testJacobianDot(mbF, mbcF, jacF); diff --git a/tests/MomentumTest.cpp b/tests/MomentumTest.cpp index 9c372a14..b9e81b68 100644 --- a/tests/MomentumTest.cpp +++ b/tests/MomentumTest.cpp @@ -81,7 +81,7 @@ BOOST_AUTO_TEST_CASE(centroidalMomentum) // test J·q against CentroidalMomentumMatrix::momentum for(int i = 0; i < 50; ++i) { - std::vector weight(mb.nrBodies()); + std::vector weight(static_cast(mb.nrBodies())); for(std::size_t i = 0; i < weight.size(); ++i) { weight[i] = Eigen::Matrix::Random()(0); @@ -182,7 +182,7 @@ BOOST_AUTO_TEST_CASE(centroidalMomentumDot) // test JDot·q against CentroidalMomentumMatrix::normalMomentumDot for(int i = 0; i < 50; ++i) { - std::vector weight(mb.nrBodies()); + std::vector weight(static_cast(mb.nrBodies())); for(std::size_t i = 0; i < weight.size(); ++i) { weight[i] = Eigen::Matrix::Random()(0); diff --git a/tests/MultiBodyTest.cpp b/tests/MultiBodyTest.cpp index 902a2166..7548e471 100644 --- a/tests/MultiBodyTest.cpp +++ b/tests/MultiBodyTest.cpp @@ -218,23 +218,24 @@ void checkMultiBodyEq(const rbd::MultiBody & mb, BOOST_CHECK_EQUAL_COLLECTIONS(mb.transforms().begin(), mb.transforms().end(), Xt.begin(), Xt.end()); // nrBodies - BOOST_CHECK_EQUAL(mb.nrBodies(), bodies.size()); + BOOST_CHECK_EQUAL(static_cast(mb.nrBodies()), bodies.size()); // nrJoints - BOOST_CHECK_EQUAL(mb.nrJoints(), bodies.size()); + BOOST_CHECK_EQUAL(static_cast(mb.nrJoints()), bodies.size()); int params = 0, dof = 0; for(int i = 0; i < static_cast(joints.size()); ++i) { + const auto ui = static_cast(i); BOOST_CHECK_EQUAL(mb.jointPosInParam(i), params); - BOOST_CHECK_EQUAL(mb.jointsPosInParam()[i], params); + BOOST_CHECK_EQUAL(mb.jointsPosInParam()[ui], params); BOOST_CHECK_EQUAL(mb.sJointPosInParam(i), params); BOOST_CHECK_EQUAL(mb.jointPosInDof(i), dof); - BOOST_CHECK_EQUAL(mb.jointsPosInDof()[i], dof); + BOOST_CHECK_EQUAL(mb.jointsPosInDof()[ui], dof); BOOST_CHECK_EQUAL(mb.sJointPosInDof(i), dof); - params += joints[i].params(); - dof += joints[i].dof(); + params += joints[ui].params(); + dof += joints[ui].dof(); } BOOST_CHECK_EQUAL(params, mb.nrParams()); @@ -563,7 +564,8 @@ BOOST_AUTO_TEST_CASE(MakeMultiBodyTest) for(int i = 0; i < mb4.nrBodies(); ++i) { - BOOST_CHECK_EQUAL(mb4.body(i).inertia().momentum(), bCom[i]); + const auto ui = static_cast(i); + BOOST_CHECK_EQUAL(mb4.body(i).inertia().momentum(), bCom[ui]); } } @@ -683,13 +685,14 @@ BOOST_AUTO_TEST_CASE(MultiBodyConfigFunction2) std::vector zp = mb.joint(i).zeroParam(); std::vector zd = mb.joint(i).zeroDof(); - BOOST_CHECK_EQUAL_COLLECTIONS(mbc.q[i].begin(), mbc.q[i].end(), zp.begin(), zp.end()); - BOOST_CHECK_EQUAL_COLLECTIONS(mbc.alpha[i].begin(), mbc.alpha[i].end(), zd.begin(), zd.end()); - BOOST_CHECK_EQUAL_COLLECTIONS(mbc.alphaD[i].begin(), mbc.alphaD[i].end(), zd.begin(), zd.end()); - BOOST_CHECK_EQUAL_COLLECTIONS(mbc.jointTorque[i].begin(), mbc.jointTorque[i].end(), zd.begin(), zd.end()); + const auto ui = static_cast(i); + BOOST_CHECK_EQUAL_COLLECTIONS(mbc.q[ui].begin(), mbc.q[ui].end(), zp.begin(), zp.end()); + BOOST_CHECK_EQUAL_COLLECTIONS(mbc.alpha[ui].begin(), mbc.alpha[ui].end(), zd.begin(), zd.end()); + BOOST_CHECK_EQUAL_COLLECTIONS(mbc.alphaD[ui].begin(), mbc.alphaD[ui].end(), zd.begin(), zd.end()); + BOOST_CHECK_EQUAL_COLLECTIONS(mbc.jointTorque[ui].begin(), mbc.jointTorque[ui].end(), zd.begin(), zd.end()); } - for(int i = 0; i < mb.nrBodies(); ++i) + for(size_t i = 0; i < static_cast(mb.nrBodies()); ++i) { BOOST_CHECK_EQUAL(mbc.force[i].vector(), Vector6d::Zero()); } @@ -831,7 +834,8 @@ BOOST_AUTO_TEST_CASE(MultiBodyBaseTransformTest) // since the multibody is construct with the old body linking api // we must add an offset to joint origin of mb3 that correspond mb0 transform // to body base - mb3 = mbg.makeMultiBody("b3", true, mb3Surface * mb0ToBase["b3"] * mbc0.bodyPosW[mb0.bodyIndexByName("b3")], + mb3 = mbg.makeMultiBody("b3", true, + mb3Surface * mb0ToBase["b3"] * mbc0.bodyPosW[static_cast(mb0.bodyIndexByName("b3"))], mb3Surface); rbd::ConfigConverter mb0ToMb3(mb0, mb3); mbc3 = rbd::MultiBodyConfig(mb3); @@ -839,7 +843,8 @@ BOOST_AUTO_TEST_CASE(MultiBodyBaseTransformTest) rbd::forwardKinematics(mb3, mbc3); auto mb3ToBase = mbg.bodiesBaseTransform("b3", mb3Surface); - mb4 = mbg.makeMultiBody("b4", true, mb4Surface * mb0ToBase["b4"] * mbc0.bodyPosW[mb0.bodyIndexByName("b4")], + mb4 = mbg.makeMultiBody("b4", true, + mb4Surface * mb0ToBase["b4"] * mbc0.bodyPosW[static_cast(mb0.bodyIndexByName("b4"))], mb4Surface); mbc4 = rbd::MultiBodyConfig(mb4); rbd::ConfigConverter mb0ToMb4(mb0, mb4); @@ -850,13 +855,14 @@ BOOST_AUTO_TEST_CASE(MultiBodyBaseTransformTest) for(int bi = 0; bi < mb0.nrBodies(); ++bi) { std::string name = mb0.body(bi).name(); - int mb3Index = mb3.bodyIndexByName(name); - int mb4Index = mb4.bodyIndexByName(name); + const auto mb3Index = static_cast(mb3.bodyIndexByName(name)); + const auto mb4Index = static_cast(mb4.bodyIndexByName(name)); + const auto ubi = static_cast(bi); BOOST_CHECK_SMALL( - ((mb0ToBase[name] * mbc0.bodyPosW[bi]).matrix() - (mb3ToBase[name] * mbc3.bodyPosW[mb3Index]).matrix()).norm(), + ((mb0ToBase[name] * mbc0.bodyPosW[ubi]).matrix() - (mb3ToBase[name] * mbc3.bodyPosW[mb3Index]).matrix()).norm(), 1e-8); BOOST_CHECK_SMALL( - ((mb0ToBase[name] * mbc0.bodyPosW[bi]).matrix() - (mb4ToBase[name] * mbc4.bodyPosW[mb4Index]).matrix()).norm(), + ((mb0ToBase[name] * mbc0.bodyPosW[ubi]).matrix() - (mb4ToBase[name] * mbc4.bodyPosW[mb4Index]).matrix()).norm(), 1e-8); } } From 786438ddaadcf08b6eb1f26c19f1fdfa451a108e Mon Sep 17 00:00:00 2001 From: Benjamin Navarro Date: Wed, 6 May 2020 14:04:56 +0200 Subject: [PATCH 03/15] docs(doxygen): minors fixes in Jacobian constructors doc --- src/RBDyn/RBDyn/Jacobian.h | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/RBDyn/RBDyn/Jacobian.h b/src/RBDyn/RBDyn/Jacobian.h index 2c7866d1..b67d077d 100644 --- a/src/RBDyn/RBDyn/Jacobian.h +++ b/src/RBDyn/RBDyn/Jacobian.h @@ -42,20 +42,20 @@ class RBDYN_DLLAPI Jacobian /** * Create a jacobian from the root body to the specified body. - * @param mb Multibody where bodyId is in. + * @param mb Multibody where bodyName is in. * @param bodyName Specified body. - * @param point Point in the body exprimed in body coordinate. - * @throw std::out_of_range If bodyId don't exist. + * @param point Point in the body expressed in body coordinate. + * @throw std::out_of_range If bodyName don't exist. */ Jacobian(const MultiBody & mb, const std::string & bodyName, const Eigen::Vector3d & point = Eigen::Vector3d::Zero()); /** * Create a jacobian from a reference body to the specified body. - * @param mb Multibody where bodyId is in. + * @param mb Multibody where bodyName is in. * @param bodyName Specified body. * @param refBodyName Reference body. - * @param point Point in the body exprimed in body coordinate. - * @throw std::out_of_range If bodyId don't exist. + * @param point Point in the body expressed in body coordinate. + * @throw std::out_of_range If bodyName don't exist. */ Jacobian(const MultiBody & mb, const std::string & bodyName, From 8eabbfe8452da592c3a9de86e8e0c8e8a903fc9d Mon Sep 17 00:00:00 2001 From: Benjamin Navarro Date: Wed, 6 May 2020 14:37:54 +0200 Subject: [PATCH 04/15] perf(jacobian): various performance improvements As suggested in PR #72 --- src/RBDyn/Jacobian.cpp | 64 ++++++++++++++------------------------ src/RBDyn/RBDyn/Jacobian.h | 2 +- 2 files changed, 24 insertions(+), 42 deletions(-) diff --git a/src/RBDyn/Jacobian.cpp b/src/RBDyn/Jacobian.cpp index 53aa3196..3933ac1b 100644 --- a/src/RBDyn/Jacobian.cpp +++ b/src/RBDyn/Jacobian.cpp @@ -8,6 +8,7 @@ // includes // std #include +#include #include // RBDyn @@ -19,25 +20,8 @@ namespace rbd Jacobian::Jacobian() {} Jacobian::Jacobian(const MultiBody & mb, const std::string & bodyName, const Eigen::Vector3d & point) -: jointsPath_(), point_(point), jac_(), jacDot_() +: Jacobian(mb, bodyName, mb.body(0).name(), point) { - body_index_ = mb.sBodyIndexByName(bodyName); - ref_index_ = -1; - - int index = body_index_; - - int dof = 0; - while(index != -1) - { - jointsPath_.insert(jointsPath_.begin(), index); - dof += mb.joint(index).dof(); - reverseJoints_.insert(reverseJoints_.begin(), false); - - index = mb.parent(index); - } - - jac_.resize(6, dof); - jacDot_.resize(6, dof); } Jacobian::Jacobian(const MultiBody & mb, @@ -60,9 +44,10 @@ Jacobian::Jacobian(const MultiBody & mb, reverseJoints_.insert(reverseJoints_.begin(), false); if(index == ref_index_) - refindexFound = true; - else - index = mb.parent(index); + { + break; + } + index = mb.parent(index); } // The two bodies don't belong to the same branch -> start from ref body to the root of the tree @@ -79,18 +64,18 @@ Jacobian::Jacobian(const MultiBody & mb, index = mb.parent(index); count++; - } while(std::find(jointsPath_.begin(), jointsPath_.end(), index) == jointsPath_.end()); + } while(std::find(jointsPath_.begin() + count, jointsPath_.end(), index) == jointsPath_.end()); // Delete common joints previously added - bool commonJoints = true; - while(commonJoints) + int commonIdx = count; + while(jointsPath_[static_cast(++commonIdx)] != index) { - const auto joint_index = jointsPath_[static_cast(count)]; - if(joint_index == index) commonJoints = false; - dof -= mb.joint(joint_index).dof(); - jointsPath_.erase(jointsPath_.begin() + count); - reverseJoints_.erase(reverseJoints_.begin() + count); + // Get to the common node } + dof -= std::accumulate(jointsPath_.begin() + count, jointsPath_.begin() + commonIdx, 0, + [&](int dofC, int idx) { return dofC + mb.joint(idx).dof(); }); + jointsPath_.erase(jointsPath_.begin() + count, jointsPath_.begin() + commonIdx); + reverseJoints_.erase(reverseJoints_.begin() + count, reverseJoints_.begin() + commonIdx); } jac_.resize(6, dof); @@ -130,7 +115,10 @@ std::vector Jacobian::dofPath(const MultiBody & mb) const std::vector dof_path; for(size_t i = 0; i < jointsPath_.size(); ++i) { - for(int j = 0; j < mb.joint(jointsPath_[i]).dof(); ++j) dof_path.push_back(jointsPath_[i]); + for(int j = 0; j < mb.joint(jointsPath_[i]).dof(); ++j) + { + dof_path.push_back(jointsPath_[i]); + } } return dof_path; } @@ -143,7 +131,7 @@ static inline const Eigen::MatrixXd & jacobian_(const MultiBody & mb, const MultiBodyConfig & mbc, const Transform & Trans_0_p, const std::vector & jointsPath, - const std::vector & reverseJoints, + const std::vector & reverseJoints, Eigen::MatrixXd & jac, int ref_index) { @@ -158,23 +146,17 @@ static inline const Eigen::MatrixXd & jacobian_(const MultiBody & mb, sva::PTransformd X_i_N = X_0_p * mbc.bodyPosW[i].inv(); + // If the joint motion is seen from child body to parent body, we have to inverted its effect + double sgn = reverseJoints[index] ? -1 : 1; for(int dof = 0; dof < joints[i].dof(); ++dof) { - if(!reverseJoints[index]) - { - jac.col(curJ + dof).noalias() = (X_i_N * (sva::MotionVecd(mbc.motionSubspace[i].col(dof)))).vector(); - } - else - { - // The joint motion is seen from child body to parent body, so has inverted effect - jac.col(curJ + dof).noalias() = (X_i_N * (-sva::MotionVecd(mbc.motionSubspace[i].col(dof)))).vector(); - } + jac.col(curJ + dof).noalias() = (X_i_N * (sva::MotionVecd(sgn * mbc.motionSubspace[i].col(dof)))).vector(); } curJ += joints[i].dof(); } // Change Jacobian base : root of the tree --> reference body - if(ref_index != -1) + if(ref_index > 0) { auto dof_count = jac.cols(); const auto joint_index = static_cast(jointsPath[0]); diff --git a/src/RBDyn/RBDyn/Jacobian.h b/src/RBDyn/RBDyn/Jacobian.h index b67d077d..771b05a9 100644 --- a/src/RBDyn/RBDyn/Jacobian.h +++ b/src/RBDyn/RBDyn/Jacobian.h @@ -468,7 +468,7 @@ class RBDYN_DLLAPI Jacobian private: std::vector jointsPath_; - std::vector reverseJoints_; + std::vector reverseJoints_; sva::PTransformd point_; Eigen::MatrixXd jac_; From c924ad18fa02ceff0bd57b750a2244f79a9aaeba Mon Sep 17 00:00:00 2001 From: Sonny Tarbouriech Date: Wed, 6 May 2020 16:05:40 +0200 Subject: [PATCH 05/15] Fixed mixing of joint and body index in the Jacobian computation --- src/RBDyn/Jacobian.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/RBDyn/Jacobian.cpp b/src/RBDyn/Jacobian.cpp index 3933ac1b..a2bd851b 100644 --- a/src/RBDyn/Jacobian.cpp +++ b/src/RBDyn/Jacobian.cpp @@ -159,9 +159,8 @@ static inline const Eigen::MatrixXd & jacobian_(const MultiBody & mb, if(ref_index > 0) { auto dof_count = jac.cols(); - const auto joint_index = static_cast(jointsPath[0]); - jac.block(0, 0, 3, dof_count) = mbc.bodyPosW[joint_index].rotation() * jac.block(0, 0, 3, dof_count); - jac.block(3, 0, 3, dof_count) = mbc.bodyPosW[joint_index].rotation() * jac.block(3, 0, 3, dof_count); + jac.block(0, 0, 3, dof_count) = mbc.bodyPosW[ref_index].rotation() * jac.block(0, 0, 3, dof_count); + jac.block(3, 0, 3, dof_count) = mbc.bodyPosW[ref_index].rotation() * jac.block(3, 0, 3, dof_count); } return jac; From f7a18281f84cc84822b55c2ebf6eb4eb2f320783 Mon Sep 17 00:00:00 2001 From: Sonny Tarbouriech Date: Wed, 10 Jun 2020 16:29:19 +0200 Subject: [PATCH 06/15] Changes in the Jacobian class to make the reference body feature compatible with the rest of the class + implemented some tests for this feature --- src/RBDyn/FV.cpp | 14 ++---- src/RBDyn/Jacobian.cpp | 97 +++++++++++++++++++++++--------------- src/RBDyn/RBDyn/Jacobian.h | 7 +-- tests/JacobianTest.cpp | 97 ++++++++++++++++++++++++++++++++++---- tests/XXXdualarm.h | 80 +++++++++++++++++++++++++++++++ 5 files changed, 234 insertions(+), 61 deletions(-) create mode 100644 tests/XXXdualarm.h diff --git a/src/RBDyn/FV.cpp b/src/RBDyn/FV.cpp index 58bf5b92..cc03e3c2 100644 --- a/src/RBDyn/FV.cpp +++ b/src/RBDyn/FV.cpp @@ -16,27 +16,23 @@ namespace rbd void forwardVelocity(const MultiBody & mb, MultiBodyConfig & mbc) { - const std::vector & joints = mb.joints(); const std::vector & pred = mb.predecessors(); const std::vector & succ = mb.successors(); for(std::size_t i = 0; i < joints.size(); ++i) { - const auto pred_index = static_cast(pred[i]); - const auto succ_index = static_cast(succ[i]); - const sva::PTransformd & X_p_i = mbc.parentToSon[i]; mbc.jointVelocity[i] = joints[i].motion(mbc.alpha[i]); if(pred[i] != -1) - mbc.bodyVelB[succ_index] = X_p_i * mbc.bodyVelB[pred_index] + mbc.jointVelocity[i]; + mbc.bodyVelB[succ[i]] = X_p_i * mbc.bodyVelB[pred[i]] + mbc.jointVelocity[i]; else - mbc.bodyVelB[succ_index] = mbc.jointVelocity[i]; + mbc.bodyVelB[succ[i]] = mbc.jointVelocity[i]; - sva::PTransformd E_0_i(mbc.bodyPosW[succ_index].rotation()); - mbc.bodyVelW[succ_index] = E_0_i.invMul(mbc.bodyVelB[succ_index]); + sva::PTransformd E_0_i(mbc.bodyPosW[succ[i]].rotation()); + mbc.bodyVelW[succ[i]] = E_0_i.invMul(mbc.bodyVelB[succ[i]]); } } @@ -54,4 +50,4 @@ void sForwardVelocity(const MultiBody & mb, MultiBodyConfig & mbc) forwardVelocity(mb, mbc); } -} // namespace rbd +} // namespace rbd \ No newline at end of file diff --git a/src/RBDyn/Jacobian.cpp b/src/RBDyn/Jacobian.cpp index a2bd851b..7fec5104 100644 --- a/src/RBDyn/Jacobian.cpp +++ b/src/RBDyn/Jacobian.cpp @@ -30,10 +30,10 @@ Jacobian::Jacobian(const MultiBody & mb, const Eigen::Vector3d & point) : jointsPath_(), point_(point), jac_(), jacDot_() { - body_index_ = mb.sBodyIndexByName(bodyName); - ref_index_ = mb.sBodyIndexByName(refBodyName); + bodyIndex_ = mb.sBodyIndexByName(bodyName); + refBodyIndex_ = mb.sBodyIndexByName(refBodyName); - int index = body_index_; + int index = bodyIndex_; int dof = 0; bool refindexFound = false; @@ -43,7 +43,7 @@ Jacobian::Jacobian(const MultiBody & mb, dof += mb.joint(index).dof(); reverseJoints_.insert(reverseJoints_.begin(), false); - if(index == ref_index_) + if(index == refBodyIndex_) { break; } @@ -51,9 +51,9 @@ Jacobian::Jacobian(const MultiBody & mb, } // The two bodies don't belong to the same branch -> start from ref body to the root of the tree - if(index != ref_index_) + if(index != refBodyIndex_) { - index = ref_index_; + index = refBodyIndex_; int count = 0; // Add joints until reaching the common node do @@ -72,10 +72,10 @@ Jacobian::Jacobian(const MultiBody & mb, { // Get to the common node } - dof -= std::accumulate(jointsPath_.begin() + count, jointsPath_.begin() + commonIdx, 0, + dof -= std::accumulate(jointsPath_.begin() + count, jointsPath_.begin() + commonIdx + 1, 0, [&](int dofC, int idx) { return dofC + mb.joint(idx).dof(); }); - jointsPath_.erase(jointsPath_.begin() + count, jointsPath_.begin() + commonIdx); - reverseJoints_.erase(reverseJoints_.begin() + count, reverseJoints_.begin() + commonIdx); + jointsPath_.erase(jointsPath_.begin() + count, jointsPath_.begin() + commonIdx + 1); + reverseJoints_.erase(reverseJoints_.begin() + count, reverseJoints_.begin() + commonIdx + 1); } jac_.resize(6, dof); @@ -94,33 +94,47 @@ MultiBody Jacobian::subMultiBody(const MultiBody & mb) const for(int index = 0; index < static_cast(jointsPath_.size()); ++index) { - int i = jointsPath_[static_cast(index)]; + int i = jointsPath_[index]; + // body info bodies.push_back(mb.body(i)); parent.push_back(index - 1); // joint info - joints.push_back(mb.joint(i)); succ.push_back(index); pred.push_back(index - 1); - Xt.push_back(mb.transform(i)); - } - - return MultiBody(std::move(bodies), std::move(joints), std::move(pred), std::move(succ), std::move(parent), - std::move(Xt)); -} -std::vector Jacobian::dofPath(const MultiBody & mb) const -{ - std::vector dof_path; - for(size_t i = 0; i < jointsPath_.size(); ++i) - { - for(int j = 0; j < mb.joint(jointsPath_[i]).dof(); ++j) + if (index == 0 ) + { + if (reverseJoints_[index]) + Xt.push_back(sva::PTransformd(Eigen::Vector3d(0., 0., 0.))); + else + Xt.push_back(mb.transform(i)); + } + else { - dof_path.push_back(jointsPath_[i]); + if (reverseJoints_[index-1]) + { + if (reverseJoints_[index]) + Xt.push_back(mb.transform(jointsPath_[index-1]).inv()); + else + Xt.push_back(mb.transform(jointsPath_[index-1]).inv()*mb.transform(jointsPath_[index])); + } + else + { + Xt.push_back(mb.transform(i)); + } } + auto joint = mb.joint(i); + if (reverseJoints_[index]) + { + auto fwd = joint.forward() ? false : true; + joint.forward(fwd); + } + joints.push_back(joint); } - return dof_path; + return MultiBody(std::move(bodies), std::move(joints), std::move(pred), std::move(succ), std::move(parent), + std::move(Xt)); } /// private implementation of the Jacobian computation @@ -132,8 +146,7 @@ static inline const Eigen::MatrixXd & jacobian_(const MultiBody & mb, const Transform & Trans_0_p, const std::vector & jointsPath, const std::vector & reverseJoints, - Eigen::MatrixXd & jac, - int ref_index) + Eigen::MatrixXd & jac) { const std::vector & joints = mb.joints(); int curJ = 0; @@ -155,14 +168,6 @@ static inline const Eigen::MatrixXd & jacobian_(const MultiBody & mb, curJ += joints[i].dof(); } - // Change Jacobian base : root of the tree --> reference body - if(ref_index > 0) - { - auto dof_count = jac.cols(); - jac.block(0, 0, 3, dof_count) = mbc.bodyPosW[ref_index].rotation() * jac.block(0, 0, 3, dof_count); - jac.block(3, 0, 3, dof_count) = mbc.bodyPosW[ref_index].rotation() * jac.block(3, 0, 3, dof_count); - } - return jac; } @@ -170,7 +175,15 @@ const Eigen::MatrixXd & Jacobian::jacobian(const MultiBody & mb, const MultiBodyConfig & mbc, const sva::PTransformd & X_0_p) { - return jacobian_(mb, mbc, X_0_p, jointsPath_, reverseJoints_, jac_, ref_index_); + jacobian_(mb, mbc, X_0_p, jointsPath_, reverseJoints_, jac_); + // Change Jacobian base : root of the tree --> reference body + if(refBodyIndex_ > 0) + { + auto dof_count = jac_.cols(); + jac_.block(0, 0, 3, dof_count) = mbc.bodyPosW[refBodyIndex_].rotation() * jac_.block(0, 0, 3, dof_count); + jac_.block(3, 0, 3, dof_count) = mbc.bodyPosW[refBodyIndex_].rotation() * jac_.block(3, 0, 3, dof_count); + } + return jac_; } const Eigen::MatrixXd & Jacobian::jacobian(const MultiBody & mb, const MultiBodyConfig & mbc) @@ -179,7 +192,15 @@ const Eigen::MatrixXd & Jacobian::jacobian(const MultiBody & mb, const MultiBody // the transformation must be read {}^0E_p {}^pT_N {}^NX_0 Eigen::Vector3d T_0_Np((point_ * mbc.bodyPosW[N]).translation()); - return jacobian_(mb, mbc, T_0_Np, jointsPath_, reverseJoints_, jac_, ref_index_); + jacobian_(mb, mbc, T_0_Np, jointsPath_, reverseJoints_, jac_); + // Change Jacobian base : root of the tree --> reference body + if(refBodyIndex_ > 0) + { + auto dof_count = jac_.cols(); + jac_.block(0, 0, 3, dof_count) = mbc.bodyPosW[refBodyIndex_].rotation() * jac_.block(0, 0, 3, dof_count); + jac_.block(3, 0, 3, dof_count) = mbc.bodyPosW[refBodyIndex_].rotation() * jac_.block(3, 0, 3, dof_count); + } + return jac_; } const Eigen::MatrixXd & Jacobian::bodyJacobian(const MultiBody & mb, const MultiBodyConfig & mbc) @@ -187,7 +208,7 @@ const Eigen::MatrixXd & Jacobian::bodyJacobian(const MultiBody & mb, const Multi auto N = static_cast(jointsPath_.back()); sva::PTransformd X_0_Np = point_ * mbc.bodyPosW[N]; - return jacobian_(mb, mbc, X_0_Np, jointsPath_, reverseJoints_, jac_, ref_index_); + return jacobian_(mb, mbc, X_0_Np, jointsPath_, reverseJoints_, jac_); } const Eigen::MatrixXd & Jacobian::vectorJacobian(const MultiBody & mb, diff --git a/src/RBDyn/RBDyn/Jacobian.h b/src/RBDyn/RBDyn/Jacobian.h index 771b05a9..e4b8b88f 100644 --- a/src/RBDyn/RBDyn/Jacobian.h +++ b/src/RBDyn/RBDyn/Jacobian.h @@ -320,9 +320,6 @@ class RBDYN_DLLAPI Jacobian return jointsPath_; } - /// @return The dof path vector from the root to the specified body. - std::vector dofPath(const MultiBody & mb) const; - /// @return The number of degree of freedom in the joint path int dof() const { @@ -474,8 +471,8 @@ class RBDYN_DLLAPI Jacobian Eigen::MatrixXd jac_; Eigen::MatrixXd jacDot_; - int body_index_; - int ref_index_; + int bodyIndex_; + int refBodyIndex_; }; } // namespace rbd diff --git a/tests/JacobianTest.cpp b/tests/JacobianTest.cpp index 54fffbc2..fb135d48 100644 --- a/tests/JacobianTest.cpp +++ b/tests/JacobianTest.cpp @@ -29,6 +29,7 @@ // Arm #include "SSSarm.h" #include "XYZSarm.h" +#include "XXXdualarm.h" const double TOL = 0.0000001; @@ -139,18 +140,17 @@ void checkJacobianMatrixFromVelocity(const rbd::MultiBody & subMb, int col = 0; for(int i = 0; i < subMb.nrJoints(); ++i) { - const auto ui = static_cast(i); for(int j = 0; j < subMb.joint(i).dof(); ++j) { - const auto uj = static_cast(j); - subMbc.alpha[ui][uj] = 1.; + subMbc.alpha[i][j] = 1.; forwardVelocity(subMb, subMbc); Eigen::Vector6d mv = velVec.back().vector(); + BOOST_CHECK_SMALL((mv - jacMat.col(col)).norm(), TOL); - subMbc.alpha[ui][uj] = 0.; + subMbc.alpha[i][j] = 0.; ++col; } } @@ -181,7 +181,8 @@ void checkFullJacobianMatrix(const rbd::MultiBody & mb, { const auto ui = static_cast(i); int joint = jac.jointsPath()[ui]; - int dof = mb.joint(i).dof(); + int dof = subMb.joint(i).dof(); + BOOST_CHECK_EQUAL(jacMat.block(0, subMb.jointPosInDof(i), 6, dof), fullJacMat.block(0, mb.jointPosInDof(joint), 6, dof)); } @@ -198,14 +199,47 @@ void checkJacobian(const rbd::MultiBody & mb, const rbd::MultiBodyConfig & mbc, // fill subMbc MultiBodyConfig subMbc(subMb); + for(int i = 0; i < subMb.nrJoints(); ++i) + { + subMbc.bodyPosW[i] = mbc.bodyPosW[jac.jointsPath()[i]]; + subMbc.jointConfig[i] = mbc.jointConfig[jac.jointsPath()[i]]; + subMbc.parentToSon[i] = mbc.parentToSon[jac.jointsPath()[i]]; + } + + // test fullJacobian + checkFullJacobianMatrix(mb, subMb, jac, jac_mat); + + // test jacobian + const MatrixXd & jac_mat_w = jac.jacobian(mb, mbc); + checkJacobianMatrixSize(subMb, jac_mat_w); + checkJacobianMatrixFromVelocity(subMb, subMbc, subMbc.bodyVelW, jac_mat_w); + + // test bodyJacobian + const MatrixXd & jac_mat_b = jac.bodyJacobian(mb, mbc); + checkJacobianMatrixSize(subMb, jac_mat_b); + checkJacobianMatrixFromVelocity(subMb, subMbc, subMbc.bodyVelB, jac_mat_w); +} + +void checkJacobianRefBody(const rbd::MultiBody & mb, const rbd::MultiBodyConfig & mbc, rbd::Jacobian & jac) +{ + using namespace Eigen; + using namespace sva; + using namespace rbd; + + const MatrixXd & jac_mat = jac.jacobian(mb, mbc); + MultiBody subMb = jac.subMultiBody(mb); + + // fill subMbc + MultiBodyConfig subMbc(subMb); + for(size_t i = 0; i < static_cast(subMb.nrJoints()); ++i) { - const auto joint_index = static_cast(jac.jointsPath()[i]); - subMbc.bodyPosW[i] = mbc.bodyPosW[joint_index]; - subMbc.jointConfig[i] = mbc.jointConfig[joint_index]; - subMbc.parentToSon[i] = mbc.parentToSon[joint_index]; + subMbc.q[i] = mbc.q[jac.jointsPath()[i]]; } + forwardKinematics(subMb, subMbc); + forwardVelocity(subMb, subMbc); + // test fullJacobian checkFullJacobianMatrix(mb, subMb, jac, jac_mat); @@ -220,6 +254,7 @@ void checkJacobian(const rbd::MultiBody & mb, const rbd::MultiBodyConfig & mbc, checkJacobianMatrixFromVelocity(subMb, subMbc, subMbc.bodyVelB, jac_mat_w); } + BOOST_AUTO_TEST_CASE(JacobianComputeTest) { using namespace Eigen; @@ -263,6 +298,50 @@ BOOST_AUTO_TEST_CASE(JacobianComputeTest) BOOST_CHECK_THROW(jac1.sJacobian(mbErr, mbcErr), std::domain_error); } +BOOST_AUTO_TEST_CASE(JacobianRefBodyTest) +{ + using namespace Eigen; + using namespace sva; + using namespace rbd; + namespace cst = boost::math::constants; + + MultiBody mb; + MultiBodyConfig mbc; + MultiBodyGraph mbg; + std::tie(mb, mbc, mbg) = makeXXXdualarm(false); + + Jacobian jac1(mb, "b31", "b32"); + Jacobian jac2(mb, "b32", "b31"); + + mbc.q = {{},{0.}, {0.}, {0.}, {0.}, {0.}}; + forwardKinematics(mb, mbc); + forwardVelocity(mb, mbc); + + checkJacobianRefBody(mb, mbc, jac1); + checkJacobianRefBody(mb, mbc, jac2); + + mbc.q = {{}, {cst::pi() / 2.}, {cst::pi() / 3.}, {cst::pi() / 4.}, {cst::pi() / 5.}, {cst::pi() / 6.}}; + forwardKinematics(mb, mbc); + forwardVelocity(mb, mbc); + + checkJacobianRefBody(mb, mbc, jac1); + checkJacobianRefBody(mb, mbc, jac2); + + // test jacobian safe version + MultiBodyConfig mbcBadNrBodyPos(mbc); + mbcBadNrBodyPos.bodyPosW.resize(1); + BOOST_CHECK_THROW(jac1.sJacobian(mb, mbcBadNrBodyPos), std::domain_error); + + MultiBodyConfig mbcBadNrJointConf(mbc); + mbcBadNrJointConf.motionSubspace.resize(1); + BOOST_CHECK_THROW(jac1.sJacobian(mb, mbcBadNrJointConf), std::domain_error); + + MultiBody mbErr = jac2.subMultiBody(mb); + MultiBodyConfig mbcErr(mbErr); + BOOST_CHECK_THROW(jac1.sJacobian(mbErr, mbcErr), std::domain_error); +} + + BOOST_AUTO_TEST_CASE(JacobianComputeTestFreeFlyer) { using namespace Eigen; diff --git a/tests/XXXdualarm.h b/tests/XXXdualarm.h new file mode 100644 index 00000000..3071e9f3 --- /dev/null +++ b/tests/XXXdualarm.h @@ -0,0 +1,80 @@ +/* + * Copyright 2012-2019 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#pragma once + +// includes +// std +#include + +// RBDyn +#include "RBDyn/Body.h" +#include "RBDyn/Joint.h" +#include "RBDyn/MultiBody.h" +#include "RBDyn/MultiBodyConfig.h" +#include "RBDyn/MultiBodyGraph.h" + +/// @return A simple XXX arm with Y as up axis. +std::tuple makeXXXdualarm(bool isFixed = true) +{ + using namespace Eigen; + using namespace sva; + using namespace rbd; + + MultiBodyGraph mbg; + + double mass = 1.; + Matrix3d I = Matrix3d::Identity(); + Vector3d h = Vector3d::Zero(); + + RBInertiad rbi(mass, h, I); + + Body b0(rbi, "b0"); + Body b1(rbi, "b1"); + Body b21(rbi, "b21"); + Body b22(rbi, "b22"); + Body b31(rbi, "b31"); + Body b32(rbi, "b32"); + + mbg.addBody(b0); + mbg.addBody(b1); + mbg.addBody(b21); + mbg.addBody(b22); + mbg.addBody(b31); + mbg.addBody(b32); + + Joint j0(Joint::RevX, true, "j0"); + Joint j11(Joint::RevX, true, "j11"); + Joint j12(Joint::RevX, true, "j12"); + Joint j21(Joint::RevX, true, "j21"); + Joint j22(Joint::RevX, true, "j22"); + + mbg.addJoint(j0); + mbg.addJoint(j11); + mbg.addJoint(j12); + mbg.addJoint(j21); + mbg.addJoint(j22); + + // Root j0 j1 j2 + // ---- b0 ---- b1 ---- b2 ----b3 + // Base RevX RevX RevX + + PTransformd to(Vector3d(0., 1., 0.)); + PTransformd to1(Vector3d(0., 1., 1.)); + PTransformd to2(Vector3d(0., 1., -1.)); + PTransformd from(Vector3d(0., -1., 0.)); + + mbg.linkBodies("b0", to, "b1", from, "j0"); + mbg.linkBodies("b1", to1, "b21", from, "j11"); + mbg.linkBodies("b1", to2, "b22", from, "j12"); + mbg.linkBodies("b21", to, "b31", from, "j21"); + mbg.linkBodies("b22", to, "b32", from, "j22"); + + MultiBody mb = mbg.makeMultiBody("b0", isFixed); + + MultiBodyConfig mbc(mb); + mbc.zero(mb); + + return std::make_tuple(mb, mbc, mbg); +} From 947b416d28f5421249ef9c7056fdc60efb0a465e Mon Sep 17 00:00:00 2001 From: Sonny Tarbouriech Date: Wed, 10 Jun 2020 16:39:00 +0200 Subject: [PATCH 07/15] Fixed clang-format errors --- src/RBDyn/Jacobian.cpp | 14 +++++++------- tests/JacobianTest.cpp | 13 ++++++++----- 2 files changed, 15 insertions(+), 12 deletions(-) diff --git a/src/RBDyn/Jacobian.cpp b/src/RBDyn/Jacobian.cpp index 7fec5104..a691415b 100644 --- a/src/RBDyn/Jacobian.cpp +++ b/src/RBDyn/Jacobian.cpp @@ -104,21 +104,21 @@ MultiBody Jacobian::subMultiBody(const MultiBody & mb) const succ.push_back(index); pred.push_back(index - 1); - if (index == 0 ) + if(index == 0) { - if (reverseJoints_[index]) + if(reverseJoints_[index]) Xt.push_back(sva::PTransformd(Eigen::Vector3d(0., 0., 0.))); else Xt.push_back(mb.transform(i)); } else { - if (reverseJoints_[index-1]) + if(reverseJoints_[index - 1]) { - if (reverseJoints_[index]) - Xt.push_back(mb.transform(jointsPath_[index-1]).inv()); + if(reverseJoints_[index]) + Xt.push_back(mb.transform(jointsPath_[index - 1]).inv()); else - Xt.push_back(mb.transform(jointsPath_[index-1]).inv()*mb.transform(jointsPath_[index])); + Xt.push_back(mb.transform(jointsPath_[index - 1]).inv() * mb.transform(jointsPath_[index])); } else { @@ -126,7 +126,7 @@ MultiBody Jacobian::subMultiBody(const MultiBody & mb) const } } auto joint = mb.joint(i); - if (reverseJoints_[index]) + if(reverseJoints_[index]) { auto fwd = joint.forward() ? false : true; joint.forward(fwd); diff --git a/tests/JacobianTest.cpp b/tests/JacobianTest.cpp index fb135d48..b4422100 100644 --- a/tests/JacobianTest.cpp +++ b/tests/JacobianTest.cpp @@ -28,8 +28,8 @@ // Arm #include "SSSarm.h" -#include "XYZSarm.h" #include "XXXdualarm.h" +#include "XYZSarm.h" const double TOL = 0.0000001; @@ -254,7 +254,6 @@ void checkJacobianRefBody(const rbd::MultiBody & mb, const rbd::MultiBodyConfig checkJacobianMatrixFromVelocity(subMb, subMbc, subMbc.bodyVelB, jac_mat_w); } - BOOST_AUTO_TEST_CASE(JacobianComputeTest) { using namespace Eigen; @@ -313,14 +312,19 @@ BOOST_AUTO_TEST_CASE(JacobianRefBodyTest) Jacobian jac1(mb, "b31", "b32"); Jacobian jac2(mb, "b32", "b31"); - mbc.q = {{},{0.}, {0.}, {0.}, {0.}, {0.}}; + mbc.q = {{}, {0.}, {0.}, {0.}, {0.}, {0.}}; forwardKinematics(mb, mbc); forwardVelocity(mb, mbc); checkJacobianRefBody(mb, mbc, jac1); checkJacobianRefBody(mb, mbc, jac2); - mbc.q = {{}, {cst::pi() / 2.}, {cst::pi() / 3.}, {cst::pi() / 4.}, {cst::pi() / 5.}, {cst::pi() / 6.}}; + mbc.q = {{}, + {cst::pi() / 2.}, + {cst::pi() / 3.}, + {cst::pi() / 4.}, + {cst::pi() / 5.}, + {cst::pi() / 6.}}; forwardKinematics(mb, mbc); forwardVelocity(mb, mbc); @@ -341,7 +345,6 @@ BOOST_AUTO_TEST_CASE(JacobianRefBodyTest) BOOST_CHECK_THROW(jac1.sJacobian(mbErr, mbcErr), std::domain_error); } - BOOST_AUTO_TEST_CASE(JacobianComputeTestFreeFlyer) { using namespace Eigen; From b1d42f489a6a1281f21e15257121d68f7a660f7c Mon Sep 17 00:00:00 2001 From: Sonny Tarbouriech Date: Wed, 10 Jun 2020 17:18:15 +0200 Subject: [PATCH 08/15] Fixed minor errors related to the previous changes --- src/RBDyn/FV.cpp | 11 +++++++---- src/RBDyn/Jacobian.cpp | 31 +++++++++++++++---------------- src/RBDyn/RBDyn/Jacobian.h | 2 +- tests/JacobianTest.cpp | 6 ++++-- 4 files changed, 27 insertions(+), 23 deletions(-) diff --git a/src/RBDyn/FV.cpp b/src/RBDyn/FV.cpp index cc03e3c2..8f05013d 100644 --- a/src/RBDyn/FV.cpp +++ b/src/RBDyn/FV.cpp @@ -22,17 +22,20 @@ void forwardVelocity(const MultiBody & mb, MultiBodyConfig & mbc) for(std::size_t i = 0; i < joints.size(); ++i) { + const auto pred_index = static_cast(pred[i]); + const auto succ_index = static_cast(succ[i]); + const sva::PTransformd & X_p_i = mbc.parentToSon[i]; mbc.jointVelocity[i] = joints[i].motion(mbc.alpha[i]); if(pred[i] != -1) - mbc.bodyVelB[succ[i]] = X_p_i * mbc.bodyVelB[pred[i]] + mbc.jointVelocity[i]; + mbc.bodyVelB[succ_index] = X_p_i * mbc.bodyVelB[pred_index] + mbc.jointVelocity[i]; else - mbc.bodyVelB[succ[i]] = mbc.jointVelocity[i]; + mbc.bodyVelB[succ_index] = mbc.jointVelocity[i]; - sva::PTransformd E_0_i(mbc.bodyPosW[succ[i]].rotation()); - mbc.bodyVelW[succ[i]] = E_0_i.invMul(mbc.bodyVelB[succ[i]]); + sva::PTransformd E_0_i(mbc.bodyPosW[succ_index].rotation()); + mbc.bodyVelW[succ_index] = E_0_i.invMul(mbc.bodyVelB[succ_index]); } } diff --git a/src/RBDyn/Jacobian.cpp b/src/RBDyn/Jacobian.cpp index a691415b..d3893e29 100644 --- a/src/RBDyn/Jacobian.cpp +++ b/src/RBDyn/Jacobian.cpp @@ -36,12 +36,11 @@ Jacobian::Jacobian(const MultiBody & mb, int index = bodyIndex_; int dof = 0; - bool refindexFound = false; - while(!refindexFound && index != -1) + while(index != -1) { jointsPath_.insert(jointsPath_.begin(), index); dof += mb.joint(index).dof(); - reverseJoints_.insert(reverseJoints_.begin(), false); + jointsSign_.insert(jointsSign_.begin(), 1); if(index == refBodyIndex_) { @@ -60,7 +59,7 @@ Jacobian::Jacobian(const MultiBody & mb, { jointsPath_.insert(jointsPath_.begin() + count, index); dof += mb.joint(index).dof(); - reverseJoints_.insert(reverseJoints_.begin(), true); + jointsSign_.insert(jointsSign_.begin(), -1); index = mb.parent(index); count++; @@ -75,7 +74,7 @@ Jacobian::Jacobian(const MultiBody & mb, dof -= std::accumulate(jointsPath_.begin() + count, jointsPath_.begin() + commonIdx + 1, 0, [&](int dofC, int idx) { return dofC + mb.joint(idx).dof(); }); jointsPath_.erase(jointsPath_.begin() + count, jointsPath_.begin() + commonIdx + 1); - reverseJoints_.erase(reverseJoints_.begin() + count, reverseJoints_.begin() + commonIdx + 1); + jointsSign_.erase(jointsSign_.begin() + count, jointsSign_.begin() + commonIdx + 1); } jac_.resize(6, dof); @@ -106,16 +105,16 @@ MultiBody Jacobian::subMultiBody(const MultiBody & mb) const if(index == 0) { - if(reverseJoints_[index]) + if(jointsSign_[index] == -1) Xt.push_back(sva::PTransformd(Eigen::Vector3d(0., 0., 0.))); else Xt.push_back(mb.transform(i)); } else { - if(reverseJoints_[index - 1]) + if(jointsSign_[index - 1] == -1) { - if(reverseJoints_[index]) + if(jointsSign_[index] == -1) Xt.push_back(mb.transform(jointsPath_[index - 1]).inv()); else Xt.push_back(mb.transform(jointsPath_[index - 1]).inv() * mb.transform(jointsPath_[index])); @@ -126,7 +125,7 @@ MultiBody Jacobian::subMultiBody(const MultiBody & mb) const } } auto joint = mb.joint(i); - if(reverseJoints_[index]) + if(jointsSign_[index] == -1) { auto fwd = joint.forward() ? false : true; joint.forward(fwd); @@ -145,7 +144,7 @@ static inline const Eigen::MatrixXd & jacobian_(const MultiBody & mb, const MultiBodyConfig & mbc, const Transform & Trans_0_p, const std::vector & jointsPath, - const std::vector & reverseJoints, + const std::vector & jointsSign, Eigen::MatrixXd & jac) { const std::vector & joints = mb.joints(); @@ -159,11 +158,11 @@ static inline const Eigen::MatrixXd & jacobian_(const MultiBody & mb, sva::PTransformd X_i_N = X_0_p * mbc.bodyPosW[i].inv(); - // If the joint motion is seen from child body to parent body, we have to inverted its effect - double sgn = reverseJoints[index] ? -1 : 1; + // If the joint motion is seen from child body to parent body, we have to invert its effect for(int dof = 0; dof < joints[i].dof(); ++dof) { - jac.col(curJ + dof).noalias() = (X_i_N * (sva::MotionVecd(sgn * mbc.motionSubspace[i].col(dof)))).vector(); + jac.col(curJ + dof).noalias() = + (X_i_N * (sva::MotionVecd(jointsSign[index] * mbc.motionSubspace[i].col(dof)))).vector(); } curJ += joints[i].dof(); @@ -175,7 +174,7 @@ const Eigen::MatrixXd & Jacobian::jacobian(const MultiBody & mb, const MultiBodyConfig & mbc, const sva::PTransformd & X_0_p) { - jacobian_(mb, mbc, X_0_p, jointsPath_, reverseJoints_, jac_); + jacobian_(mb, mbc, X_0_p, jointsPath_, jointsSign_, jac_); // Change Jacobian base : root of the tree --> reference body if(refBodyIndex_ > 0) { @@ -192,7 +191,7 @@ const Eigen::MatrixXd & Jacobian::jacobian(const MultiBody & mb, const MultiBody // the transformation must be read {}^0E_p {}^pT_N {}^NX_0 Eigen::Vector3d T_0_Np((point_ * mbc.bodyPosW[N]).translation()); - jacobian_(mb, mbc, T_0_Np, jointsPath_, reverseJoints_, jac_); + jacobian_(mb, mbc, T_0_Np, jointsPath_, jointsSign_, jac_); // Change Jacobian base : root of the tree --> reference body if(refBodyIndex_ > 0) { @@ -208,7 +207,7 @@ const Eigen::MatrixXd & Jacobian::bodyJacobian(const MultiBody & mb, const Multi auto N = static_cast(jointsPath_.back()); sva::PTransformd X_0_Np = point_ * mbc.bodyPosW[N]; - return jacobian_(mb, mbc, X_0_Np, jointsPath_, reverseJoints_, jac_); + return jacobian_(mb, mbc, X_0_Np, jointsPath_, jointsSign_, jac_); } const Eigen::MatrixXd & Jacobian::vectorJacobian(const MultiBody & mb, diff --git a/src/RBDyn/RBDyn/Jacobian.h b/src/RBDyn/RBDyn/Jacobian.h index e4b8b88f..6639ede8 100644 --- a/src/RBDyn/RBDyn/Jacobian.h +++ b/src/RBDyn/RBDyn/Jacobian.h @@ -465,7 +465,7 @@ class RBDYN_DLLAPI Jacobian private: std::vector jointsPath_; - std::vector reverseJoints_; + std::vector jointsSign_; sva::PTransformd point_; Eigen::MatrixXd jac_; diff --git a/tests/JacobianTest.cpp b/tests/JacobianTest.cpp index b4422100..4aed1619 100644 --- a/tests/JacobianTest.cpp +++ b/tests/JacobianTest.cpp @@ -140,9 +140,11 @@ void checkJacobianMatrixFromVelocity(const rbd::MultiBody & subMb, int col = 0; for(int i = 0; i < subMb.nrJoints(); ++i) { + const auto ui = static_cast(i); for(int j = 0; j < subMb.joint(i).dof(); ++j) { - subMbc.alpha[i][j] = 1.; + const auto uj = static_cast(j); + subMbc.alpha[ui][uj] = 1.; forwardVelocity(subMb, subMbc); @@ -150,7 +152,7 @@ void checkJacobianMatrixFromVelocity(const rbd::MultiBody & subMb, BOOST_CHECK_SMALL((mv - jacMat.col(col)).norm(), TOL); - subMbc.alpha[i][j] = 0.; + subMbc.alpha[ui][uj] = 0.; ++col; } } From 61d10f8116dc761c8f61ea9f424f7a6ec6899dfd Mon Sep 17 00:00:00 2001 From: Sonny Tarbouriech Date: Thu, 11 Jun 2020 10:53:51 +0200 Subject: [PATCH 09/15] Fixed implicit conversion errors --- src/RBDyn/Jacobian.cpp | 27 ++++++++++++++++----------- tests/JacobianTest.cpp | 12 +++++++----- 2 files changed, 23 insertions(+), 16 deletions(-) diff --git a/src/RBDyn/Jacobian.cpp b/src/RBDyn/Jacobian.cpp index d3893e29..2fe6b627 100644 --- a/src/RBDyn/Jacobian.cpp +++ b/src/RBDyn/Jacobian.cpp @@ -93,7 +93,8 @@ MultiBody Jacobian::subMultiBody(const MultiBody & mb) const for(int index = 0; index < static_cast(jointsPath_.size()); ++index) { - int i = jointsPath_[index]; + const auto uindex = static_cast(index); + int i = jointsPath_[uindex]; // body info bodies.push_back(mb.body(i)); @@ -105,19 +106,19 @@ MultiBody Jacobian::subMultiBody(const MultiBody & mb) const if(index == 0) { - if(jointsSign_[index] == -1) + if(jointsSign_[uindex] == -1) Xt.push_back(sva::PTransformd(Eigen::Vector3d(0., 0., 0.))); else Xt.push_back(mb.transform(i)); } else { - if(jointsSign_[index - 1] == -1) + if(jointsSign_[uindex - 1] == -1) { - if(jointsSign_[index] == -1) - Xt.push_back(mb.transform(jointsPath_[index - 1]).inv()); + if(jointsSign_[uindex] == -1) + Xt.push_back(mb.transform(jointsPath_[uindex - 1]).inv()); else - Xt.push_back(mb.transform(jointsPath_[index - 1]).inv() * mb.transform(jointsPath_[index])); + Xt.push_back(mb.transform(jointsPath_[uindex - 1]).inv() * mb.transform(jointsPath_[uindex])); } else { @@ -125,7 +126,7 @@ MultiBody Jacobian::subMultiBody(const MultiBody & mb) const } } auto joint = mb.joint(i); - if(jointsSign_[index] == -1) + if(jointsSign_[uindex] == -1) { auto fwd = joint.forward() ? false : true; joint.forward(fwd); @@ -179,8 +180,10 @@ const Eigen::MatrixXd & Jacobian::jacobian(const MultiBody & mb, if(refBodyIndex_ > 0) { auto dof_count = jac_.cols(); - jac_.block(0, 0, 3, dof_count) = mbc.bodyPosW[refBodyIndex_].rotation() * jac_.block(0, 0, 3, dof_count); - jac_.block(3, 0, 3, dof_count) = mbc.bodyPosW[refBodyIndex_].rotation() * jac_.block(3, 0, 3, dof_count); + jac_.block(0, 0, 3, dof_count) = + mbc.bodyPosW[static_cast(refBodyIndex_)].rotation() * jac_.block(0, 0, 3, dof_count); + jac_.block(3, 0, 3, dof_count) = + mbc.bodyPosW[static_cast(refBodyIndex_)].rotation() * jac_.block(3, 0, 3, dof_count); } return jac_; } @@ -196,8 +199,10 @@ const Eigen::MatrixXd & Jacobian::jacobian(const MultiBody & mb, const MultiBody if(refBodyIndex_ > 0) { auto dof_count = jac_.cols(); - jac_.block(0, 0, 3, dof_count) = mbc.bodyPosW[refBodyIndex_].rotation() * jac_.block(0, 0, 3, dof_count); - jac_.block(3, 0, 3, dof_count) = mbc.bodyPosW[refBodyIndex_].rotation() * jac_.block(3, 0, 3, dof_count); + jac_.block(0, 0, 3, dof_count) = + mbc.bodyPosW[static_cast(refBodyIndex_)].rotation() * jac_.block(0, 0, 3, dof_count); + jac_.block(3, 0, 3, dof_count) = + mbc.bodyPosW[static_cast(refBodyIndex_)].rotation() * jac_.block(3, 0, 3, dof_count); } return jac_; } diff --git a/tests/JacobianTest.cpp b/tests/JacobianTest.cpp index 4aed1619..fa9ab0c2 100644 --- a/tests/JacobianTest.cpp +++ b/tests/JacobianTest.cpp @@ -201,11 +201,12 @@ void checkJacobian(const rbd::MultiBody & mb, const rbd::MultiBodyConfig & mbc, // fill subMbc MultiBodyConfig subMbc(subMb); - for(int i = 0; i < subMb.nrJoints(); ++i) + for(size_t i = 0; i < static_cast(subMb.nrJoints()); ++i) { - subMbc.bodyPosW[i] = mbc.bodyPosW[jac.jointsPath()[i]]; - subMbc.jointConfig[i] = mbc.jointConfig[jac.jointsPath()[i]]; - subMbc.parentToSon[i] = mbc.parentToSon[jac.jointsPath()[i]]; + const auto joint_index = static_cast(jac.jointsPath()[i]); + subMbc.bodyPosW[i] = mbc.bodyPosW[joint_index]; + subMbc.jointConfig[i] = mbc.jointConfig[joint_index]; + subMbc.parentToSon[i] = mbc.parentToSon[joint_index]; } // test fullJacobian @@ -236,7 +237,8 @@ void checkJacobianRefBody(const rbd::MultiBody & mb, const rbd::MultiBodyConfig for(size_t i = 0; i < static_cast(subMb.nrJoints()); ++i) { - subMbc.q[i] = mbc.q[jac.jointsPath()[i]]; + const auto joint_index = static_cast(jac.jointsPath()[i]); + subMbc.q[i] = mbc.q[joint_index]; } forwardKinematics(subMb, subMbc); From 0af14d5d261071a894970a61cc7260f17936473b Mon Sep 17 00:00:00 2001 From: Sonny Tarbouriech Date: Wed, 17 Jun 2020 11:22:09 +0200 Subject: [PATCH 10/15] Deleted useless function rootBodyIndex in MB class --- src/RBDyn/RBDyn/MultiBody.h | 8 -------- 1 file changed, 8 deletions(-) diff --git a/src/RBDyn/RBDyn/MultiBody.h b/src/RBDyn/RBDyn/MultiBody.h index edf7bc9d..89e3614a 100644 --- a/src/RBDyn/RBDyn/MultiBody.h +++ b/src/RBDyn/RBDyn/MultiBody.h @@ -82,14 +82,6 @@ class RBDYN_DLLAPI MultiBody bodies_[static_cast(num)] = b; } - /// @return Index of the root body - int rootBodyIndex() const - { - int index = bodyIndexByName(bodies_[0].name()); - while(parent(index) != -1) index = parent(index); - return index; - } - /// @return Joints of the multibody system. const std::vector & joints() const { From 968edb72f46d2eae9dc2afcd02ae16968011b8fc Mon Sep 17 00:00:00 2001 From: Sonny Tarbouriech Date: Wed, 17 Jun 2020 15:38:43 +0200 Subject: [PATCH 11/15] Added visual representation for XXXdualarm robot in tests --- tests/XXXdualarm.h | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/tests/XXXdualarm.h b/tests/XXXdualarm.h index 3071e9f3..7a4b96bc 100644 --- a/tests/XXXdualarm.h +++ b/tests/XXXdualarm.h @@ -56,9 +56,14 @@ std::tuple makeXXXdua mbg.addJoint(j21); mbg.addJoint(j22); - // Root j0 j1 j2 - // ---- b0 ---- b1 ---- b2 ----b3 - // Base RevX RevX RevX + // j21 + // b21 ----- b31 + // j11 / + // Root j0 / + // ----- b0 ----- b1 + // Base \ + // j21 \ j22 + // b22 ----- b32 PTransformd to(Vector3d(0., 1., 0.)); PTransformd to1(Vector3d(0., 1., 1.)); From 322a3339b310361d8916a4cbec5e3aa8e142e5b4 Mon Sep 17 00:00:00 2001 From: Sonny Tarbouriech Date: Wed, 17 Jun 2020 16:14:13 +0200 Subject: [PATCH 12/15] Changed visual representation to avoid CI fails --- tests/XXXdualarm.h | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/tests/XXXdualarm.h b/tests/XXXdualarm.h index 7a4b96bc..f32abcae 100644 --- a/tests/XXXdualarm.h +++ b/tests/XXXdualarm.h @@ -56,14 +56,14 @@ std::tuple makeXXXdua mbg.addJoint(j21); mbg.addJoint(j22); - // j21 - // b21 ----- b31 - // j11 / - // Root j0 / + // j11 j21 + // ----- b21 ----- b31 + // | + // Root j0 | // ----- b0 ----- b1 - // Base \ - // j21 \ j22 - // b22 ----- b32 + // Base | + // | j21 j22 + // ----- b22 ----- b32 PTransformd to(Vector3d(0., 1., 0.)); PTransformd to1(Vector3d(0., 1., 1.)); From af8b49fde86ff5ad5270fd657691a2b2b0700f3b Mon Sep 17 00:00:00 2001 From: Sonny Tarbouriech Date: Thu, 18 Jun 2020 10:12:02 +0200 Subject: [PATCH 13/15] Fixed uncorrect use of floating-based dual arm robot in JacobianTest --- tests/JacobianTest.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/tests/JacobianTest.cpp b/tests/JacobianTest.cpp index fa9ab0c2..c691303b 100644 --- a/tests/JacobianTest.cpp +++ b/tests/JacobianTest.cpp @@ -316,14 +316,18 @@ BOOST_AUTO_TEST_CASE(JacobianRefBodyTest) Jacobian jac1(mb, "b31", "b32"); Jacobian jac2(mb, "b32", "b31"); - mbc.q = {{}, {0.}, {0.}, {0.}, {0.}, {0.}}; + Quaterniond quat(AngleAxisd(cst::pi() / 2., Vector3d::UnitX()) + * AngleAxisd(cst::pi() / 8., Vector3d::UnitZ())); + Vector3d tran = Vector3d::Random() * 10.; + + mbc.q = {{quat.w(), quat.x(), quat.y(), quat.z(), tran.x(), tran.y(), tran.z()}, {0.}, {0.}, {0.}, {0.}, {0.}}; forwardKinematics(mb, mbc); forwardVelocity(mb, mbc); checkJacobianRefBody(mb, mbc, jac1); checkJacobianRefBody(mb, mbc, jac2); - mbc.q = {{}, + mbc.q = {{quat.w(), quat.x(), quat.y(), quat.z(), tran.x(), tran.y(), tran.z()}, {cst::pi() / 2.}, {cst::pi() / 3.}, {cst::pi() / 4.}, From 0e2fcd06104130cac9ea03181133589cfde43e4f Mon Sep 17 00:00:00 2001 From: Benjamin Navarro Date: Thu, 18 Jun 2020 13:37:18 +0200 Subject: [PATCH 14/15] fix: incorrect integer signedness comparison --- src/RBDyn/MultiBodyConfig.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/RBDyn/MultiBodyConfig.cpp b/src/RBDyn/MultiBodyConfig.cpp index 6dfefaa1..1fcd7172 100644 --- a/src/RBDyn/MultiBodyConfig.cpp +++ b/src/RBDyn/MultiBodyConfig.cpp @@ -419,7 +419,7 @@ void checkMatchMotionSubspace(const MultiBody & mb, const MultiBodyConfig & mbc) for(int i = 0; i < static_cast(mbc.motionSubspace.size()); ++i) { const auto ui = static_cast(i); - if(mbc.motionSubspace[ui].cols() != static_cast(mb.joint(i).dof())) + if(mbc.motionSubspace[ui].cols() != mb.joint(i).dof()) { std::ostringstream str; str << "Bad motionSubspace matrix size for Joint " << mb.joint(i) << " at position " << i From 9dde8318bd87ef755153ba04b247d4fb928ad573 Mon Sep 17 00:00:00 2001 From: Pierre Gergondet Date: Mon, 24 Aug 2020 12:26:09 +0800 Subject: [PATCH 15/15] [ci] Enable Debian packaging on pull request (no upload) --- .github/workflows/package.yml | 3 +++ .github/workflows/sources/package.yml | 3 +++ 2 files changed, 6 insertions(+) diff --git a/.github/workflows/package.yml b/.github/workflows/package.yml index e66239ca..622065c3 100644 --- a/.github/workflows/package.yml +++ b/.github/workflows/package.yml @@ -14,6 +14,9 @@ on: - "**" tags: - v* + pull_request: + branches: + - "**" jobs: check-tag: runs-on: ubuntu-18.04 diff --git a/.github/workflows/sources/package.yml b/.github/workflows/sources/package.yml index fe554032..1c1d59ff 100644 --- a/.github/workflows/sources/package.yml +++ b/.github/workflows/sources/package.yml @@ -30,6 +30,9 @@ on: - '**' tags: - v* + pull_request: + branches: + - '**' jobs: # For a given tag vX.Y.Z, this checks: