Skip to content

Commit

Permalink
Merge pull request #2 from pariterre/master
Browse files Browse the repository at this point in the history 
Fixed some issue on Casadi. Should be good to go!
  • Loading branch information
@pariterre
pariterre authored Feb 21, 2020
2 parents 02a72ed + c18d595 commit c4bbe04
Show file tree
Hide file tree
Showing 12 changed files with 188 additions and 164 deletions.
7 changes: 5 additions & 2 deletions include/rbdl/CasadiMath/MX_Xd_dynamic.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand Down Expand Up @@ -164,15 +166,15 @@ 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() {
*this /= norm();
}

MX_Xd_dynamic squaredNorm() const {
return casadi::MX::norm_2(*this);
return norm() * norm();
}


Expand Down Expand Up @@ -231,6 +233,7 @@ class MX_Xd_dynamic : public casadi::MX{
}
};

}

/* MX_XD_DYNAMICS_H */
#endif
Expand Down
2 changes: 2 additions & 0 deletions include/rbdl/CasadiMath/MX_Xd_scalar.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@

#include <casadi.hpp>

namespace RBDLCasadiMath {

class MX_Xd_scalar : public casadi::MX{
public:
Expand Down Expand Up @@ -102,6 +103,7 @@ class MX_Xd_scalar : public casadi::MX{
}
};

}

/* MX_XD_SCALAR_H */
#endif
Expand Down
10 changes: 8 additions & 2 deletions include/rbdl/CasadiMath/MX_Xd_static.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,10 +16,14 @@
#include "MX_Xd_scalar.h"
#include "MX_Xd_dynamic.h"


namespace RBDLCasadiMath {

template <unsigned int nrows, unsigned int ncols>
class MX_Xd_static : public casadi::MX{
public:
MX_Xd_static() : casadi::MX(nrows, ncols){

}

virtual ~MX_Xd_static(){
Expand Down Expand Up @@ -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() {
Expand Down Expand Up @@ -423,6 +427,8 @@ class MX_Xd_static : public casadi::MX{
}
};

}

/* MX_XD_STATIC_H */
#endif

8 changes: 8 additions & 0 deletions include/rbdl/CasadiMath/MX_Xd_subMatrix.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,8 @@

#include <casadi.hpp>

namespace RBDLCasadiMath {

class MX_Xd_SubMatrix : public casadi::SubMatrix<casadi::MX, casadi::Slice, casadi::Slice>{
public:
MX_Xd_SubMatrix(casadi::MX& mat, const casadi::Slice& i, const casadi::Slice& j) :
Expand All @@ -32,6 +34,10 @@ class MX_Xd_SubMatrix : public casadi::SubMatrix<casadi::MX, casadi::Slice, casa

}

virtual ~MX_Xd_SubMatrix(){

}

void operator=(const casadi::SubMatrix<casadi::MX, casadi::Slice, casadi::Slice>& submat){
this->casadi::SubMatrix<casadi::MX, casadi::Slice, casadi::Slice>::operator=(submat);
}
Expand All @@ -46,6 +52,8 @@ class MX_Xd_SubMatrix : public casadi::SubMatrix<casadi::MX, casadi::Slice, casa
}
};

}

/* MX_XD_SUBMATRIX_H */
#endif

7 changes: 6 additions & 1 deletion include/rbdl/CasadiMath/MX_Xd_utils.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,8 @@
#include "rbdl/CasadiMath/MX_Xd_subMatrix.h"


namespace RBDLCasadiMath {

template <unsigned int nrows, unsigned int ncols>
MX_Xd_static<nrows, ncols> operator*(
const MX_Xd_static<nrows, ncols>& me,
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -382,7 +388,6 @@ inline MX_Xd_scalar pow(const MX_Xd_scalar& m, double exponent){
return casadi::MX::mpower(m, exponent);
}


}

#endif
66 changes: 33 additions & 33 deletions include/rbdl/Model.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -125,7 +125,7 @@ struct RBDL_DLLAPI Model {

/// \brief The id of the parents body
std::vector<unsigned int> 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<unsigned int> lambda_q;
/// \brief Contains the ids of all the children of a given body
Expand All @@ -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.
*/
Expand Down Expand Up @@ -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<Math::SpatialTransform> 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<unsigned int> mFixedJointCount;

Expand All @@ -206,7 +206,7 @@ struct RBDL_DLLAPI Model {

/// \brief The velocity dependent spatial acceleration
std::vector<Math::SpatialVector> c;
/// \brief The spatial inertia of the bodies
/// \brief The spatial inertia of the bodies
std::vector<Math::SpatialMatrix> IA;
/// \brief The spatial bias force
std::vector<Math::SpatialVector> pA;
Expand All @@ -218,7 +218,7 @@ struct RBDL_DLLAPI Model {
Math::VectorNd u;
/// \brief Internal forces on the body (used only InverseDynamics())
std::vector<Math::SpatialVector> f;
/// \brief The spatial inertia of body i (used only in
/// \brief The spatial inertia of body i (used only in
/// CompositeRigidBodyAlgorithm())
std::vector<Math::SpatialRigidBodyInertia> I;
std::vector<Math::SpatialRigidBodyInertia> Ic;
Expand Down Expand Up @@ -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<unsigned int>::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.
*/
Expand Down Expand Up @@ -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
Expand All @@ -300,18 +300,18 @@ 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 (
const unsigned int parent_id,
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
Expand All @@ -321,15 +321,15 @@ 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 (
const unsigned int parent_id,
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()
Expand All @@ -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\<unsigned
* \returns the id of the body or \c std::numeric_limits\<unsigned
* int\>::max() if the id was not found.
*/
unsigned int GetBodyId (const char *body_name) const {
Expand All @@ -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<std::string, unsigned int>::const_iterator iter
std::map<std::string, unsigned int>::const_iterator iter
= mBodyNameMap.begin();

while (iter != mBodyNameMap.end()) {
Expand All @@ -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<unsigned int>::max()
if (body_id >= fixed_body_discriminator
&& body_id < std::numeric_limits<unsigned int>::max()
&& body_id - fixed_body_discriminator < mFixedBodies.size()) {
return true;
}
Expand All @@ -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<unsigned int>::max()) {
if (id - fixed_body_discriminator < mFixedBodies.size())
return true;
Expand All @@ -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];
Expand All @@ -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) {
Expand All @@ -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 "
Expand All @@ -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]]);
}

Expand All @@ -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;
Expand Down
Loading

0 comments on commit c4bbe04

Please sign in to comment.