diff --git a/include/rbdl/CasadiMath/MX_Xd_dynamic.h b/include/rbdl/CasadiMath/MX_Xd_dynamic.h index 4280bbe..6e438fa 100644 --- a/include/rbdl/CasadiMath/MX_Xd_dynamic.h +++ b/include/rbdl/CasadiMath/MX_Xd_dynamic.h @@ -16,6 +16,8 @@ #include "MX_Xd_scalar.h" #include "MX_Xd_subMatrix.h" +namespace RBDLCasadiMath { + class MX_Xd_dynamic : public casadi::MX{ public: MX_Xd_dynamic( @@ -164,7 +166,7 @@ class MX_Xd_dynamic : public casadi::MX{ } MX_Xd_dynamic norm() const { - return casadi::MX::norm_1(*this); + return casadi::MX::norm_2(*this); } void normalize() { @@ -172,7 +174,7 @@ class MX_Xd_dynamic : public casadi::MX{ } MX_Xd_dynamic squaredNorm() const { - return casadi::MX::norm_2(*this); + return norm() * norm(); } @@ -231,6 +233,7 @@ class MX_Xd_dynamic : public casadi::MX{ } }; +} /* MX_XD_DYNAMICS_H */ #endif diff --git a/include/rbdl/CasadiMath/MX_Xd_scalar.h b/include/rbdl/CasadiMath/MX_Xd_scalar.h index fb4cad1..0bf054a 100644 --- a/include/rbdl/CasadiMath/MX_Xd_scalar.h +++ b/include/rbdl/CasadiMath/MX_Xd_scalar.h @@ -14,6 +14,7 @@ #include +namespace RBDLCasadiMath { class MX_Xd_scalar : public casadi::MX{ public: @@ -102,6 +103,7 @@ class MX_Xd_scalar : public casadi::MX{ } }; +} /* MX_XD_SCALAR_H */ #endif diff --git a/include/rbdl/CasadiMath/MX_Xd_static.h b/include/rbdl/CasadiMath/MX_Xd_static.h index 681213c..31c0698 100644 --- a/include/rbdl/CasadiMath/MX_Xd_static.h +++ b/include/rbdl/CasadiMath/MX_Xd_static.h @@ -16,10 +16,14 @@ #include "MX_Xd_scalar.h" #include "MX_Xd_dynamic.h" + +namespace RBDLCasadiMath { + template class MX_Xd_static : public casadi::MX{ public: MX_Xd_static() : casadi::MX(nrows, ncols){ + } virtual ~MX_Xd_static(){ @@ -356,11 +360,11 @@ class MX_Xd_static : public casadi::MX{ } MX_Xd_scalar norm() const{ - return casadi::MX::norm_1(*this); + return casadi::MX::norm_2(*this); } MX_Xd_scalar squaredNorm() const{ - return casadi::MX::norm_2(*this); + return norm() * norm(); } void normalize() { @@ -423,6 +427,8 @@ class MX_Xd_static : public casadi::MX{ } }; +} + /* MX_XD_STATIC_H */ #endif diff --git a/include/rbdl/CasadiMath/MX_Xd_subMatrix.h b/include/rbdl/CasadiMath/MX_Xd_subMatrix.h index a359bdf..d0feadb 100644 --- a/include/rbdl/CasadiMath/MX_Xd_subMatrix.h +++ b/include/rbdl/CasadiMath/MX_Xd_subMatrix.h @@ -14,6 +14,8 @@ #include +namespace RBDLCasadiMath { + class MX_Xd_SubMatrix : public casadi::SubMatrix{ public: MX_Xd_SubMatrix(casadi::MX& mat, const casadi::Slice& i, const casadi::Slice& j) : @@ -32,6 +34,10 @@ class MX_Xd_SubMatrix : public casadi::SubMatrix& submat){ this->casadi::SubMatrix::operator=(submat); } @@ -46,6 +52,8 @@ class MX_Xd_SubMatrix : public casadi::SubMatrix MX_Xd_static operator*( const MX_Xd_static& me, @@ -321,7 +323,11 @@ inline MX_Xd_scalar operator-( return out.casadi::MX::operator+=(other); } +} + + namespace std { +using namespace RBDLCasadiMath; inline MX_Xd_scalar sqrt(const MX_Xd_scalar& x){ return casadi::MX::sqrt(x); @@ -382,7 +388,6 @@ inline MX_Xd_scalar pow(const MX_Xd_scalar& m, double exponent){ return casadi::MX::mpower(m, exponent); } - } #endif diff --git a/include/rbdl/Model.h b/include/rbdl/Model.h index f479b97..27e6718 100644 --- a/include/rbdl/Model.h +++ b/include/rbdl/Model.h @@ -24,7 +24,7 @@ // as members need to have a special allocater. This can be achieved with // the following macro. -#ifdef EIGEN_CORE_H +#ifdef RBDL_USE_EIGEN3_MATH EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Joint) EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Body) EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::FixedBody) @@ -125,7 +125,7 @@ struct RBDL_DLLAPI Model { /// \brief The id of the parents body std::vector lambda; - /** \brief The index of the parent degree of freedom that is directly + /** \brief The index of the parent degree of freedom that is directly influencing the current one*/ std::vector lambda_q; /// \brief Contains the ids of all the children of a given body @@ -138,18 +138,18 @@ struct RBDL_DLLAPI Model { */ unsigned int dof_count; - /** \brief The size of the \f$\mathbf{q}\f$-vector. + /** \brief The size of the \f$\mathbf{q}\f$-vector. * For models without spherical joints the value is the same as * Model::dof_count, otherwise additional values for the w-component of the - * Quaternion is stored at the end of \f$\mathbf{q}\f$. + * Quaternion is stored at the end of \f$\mathbf{q}\f$. * * \sa \ref joint_description for more details. */ unsigned int q_size; - /** \brief The size of the + /** \brief The size of the * * (\f$\mathbf{\dot{q}}, \mathbf{\ddot{q}}\f$, - * and \f$\mathbf{\tau}\f$-vector. + * and \f$\mathbf{\tau}\f$-vector. * * \sa \ref joint_description for more details. */ @@ -186,7 +186,7 @@ struct RBDL_DLLAPI Model { /// \brief Transformations from the parent body to the frame of the joint. // It is expressed in the coordinate frame of the parent. std::vector X_T; - /// \brief The number of fixed joints that have been declared before + /// \brief The number of fixed joints that have been declared before /// each joint. std::vector mFixedJointCount; @@ -206,7 +206,7 @@ struct RBDL_DLLAPI Model { /// \brief The velocity dependent spatial acceleration std::vector c; - /// \brief The spatial inertia of the bodies + /// \brief The spatial inertia of the bodies std::vector IA; /// \brief The spatial bias force std::vector pA; @@ -218,7 +218,7 @@ struct RBDL_DLLAPI Model { Math::VectorNd u; /// \brief Internal forces on the body (used only InverseDynamics()) std::vector f; - /// \brief The spatial inertia of body i (used only in + /// \brief The spatial inertia of body i (used only in /// CompositeRigidBodyAlgorithm()) std::vector I; std::vector Ic; @@ -247,7 +247,7 @@ struct RBDL_DLLAPI Model { * are fixed to a moving body. The value of max(unsigned int) is * determined via std::numeric_limits::max() and the * default value of fixed_body_discriminator is max (unsigned int) / 2. - * + * * On normal systems max (unsigned int) is 4294967294 which means there * could be a total of 2147483646 movable and / or fixed bodies. */ @@ -287,10 +287,10 @@ struct RBDL_DLLAPI Model { * \param parent_id id of the parent body * \param joint_frame the transformation from the parent frame to the origin * of the joint frame (represents X_T in RBDA) - * \param joint specification for the joint that describes the + * \param joint specification for the joint that describes the * connection * \param body specification of the body itself - * \param body_name human readable name for the body (can be used to + * \param body_name human readable name for the body (can be used to * retrieve its id with GetBodyId()) * * \returns id of the added body @@ -300,7 +300,7 @@ struct RBDL_DLLAPI Model { const Math::SpatialTransform &joint_frame, const Joint &joint, const Body &body, - std::string body_name = "" + std::string body_name = "" ); unsigned int AddBodySphericalJoint ( @@ -308,10 +308,10 @@ struct RBDL_DLLAPI Model { const Math::SpatialTransform &joint_frame, const Joint &joint, const Body &body, - std::string body_name = "" + std::string body_name = "" ); - /** \brief Adds a Body to the model such that the previously added Body + /** \brief Adds a Body to the model such that the previously added Body * is the Parent. * * This function is basically the same as Model::AddBody() however the @@ -321,7 +321,7 @@ struct RBDL_DLLAPI Model { const Math::SpatialTransform &joint_frame, const Joint &joint, const Body &body, - std::string body_name = "" + std::string body_name = "" ); unsigned int AddBodyCustomJoint ( @@ -329,7 +329,7 @@ struct RBDL_DLLAPI Model { const Math::SpatialTransform &joint_frame, CustomJoint *custom_joint, const Body &body, - std::string body_name = "" + std::string body_name = "" ); /** \brief Returns the id of a body that was passed to AddBody() @@ -340,7 +340,7 @@ struct RBDL_DLLAPI Model { * \note Instead of querying this function repeatedly, it might be * advisable to query it once and reuse the returned id. * - * \returns the id of the body or \c std::numeric_limits\::max() if the id was not found. */ unsigned int GetBodyId (const char *body_name) const { @@ -353,7 +353,7 @@ struct RBDL_DLLAPI Model { /** \brief Returns the name of a body for a given body id */ std::string GetBodyName (unsigned int body_id) const { - std::map::const_iterator iter + std::map::const_iterator iter = mBodyNameMap.begin(); while (iter != mBodyNameMap.end()) { @@ -369,8 +369,8 @@ struct RBDL_DLLAPI Model { /** \brief Checks whether the body is rigidly attached to another body. */ bool IsFixedBodyId (unsigned int body_id) { - if (body_id >= fixed_body_discriminator - && body_id < std::numeric_limits::max() + if (body_id >= fixed_body_discriminator + && body_id < std::numeric_limits::max() && body_id - fixed_body_discriminator < mFixedBodies.size()) { return true; } @@ -380,7 +380,7 @@ struct RBDL_DLLAPI Model { bool IsBodyId (unsigned int id) { if (id > 0 && id < mBodies.size()) return true; - if (id >= fixed_body_discriminator + if (id >= fixed_body_discriminator && id < std::numeric_limits::max()) { if (id - fixed_body_discriminator < mFixedBodies.size()) return true; @@ -400,7 +400,7 @@ struct RBDL_DLLAPI Model { return mFixedBodies[id - fixed_body_discriminator].mMovableParent; } - unsigned int parent_id = lambda[id]; + unsigned int parent_id = lambda[id]; while (mBodies[parent_id].mIsVirtual) { parent_id = lambda[parent_id]; @@ -409,7 +409,7 @@ struct RBDL_DLLAPI Model { return parent_id; } - /** Returns the joint frame transformtion, i.e. the second argument to + /** Returns the joint frame transformtion, i.e. the second argument to Model::AddBody(). */ Math::SpatialTransform GetJointFrame (unsigned int id) { @@ -426,13 +426,13 @@ struct RBDL_DLLAPI Model { } return X_T[child_id]; } else - return X_T[id]; + return X_T[id]; } - /** Sets the joint frame transformtion, i.e. the second argument to + /** Sets the joint frame transformtion, i.e. the second argument to Model::AddBody(). */ - void SetJointFrame (unsigned int id, + void SetJointFrame (unsigned int id, const Math::SpatialTransform &transform) { if (id >= fixed_body_discriminator) { std::cerr << "Error: setting of parent transform " @@ -458,13 +458,13 @@ struct RBDL_DLLAPI Model { * * See \ref joint_singularities for details. */ - Math::Quaternion GetQuaternion (unsigned int i, + Math::Quaternion GetQuaternion (unsigned int i, const Math::VectorNd &Q) const { assert (mJoints[i].mJointType == JointTypeSpherical); unsigned int q_index = mJoints[i].q_index; - return Math::Quaternion ( Q[q_index], - Q[q_index + 1], - Q[q_index + 2], + return Math::Quaternion ( Q[q_index], + Q[q_index + 1], + Q[q_index + 2], Q[multdof3_w_index[i]]); } @@ -473,8 +473,8 @@ struct RBDL_DLLAPI Model { * * See \ref joint_singularities for details. */ - void SetQuaternion (unsigned int i, - const Math::Quaternion &quat, + void SetQuaternion (unsigned int i, + const Math::Quaternion &quat, Math::VectorNd &Q) const { assert (mJoints[i].mJointType == JointTypeSpherical); unsigned int q_index = mJoints[i].q_index; diff --git a/include/rbdl/rbdl_math.h b/include/rbdl/rbdl_math.h index d5b2db9..fe56894 100644 --- a/include/rbdl/rbdl_math.h +++ b/include/rbdl/rbdl_math.h @@ -42,22 +42,22 @@ typedef SimpleMath::Dynamic::Matrix VectorN_t; #ifdef RBDL_USE_CASADI_MATH #include "rbdl/CasadiMath/MX_Xd_utils.h" -typedef MX_Xd_scalar Vector1_t; -typedef MX_Xd_static<2,1> Vector2_t; -typedef MX_Xd_static<3,1> Vector3_t; -typedef MX_Xd_static<2,2> Matrix2_t; -typedef MX_Xd_static<3,3> Matrix3_t; -typedef MX_Xd_static<4,1> Vector4_t; - -typedef MX_Xd_static<6,1> SpatialVector_t; -typedef MX_Xd_static<6,6> SpatialMatrix_t; - -typedef MX_Xd_static<6,3> Matrix63_t; -typedef MX_Xd_static<4,3> Matrix43_t; -typedef MX_Xd_static<4,4> Matrix4_t; - -typedef MX_Xd_dynamic MatrixN_t; -typedef MX_Xd_dynamic VectorN_t; +typedef RBDLCasadiMath::MX_Xd_scalar Vector1_t; +typedef RBDLCasadiMath::MX_Xd_static<2,1> Vector2_t; +typedef RBDLCasadiMath::MX_Xd_static<3,1> Vector3_t; +typedef RBDLCasadiMath::MX_Xd_static<2,2> Matrix2_t; +typedef RBDLCasadiMath::MX_Xd_static<3,3> Matrix3_t; +typedef RBDLCasadiMath::MX_Xd_static<4,1> Vector4_t; + +typedef RBDLCasadiMath::MX_Xd_static<6,1> SpatialVector_t; +typedef RBDLCasadiMath::MX_Xd_static<6,6> SpatialMatrix_t; + +typedef RBDLCasadiMath::MX_Xd_static<6,3> Matrix63_t; +typedef RBDLCasadiMath::MX_Xd_static<4,3> Matrix43_t; +typedef RBDLCasadiMath::MX_Xd_static<4,4> Matrix4_t; + +typedef RBDLCasadiMath::MX_Xd_dynamic MatrixN_t; +typedef RBDLCasadiMath::MX_Xd_dynamic VectorN_t; #else diff --git a/python/rbdl_ptr_functions.h b/python/rbdl_ptr_functions.h index e8c1e45..b21a885 100644 --- a/python/rbdl_ptr_functions.h +++ b/python/rbdl_ptr_functions.h @@ -42,7 +42,7 @@ namespace Math { RBDL_DLLAPI inline VectorNdRef VectorFromPtr (double *ptr, unsigned int n) { #ifdef RBDL_USE_SIMPLE_MATH return SimpleMath::Map (ptr, n, 1); -#elif defined EIGEN_CORE_H +#elif defined RBDL_USE_EIGEN3_MATH return Eigen::Map (ptr, n, 1); #else std::cerr << __func__ << " not defined for used math library!" << std::endl; @@ -54,7 +54,7 @@ RBDL_DLLAPI inline VectorNdRef VectorFromPtr (double *ptr, unsigned int n) { RBDL_DLLAPI inline MatrixNdRef MatrixFromPtr (double *ptr, unsigned int rows, unsigned int cols, bool row_major = true) { #ifdef RBDL_USE_SIMPLE_MATH return SimpleMath::Map (ptr, rows, cols); -#elif defined EIGEN_CORE_H +#elif defined RBDL_USE_EIGEN3_MATH #ifdef PTR_DATA_ROW_MAJOR return Eigen::Map (ptr, rows, cols); #else @@ -562,7 +562,7 @@ void ForwardDynamicsPtr ( if (model.mJoints[i].mDoFCount == 3) { model.multdof3_U[i] = model.IA[i] * model.multdof3_S[i]; -#ifdef EIGEN_CORE_H +#ifdef RBDL_USE_EIGEN3_MATH model.multdof3_Dinv[i] = (model.multdof3_S[i].transpose() * model.multdof3_U[i]).inverse().eval(); #else model.multdof3_Dinv[i] = (model.multdof3_S[i].transpose() * model.multdof3_U[i]).inverse(); @@ -576,7 +576,7 @@ void ForwardDynamicsPtr ( if (lambda != 0) { SpatialMatrix Ia = model.IA[i] - model.multdof3_U[i] * model.multdof3_Dinv[i] * model.multdof3_U[i].transpose(); SpatialVector pa = model.pA[i] + Ia * model.c[i] + model.multdof3_U[i] * model.multdof3_Dinv[i] * model.multdof3_u[i]; -#ifdef EIGEN_CORE_H +#ifdef RBDL_USE_EIGEN3_MATH model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix(); model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa); #else @@ -595,7 +595,7 @@ void ForwardDynamicsPtr ( if (lambda != 0) { SpatialMatrix Ia = model.IA[i] - model.U[i] * (model.U[i] / model.d[i]).transpose(); SpatialVector pa = model.pA[i] + Ia * model.c[i] + model.U[i] * model.u[i] / model.d[i]; -#ifdef EIGEN_CORE_H +#ifdef RBDL_USE_EIGEN3_MATH model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix(); model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa); #else diff --git a/src/Constraints.cc b/src/Constraints.cc index c5a3931..e7e8fdc 100644 --- a/src/Constraints.cc +++ b/src/Constraints.cc @@ -1106,7 +1106,7 @@ void ComputeConstraintImpulsesDirect ( for (unsigned int i = 0; i < model.dof_count; i++) QDotPlus[i] = CS.x[i]; - // Copy back constraint impulses + // Copy back constraint impulses for (unsigned int i = 0; i < CS.size(); i++) { CS.impulse[i] = CS.x[model.dof_count + i]; } @@ -1207,29 +1207,29 @@ void ForwardDynamicsApplyConstraintForces ( if (model.mJoints[i].mDoFCount == 3 && model.mJoints[i].mJointType != JointTypeCustom) { unsigned int lambda = model.lambda[i]; - model.multdof3_u[i] = Vector3d (Tau[q_index], - Tau[q_index + 1], - Tau[q_index + 2]) + model.multdof3_u[i] = Vector3d (Tau[q_index], + Tau[q_index + 1], + Tau[q_index + 2]) - model.multdof3_S[i].transpose() * model.pA[i]; if (lambda != 0) { - SpatialMatrix Ia = model.IA[i] - (model.multdof3_U[i] - * model.multdof3_Dinv[i] + SpatialMatrix Ia = model.IA[i] - (model.multdof3_U[i] + * model.multdof3_Dinv[i] * model.multdof3_U[i].transpose()); - SpatialVector pa = model.pA[i] + Ia * model.c[i] + SpatialVector pa = model.pA[i] + Ia * model.c[i] + model.multdof3_U[i] * model.multdof3_Dinv[i] * model.multdof3_u[i]; -#ifdef EIGEN_CORE_H - model.IA[lambda].noalias() += (model.X_lambda[i].toMatrixTranspose() +#ifdef RBDL_USE_EIGEN3_MATH + model.IA[lambda].noalias() += (model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix()); model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa); #else - model.IA[lambda] += (model.X_lambda[i].toMatrixTranspose() + model.IA[lambda] += (model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix()); model.pA[lambda] += model.X_lambda[i].applyTranspose(pa); #endif - LOG << "pA[" << lambda << "] = " << model.pA[lambda].transpose() + LOG << "pA[" << lambda << "] = " << model.pA[lambda].transpose() << std::endl; } } else if (model.mJoints[i].mDoFCount == 1 @@ -1238,20 +1238,20 @@ void ForwardDynamicsApplyConstraintForces ( unsigned int lambda = model.lambda[i]; if (lambda != 0) { - SpatialMatrix Ia = model.IA[i] + SpatialMatrix Ia = model.IA[i] - model.U[i] * (model.U[i] / model.d[i]).transpose(); - SpatialVector pa = model.pA[i] + Ia * model.c[i] + SpatialVector pa = model.pA[i] + Ia * model.c[i] + model.U[i] * model.u[i] / model.d[i]; -#ifdef EIGEN_CORE_H - model.IA[lambda].noalias() += (model.X_lambda[i].toMatrixTranspose() +#ifdef RBDL_USE_EIGEN3_MATH + model.IA[lambda].noalias() += (model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix()); model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa); #else - model.IA[lambda] += (model.X_lambda[i].toMatrixTranspose() + model.IA[lambda] += (model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix()); model.pA[lambda] += model.X_lambda[i].applyTranspose(pa); #endif - LOG << "pA[" << lambda << "] = " + LOG << "pA[" << lambda << "] = " << model.pA[lambda].transpose() << std::endl; } } else if(model.mJoints[i].mJointType == JointTypeCustom) { @@ -1279,7 +1279,7 @@ void ForwardDynamicsApplyConstraintForces ( + ( model.mCustomJoints[kI]->U * model.mCustomJoints[kI]->Dinv * model.mCustomJoints[kI]->u); -#ifdef EIGEN_CORE_H +#ifdef RBDL_USE_EIGEN3_MATH model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix(); @@ -1308,8 +1308,8 @@ void ForwardDynamicsApplyConstraintForces ( if (model.mJoints[i].mDoFCount == 3 && model.mJoints[i].mJointType != JointTypeCustom) { - Vector3d qdd_temp = model.multdof3_Dinv[i] * - (model.multdof3_u[i] + Vector3d qdd_temp = model.multdof3_Dinv[i] * + (model.multdof3_u[i] - model.multdof3_U[i].transpose() * model.a[i]); QDDot[q_index] = qdd_temp[0]; @@ -1325,7 +1325,7 @@ void ForwardDynamicsApplyConstraintForces ( unsigned int dofI = model.mCustomJoints[kI]->mDoFCount; VectorNd qdd_temp = VectorNd::Zero(dofI); - qdd_temp = model.mCustomJoints[kI]->Dinv + qdd_temp = model.mCustomJoints[kI]->Dinv * (model.mCustomJoints[kI]->u - model.mCustomJoints[kI]->U.transpose() * model.a[i]); @@ -1383,10 +1383,10 @@ void ForwardDynamicsAccelerationDeltas ( unsigned int lambda = model.lambda[i]; if (lambda != 0) { - CS.d_pA[lambda] = CS.d_pA[lambda] + CS.d_pA[lambda] = CS.d_pA[lambda] + model.X_lambda[i].applyTranspose ( - CS.d_pA[i] + (model.multdof3_U[i] - * model.multdof3_Dinv[i] + CS.d_pA[i] + (model.multdof3_U[i] + * model.multdof3_Dinv[i] * CS.d_multdof3_u[i])); } } else if(model.mJoints[i].mDoFCount == 1 @@ -1395,7 +1395,7 @@ void ForwardDynamicsAccelerationDeltas ( unsigned int lambda = model.lambda[i]; if (lambda != 0) { - CS.d_pA[lambda] = CS.d_pA[lambda] + CS.d_pA[lambda] = CS.d_pA[lambda] + model.X_lambda[i].applyTranspose ( CS.d_pA[i] + model.U[i] * CS.d_u[i] / model.d[i]); } @@ -1441,7 +1441,7 @@ void ForwardDynamicsAccelerationDeltas ( if (model.mJoints[i].mDoFCount == 3 && model.mJoints[i].mJointType != JointTypeCustom) { - Vector3d qdd_temp = model.multdof3_Dinv[i] + Vector3d qdd_temp = model.multdof3_Dinv[i] * (CS.d_multdof3_u[i] - model.multdof3_U[i].transpose() * Xa); QDDot_t[q_index] = qdd_temp[0]; diff --git a/src/Dynamics.cc b/src/Dynamics.cc index 303fbd5..095cea4 100644 --- a/src/Dynamics.cc +++ b/src/Dynamics.cc @@ -397,7 +397,7 @@ RBDL_DLLAPI void ForwardDynamics ( + Ia * model.c[i] + model.U[i] * model.u[i] / model.d[i]; -#ifdef EIGEN_CORE_H +#ifdef RBDL_USE_EIGEN3_MATH model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix(); @@ -416,7 +416,7 @@ RBDL_DLLAPI void ForwardDynamics ( } else if (model.mJoints[i].mDoFCount == 3 && model.mJoints[i].mJointType != JointTypeCustom) { model.multdof3_U[i] = model.IA[i] * model.multdof3_S[i]; -#ifdef EIGEN_CORE_H +#ifdef RBDL_USE_EIGEN3_MATH model.multdof3_Dinv[i] = (model.multdof3_S[i].transpose() * model.multdof3_U[i]).inverse().eval(); #else @@ -441,7 +441,7 @@ RBDL_DLLAPI void ForwardDynamics ( + model.multdof3_U[i] * model.multdof3_Dinv[i] * model.multdof3_u[i]; -#ifdef EIGEN_CORE_H +#ifdef RBDL_USE_EIGEN3_MATH model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose() * Ia @@ -467,7 +467,7 @@ RBDL_DLLAPI void ForwardDynamics ( model.mCustomJoints[kI]->U = model.IA[i] * model.mCustomJoints[kI]->S; -#ifdef EIGEN_CORE_H +#ifdef RBDL_USE_EIGEN3_MATH model.mCustomJoints[kI]->Dinv = (model.mCustomJoints[kI]->S.transpose() * model.mCustomJoints[kI]->U).inverse().eval(); @@ -494,7 +494,7 @@ RBDL_DLLAPI void ForwardDynamics ( * model.mCustomJoints[kI]->Dinv * model.mCustomJoints[kI]->u); -#ifdef EIGEN_CORE_H +#ifdef RBDL_USE_EIGEN3_MATH model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix(); @@ -677,7 +677,7 @@ RBDL_DLLAPI void CalcMInvTimesTau ( Model &model, if (lambda != 0) { SpatialMatrix Ia = model.IA[i] - model.U[i] * (model.U[i] / model.d[i]).transpose(); -#ifdef EIGEN_CORE_H +#ifdef RBDL_USE_EIGEN3_MATH model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix(); @@ -692,7 +692,7 @@ RBDL_DLLAPI void CalcMInvTimesTau ( Model &model, model.multdof3_U[i] = model.IA[i] * model.multdof3_S[i]; -#ifdef EIGEN_CORE_H +#ifdef RBDL_USE_EIGEN3_MATH model.multdof3_Dinv[i] = (model.multdof3_S[i].transpose()*model.multdof3_U[i]).inverse().eval(); #else @@ -709,7 +709,7 @@ RBDL_DLLAPI void CalcMInvTimesTau ( Model &model, - ( model.multdof3_U[i] * model.multdof3_Dinv[i] * model.multdof3_U[i].transpose()); -#ifdef EIGEN_CORE_H +#ifdef RBDL_USE_EIGEN3_MATH model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose() * Ia @@ -725,7 +725,7 @@ RBDL_DLLAPI void CalcMInvTimesTau ( Model &model, unsigned int dofI = model.mCustomJoints[kI]->mDoFCount; model.mCustomJoints[kI]->U = model.IA[i] * model.mCustomJoints[kI]->S; -#ifdef EIGEN_CORE_H +#ifdef RBDL_USE_EIGEN3_MATH model.mCustomJoints[kI]->Dinv = (model.mCustomJoints[kI]->S.transpose() * model.mCustomJoints[kI]->U ).inverse().eval(); @@ -743,7 +743,7 @@ RBDL_DLLAPI void CalcMInvTimesTau ( Model &model, - ( model.mCustomJoints[kI]->U * model.mCustomJoints[kI]->Dinv * model.mCustomJoints[kI]->U.transpose()); -#ifdef EIGEN_CORE_H +#ifdef RBDL_USE_EIGEN3_MATH model.IA[lambda].noalias() += model.X_lambda[i].toMatrixTranspose() * Ia * model.X_lambda[i].toMatrix(); @@ -769,7 +769,7 @@ RBDL_DLLAPI void CalcMInvTimesTau ( Model &model, if (lambda != 0) { SpatialVector pa = model.pA[i] + model.U[i] * model.u[i] / model.d[i]; -#ifdef EIGEN_CORE_H +#ifdef RBDL_USE_EIGEN3_MATH model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa); #else model.pA[lambda] += model.X_lambda[i].applyTranspose(pa); @@ -795,7 +795,7 @@ RBDL_DLLAPI void CalcMInvTimesTau ( Model &model, * model.multdof3_Dinv[i] * model.multdof3_u[i]; -#ifdef EIGEN_CORE_H +#ifdef RBDL_USE_EIGEN3_MATH model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa); #else @@ -821,7 +821,7 @@ RBDL_DLLAPI void CalcMInvTimesTau ( Model &model, * model.mCustomJoints[kI]->Dinv * model.mCustomJoints[kI]->u); -#ifdef EIGEN_CORE_H +#ifdef RBDL_USE_EIGEN3_MATH model.pA[lambda].noalias() += model.X_lambda[i].applyTranspose(pa); #else diff --git a/src/Model.cc b/src/Model.cc index cac3736..e4906b7 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -68,8 +68,8 @@ Model::Model() { d = VectorNd::Zero(1); f.push_back (zero_spatial); - SpatialRigidBodyInertia rbi(0., - Vector3d (0., 0., 0.), + SpatialRigidBodyInertia rbi(0., + Vector3d (0., 0., 0.), Matrix3d::Zero()); Ic.push_back (rbi); I.push_back(rbi); @@ -98,7 +98,7 @@ unsigned int AddBodyFixedJoint ( fbody.mParentTransform = joint_frame; if (model.IsFixedBodyId(parent_id)) { - FixedBody fixed_parent = + FixedBody fixed_parent = model.mFixedBodies[parent_id - model.fixed_body_discriminator]; fbody.mMovableParent = fixed_parent.mMovableParent; @@ -109,21 +109,21 @@ unsigned int AddBodyFixedJoint ( Body parent_body = model.mBodies[fbody.mMovableParent]; parent_body.Join (fbody.mParentTransform, body); model.mBodies[fbody.mMovableParent] = parent_body; - model.I[fbody.mMovableParent] = - SpatialRigidBodyInertia::createFromMassComInertiaC ( - parent_body.mMass, - parent_body.mCenterOfMass, + model.I[fbody.mMovableParent] = + SpatialRigidBodyInertia::createFromMassComInertiaC ( + parent_body.mMass, + parent_body.mCenterOfMass, parent_body.mInertia); model.mFixedBodies.push_back (fbody); - if (model.mFixedBodies.size() > + if (model.mFixedBodies.size() > std::numeric_limits::max() - model.fixed_body_discriminator) { - std::cerr << "Error: cannot add more than " - << std::numeric_limits::max() - - model.mFixedBodies.size() + std::cerr << "Error: cannot add more than " + << std::numeric_limits::max() + - model.mFixedBodies.size() << " fixed bodies. You need to modify " - << "Model::fixed_body_discriminator for this." + << "Model::fixed_body_discriminator for this." << std::endl; assert (0); abort(); @@ -131,14 +131,14 @@ unsigned int AddBodyFixedJoint ( if (body_name.size() != 0) { if (model.mBodyNameMap.find(body_name) != model.mBodyNameMap.end()) { - std::cerr << "Error: Body with name '" - << body_name - << "' already exists!" + std::cerr << "Error: Body with name '" + << body_name + << "' already exists!" << std::endl; assert (0); abort(); } - model.mBodyNameMap[body_name] = model.mFixedBodies.size() + model.mBodyNameMap[body_name] = model.mFixedBodies.size() + model.fixed_body_discriminator - 1; } @@ -172,8 +172,8 @@ unsigned int AddBodyMultiDofJoint ( // no action required {} else { - std::cerr << "Error: Invalid joint type: " - << joint.mJointType + std::cerr << "Error: Invalid joint type: " + << joint.mJointType << std::endl; assert (0 && !"Invalid joint type!"); @@ -186,15 +186,15 @@ unsigned int AddBodyMultiDofJoint ( SpatialTransform joint_frame_transform; if (joint.mJointType == JointTypeFloatingBase) { - null_parent = model.AddBody (parent_id, - joint_frame, - JointTypeTranslationXYZ, + null_parent = model.AddBody (parent_id, + joint_frame, + JointTypeTranslationXYZ, null_body); - return model.AddBody (null_parent, - SpatialTransform(), - JointTypeSpherical, - body, + return model.AddBody (null_parent, + SpatialTransform(), + JointTypeSpherical, + body, body_name); } @@ -240,21 +240,21 @@ unsigned int AddBodyMultiDofJoint ( else joint_frame_transform = SpatialTransform(); - if (j == joint_count - 1) + if (j == joint_count - 1) // if we are at the last we must add the real body break; else { // otherwise we just add an intermediate body - null_parent = model.AddBody (null_parent, - joint_frame_transform, + null_parent = model.AddBody (null_parent, + joint_frame_transform, single_dof_joint, null_body); } } - return model.AddBody (null_parent, - joint_frame_transform, - single_dof_joint, + return model.AddBody (null_parent, + joint_frame_transform, + single_dof_joint, body, body_name); } @@ -269,33 +269,33 @@ unsigned int Model::AddBody( assert (joint.mJointType != JointTypeUndefined); if (joint.mJointType == JointTypeFixed) { - previously_added_body_id = AddBodyFixedJoint (*this, - parent_id, - joint_frame, - joint, - body, + previously_added_body_id = AddBodyFixedJoint (*this, + parent_id, + joint_frame, + joint, + body, body_name); return previously_added_body_id; - } else if ( (joint.mJointType == JointTypeSpherical) - || (joint.mJointType == JointTypeEulerZYX) - || (joint.mJointType == JointTypeEulerXYZ) - || (joint.mJointType == JointTypeEulerYXZ) - || (joint.mJointType == JointTypeTranslationXYZ) - || (joint.mJointType == JointTypeCustom) + } else if ( (joint.mJointType == JointTypeSpherical) + || (joint.mJointType == JointTypeEulerZYX) + || (joint.mJointType == JointTypeEulerXYZ) + || (joint.mJointType == JointTypeEulerYXZ) + || (joint.mJointType == JointTypeTranslationXYZ) + || (joint.mJointType == JointTypeCustom) ) { // no action required - } else if (joint.mJointType != JointTypePrismatic + } else if (joint.mJointType != JointTypePrismatic && joint.mJointType != JointTypeRevolute && joint.mJointType != JointTypeRevoluteX && joint.mJointType != JointTypeRevoluteY && joint.mJointType != JointTypeRevoluteZ && joint.mJointType != JointTypeHelical ) { - previously_added_body_id = AddBodyMultiDofJoint (*this, - parent_id, - joint_frame, - joint, + previously_added_body_id = AddBodyMultiDofJoint (*this, + parent_id, + joint_frame, + joint, body, body_name); return previously_added_body_id; @@ -321,7 +321,7 @@ unsigned int Model::AddBody( lambda_q_last = lambda_q_last + mJoints[mJoints.size() - 1].mDoFCount; } else if (mJoints[mJoints.size() - 1].mJointType == JointTypeCustom) { unsigned int custom_index = mJoints[mJoints.size() - 1].custom_joint_index; - lambda_q_last = lambda_q_last + lambda_q_last = lambda_q_last + mCustomJoints[mCustomJoints.size() - 1]->mDoFCount; } @@ -338,8 +338,8 @@ unsigned int Model::AddBody( if (body_name.size() != 0) { if (mBodyNameMap.find(body_name) != mBodyNameMap.end()) { - std::cerr << "Error: Body with name '" - << body_name + std::cerr << "Error: Body with name '" + << body_name << "' already exists!" << std::endl; assert (0); @@ -357,11 +357,11 @@ unsigned int Model::AddBody( mJoints.push_back(joint); if (mJoints[prev_joint_index].mJointType != JointTypeCustom) { - mJoints[mJoints.size() - 1].q_index = + mJoints[mJoints.size() - 1].q_index = mJoints[prev_joint_index].q_index + mJoints[prev_joint_index].mDoFCount; } else { - mJoints[mJoints.size() - 1].q_index = - mJoints[prev_joint_index].q_index + mJoints[prev_joint_index].mDoFCount; + mJoints[mJoints.size() - 1].q_index = + mJoints[prev_joint_index].q_index + mJoints[prev_joint_index].mDoFCount; } S.push_back (joint.mJointAxes[0]); @@ -411,8 +411,8 @@ unsigned int Model::AddBody( f.push_back (SpatialVector (0., 0., 0., 0., 0., 0.)); - SpatialRigidBodyInertia rbi = - SpatialRigidBodyInertia::createFromMassComInertiaC (body.mMass, + SpatialRigidBodyInertia rbi = + SpatialRigidBodyInertia::createFromMassComInertiaC (body.mMass, body.mCenterOfMass, body.mInertia); @@ -422,8 +422,8 @@ unsigned int Model::AddBody( hdotc.push_back (SpatialVector(0., 0., 0., 0., 0., 0.)); if (mBodies.size() == fixed_body_discriminator) { - std::cerr << "Error: cannot add more than " - << fixed_body_discriminator + std::cerr << "Error: cannot add more than " + << fixed_body_discriminator << " movable bodies. You need to modify " "Model::fixed_body_discriminator for this." << std::endl; @@ -448,7 +448,7 @@ unsigned int Model::AddBody( while (joint_types.size() != 0) { current_joint_type = joint_types[0].first; - std::vector >::iterator type_iter = + std::vector >::iterator type_iter = joint_types.begin(); while (type_iter != joint_types.end()) { @@ -462,7 +462,7 @@ unsigned int Model::AddBody( } // for (unsigned int i = 0; i < mJointUpdateOrder.size(); i++) { - // std::cout << "i = " << i << ": joint_id = " << mJointUpdateOrder[i] + // std::cout << "i = " << i << ": joint_id = " << mJointUpdateOrder[i] // << " joint_type = " << mJoints[mJointUpdateOrder[i]].mJointType << std::endl; // } @@ -474,9 +474,9 @@ unsigned int Model::AppendBody ( const Joint &joint, const Body &body, std::string body_name) { - return Model::AddBody (previously_added_body_id, + return Model::AddBody (previously_added_body_id, joint_frame, - joint, + joint, body, body_name); } @@ -494,9 +494,9 @@ unsigned int Model::AddBodyCustomJoint ( mCustomJoints.push_back (custom_joint); - unsigned int body_id = AddBody (parent_id, - joint_frame, - proxy_joint, + unsigned int body_id = AddBody (parent_id, + joint_frame, + proxy_joint, body, body_name); diff --git a/src/rbdl_utils.cc b/src/rbdl_utils.cc index fa7666a..261f858 100644 --- a/src/rbdl_utils.cc +++ b/src/rbdl_utils.cc @@ -147,7 +147,7 @@ RBDL_DLLAPI std::string GetNamedBodyOriginsOverview (Model &model) { for (unsigned int body_id = 0; body_id < model.mBodies.size(); body_id++) { std::string body_name = model.GetBodyName (body_id); - if (body_name.size() == 0) + if (body_name.size() == 0) continue; Vector3d position = CalcBodyToBaseCoordinates (model, Q, body_id, Vector3d (0., 0., 0.), false); @@ -172,7 +172,7 @@ RBDL_DLLAPI void CalcCenterOfMass ( bool update_kinematics) { // If we want to compute com_acceleration or change of angular momentum // we must have qddot provided. - assert( (com_acceleration == NULL && change_of_angular_momentum == NULL) + assert( (com_acceleration == NULL && change_of_angular_momentum == NULL) || (qddot != NULL) ); if (update_kinematics)