Permalink
Browse files

Use jrl-mathtools and boost math types.

  * This is done to keep compatibility with dependant libraries that
    use boost types.
  * Add conversion utility function from and to RBDL (Eigen) math
    types.
  * RBDL math types still used in algorithms.
  • Loading branch information...
1 parent f1c6da4 commit fa65bef1c76aa9cba2ec298f4e231ed4ec943c6c Antonio El Khoury committed Oct 16, 2012
Showing with 260 additions and 22 deletions.
  1. +2 −0 CMakeLists.txt
  2. +215 −0 include/ard/rbdl/tools/math-util.hh
  3. +20 −8 include/ard/rbdl/tools/types.hh
  4. +1 −0 src/CMakeLists.txt
  5. +7 −3 src/body.cc
  6. +12 −9 src/dynamic-robot.cc
  7. +2 −2 src/joint.cc
  8. +1 −0 tests/CMakeLists.txt
View
@@ -18,6 +18,7 @@ SET(${PROJECT_NAME}_TOOLS_HEADERS
include/ard/rbdl/tools/fwd.hh
include/ard/rbdl/tools/types.hh
include/ard/rbdl/tools/pointer-util.hh
+ include/ard/rbdl/tools/math-util.hh
)
SET(${PROJECT_NAME}_MODEL_HEADERS
@@ -43,6 +44,7 @@ SETUP_PROJECT()
# Declare dependencies
SEARCH_FOR_BOOST()
ADD_REQUIRED_DEPENDENCY("abstract-robot-dynamics")
+ADD_REQUIRED_DEPENDENCY("jrl-mal")
ADD_ROSPACK_DEPENDENCY("rbdl")
PKG_CONFIG_APPEND_LIBS(${PROJECT_NAME})
@@ -0,0 +1,215 @@
+// Copyright (C) 2012 by Antonio El Khoury.
+//
+// This file is part of the ard-rbdl.
+//
+// ard-rbdl is free software: you can redistribute it and/or modify it
+// under the terms of the GNU Lesser General Public License as
+// published by the Free Software Foundation, either version 3 of the
+// License, or (at your option) any later version.
+//
+// ard-rbdl is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License along with ard-rbdl. If not, see
+// <http://www.gnu.org/licenses/>.
+
+/// \brief Declarations of utility functions.
+
+#ifndef ARD_RBDL_MATH_UTIL_HH
+# define ARD_RBDL_MATH_UTIL_HH
+
+# include <boost/foreach.hpp>
+
+# include <ard/rbdl/tools/types.hh>
+
+namespace ard
+{
+ namespace rbdl
+ {
+ inline void toRbdlFromMal (rbdlVector3d_t& dst, const vector3d& src)
+ {
+ dst[0] = src[0];
+ dst[1] = src[1];
+ dst[2] = src[2];
+ }
+
+ inline rbdlVector3d_t toRbdlFromMal (const vector3d& src)
+ {
+ rbdlVector3d_t dst;
+ toRbdlFromMal (dst, src);
+ return dst;
+ }
+
+ inline void toRbdlFromMal (rbdlMatrix3d_t& dst, const matrix3d& src)
+ {
+ dst(0,0) = src(0,0);
+ dst(0,1) = src(0,1);
+ dst(0,2) = src(0,2);
+ dst(1,0) = src(1,0);
+ dst(1,1) = src(1,1);
+ dst(1,2) = src(1,2);
+ dst(2,0) = src(2,0);
+ dst(2,1) = src(2,1);
+ dst(2,2) = src(2,2);
+ }
+
+ inline rbdlMatrix3d_t toRbdlFromMal (const matrix3d& src)
+ {
+ rbdlMatrix3d_t dst;
+ toRbdlFromMal (dst, src);
+ return dst;
+ }
+
+ inline void toRbdlFromMal (rbdlMatrix4d_t& dst, const matrix4d& src)
+ {
+ dst(0,0) = src(0,0);
+ dst(0,1) = src(0,1);
+ dst(0,2) = src(0,2);
+ dst(0,3) = src(0,3);
+ dst(1,0) = src(1,0);
+ dst(1,1) = src(1,1);
+ dst(1,2) = src(1,2);
+ dst(1,3) = src(1,3);
+ dst(2,0) = src(2,0);
+ dst(2,1) = src(2,1);
+ dst(2,2) = src(2,2);
+ dst(2,3) = src(2,3);
+ dst(3,0) = src(3,0);
+ dst(3,1) = src(3,1);
+ dst(3,2) = src(3,2);
+ dst(3,3) = src(3,3);
+ }
+
+ inline rbdlMatrix4d_t toRbdlFromMal (const matrix4d& src)
+ {
+ rbdlMatrix4d_t dst;
+ toRbdlFromMal (dst, src);
+ return dst;
+ }
+
+ inline void toRbdlFromMal (rbdlVectorN_t& dst, const vectorN& src)
+ {
+ dst.resize (src.size ());
+ for (unsigned i = 0; i < src.size (); ++i)
+ dst[i] = src[i];
+ }
+
+ inline rbdlVectorN_t toRbdlFromMal (const vectorN& src)
+ {
+ rbdlVectorN_t dst;
+ toRbdlFromMal (dst, src);
+ return dst;
+ }
+
+ inline void toRbdlFromMal (rbdlMatrixNxP_t& dst, const matrixNxP& src)
+ {
+ dst.resize (src.size1 (), src.size2 ());
+ for (unsigned i = 0; i < src.size1 (); ++i)
+ for (unsigned j = 0; j < src.size2 (); ++j)
+ dst(i,j) = src(i,j);
+ }
+
+ inline rbdlMatrixNxP_t toRbdlFromMal (const matrixNxP& src)
+ {
+ rbdlMatrixNxP_t dst;
+ toRbdlFromMal (dst, src);
+ return dst;
+ }
+
+ inline void toMalFromRbdl (vector3d& dst, const rbdlVector3d_t& src)
+ {
+ dst[0] = src[0];
+ dst[1] = src[1];
+ dst[2] = src[2];
+ }
+
+ inline vector3d toMalFromRbdl (const rbdlVector3d_t& src)
+ {
+ vector3d dst;
+ toMalFromRbdl (dst, src);
+ return dst;
+ }
+
+ inline void toMalFromRbdl (matrix3d& dst, const rbdlMatrix3d_t& src)
+ {
+ dst(0,0) = src(0,0);
+ dst(0,1) = src(0,1);
+ dst(0,2) = src(0,2);
+ dst(1,0) = src(1,0);
+ dst(1,1) = src(1,1);
+ dst(1,2) = src(1,2);
+ dst(2,0) = src(2,0);
+ dst(2,1) = src(2,1);
+ dst(2,2) = src(2,2);
+ }
+
+ inline matrix3d toMalFromRbdl (const rbdlMatrix3d_t& src)
+ {
+ matrix3d dst;
+ toMalFromRbdl (dst, src);
+ return dst;
+ }
+
+ inline void toMalFromRbdl (matrix4d& dst, const rbdlMatrix4d_t& src)
+ {
+ dst(0,0) = src(0,0);
+ dst(0,1) = src(0,1);
+ dst(0,2) = src(0,2);
+ dst(0,3) = src(0,3);
+ dst(1,0) = src(1,0);
+ dst(1,1) = src(1,1);
+ dst(1,2) = src(1,2);
+ dst(1,3) = src(1,3);
+ dst(2,0) = src(2,0);
+ dst(2,1) = src(2,1);
+ dst(2,2) = src(2,2);
+ dst(2,3) = src(2,3);
+ dst(3,0) = src(3,0);
+ dst(3,1) = src(3,1);
+ dst(3,2) = src(3,2);
+ dst(3,3) = src(3,3);
+ }
+
+ inline matrix4d toMalFromRbdl (const rbdlMatrix4d_t& src)
+ {
+ matrix4d dst;
+ toMalFromRbdl (dst, src);
+ return dst;
+ }
+
+ inline void toMalFromRbdl (vectorN& dst, const rbdlVectorN_t& src)
+ {
+ dst.resize (src.size ());
+ for (unsigned i = 0; i < src.size (); ++i)
+ dst[i] = src[i];
+ }
+
+ inline vectorN toMalFromRbdl (const rbdlVectorN_t& src)
+ {
+ vectorN dst;
+ toMalFromRbdl (dst, src);
+ return dst;
+ }
+
+ inline void toMalFromRbdl (matrixNxP& dst, const rbdlMatrixNxP_t& src)
+ {
+ dst.resize (src.cols (), src.rows ());
+ for (unsigned i = 0; i < src.cols (); ++i)
+ for (unsigned j = 0; j < src.rows (); ++j)
+ dst(i,j) = src(i,j);
+ }
+
+ inline matrixNxP toMalFromRbdl (const rbdlMatrixNxP_t& src)
+ {
+ matrixNxP dst;
+ toMalFromRbdl (dst, src);
+ return dst;
+ }
+
+ } // end of namespace rbdl.
+} // end of namespace ard.
+
+#endif //! ARD_RBDL_MATH_UTIL_HH
@@ -26,14 +26,17 @@
# include <rbdl/rbdl.h>
-// Math typedefs.
-typedef RigidBodyDynamics::Math::Vector3d vector3d;
-typedef RigidBodyDynamics::Math::Matrix3d matrix3d;
-typedef Eigen::Matrix<double, 4, 4> matrix4d;
-typedef RigidBodyDynamics::Math::VectorNd vectorN;
-typedef RigidBodyDynamics::Math::MatrixNd matrixNxP;
-typedef RigidBodyDynamics::Math::SpatialTransform rbdlSpatialTransform_t;
-typedef RigidBodyDynamics::Math::SpatialVector rbdlSpatialVector_t;
+# include <jrl/mathtools/vector3.hh>
+# include <jrl/mathtools/matrix3x3.hh>
+# include <jrl/mathtools/matrix4x4.hh>
+# include <boost/numeric/ublas/vector.hpp>
+# include <boost/numeric/ublas/matrix.hpp>
+
+typedef jrlMathTools::Vector3D<double> vector3d;
+typedef jrlMathTools::Matrix3x3<double> matrix3d;
+typedef jrlMathTools::Matrix4x4<double> matrix4d;
+typedef boost::numeric::ublas::vector<double> vectorN;
+typedef boost::numeric::ublas::matrix<double> matrixNxP;
# include <abstract-robot-dynamics/traits/shared-pointer.hh>
# include <abstract-robot-dynamics/abstract-robot-dynamics.hh>
@@ -51,6 +54,15 @@ namespace ard
ARD_RBDL_DEFINE_TYPES (RigidBodyDynamics::Joint, rbdlJoint);
ARD_RBDL_DEFINE_TYPES (RigidBodyDynamics::Body, rbdlBody);
ARD_RBDL_DEFINE_TYPES (RigidBodyDynamics::Model, rbdlModel);
+
+ // RBDL Math typedefs.
+ typedef RigidBodyDynamics::Math::Vector3d rbdlVector3d_t;
+ typedef RigidBodyDynamics::Math::Matrix3d rbdlMatrix3d_t;
+ typedef Eigen::Matrix<double, 4, 4> rbdlMatrix4d_t;
+ typedef RigidBodyDynamics::Math::VectorNd rbdlVectorN_t;
+ typedef RigidBodyDynamics::Math::MatrixNd rbdlMatrixNxP_t;
+ typedef RigidBodyDynamics::Math::SpatialTransform rbdlSpatialTransform_t;
+ typedef RigidBodyDynamics::Math::SpatialVector rbdlSpatialVector_t;
// Abstract robot dynamics typedefs.
ARD_RBDL_DEFINE_TYPES (CjrlJoint, ardJoint);
View
@@ -14,6 +14,7 @@ ADD_LIBRARY(${LIBRARY_NAME}
SET_TARGET_PROPERTIES(${LIBRARY_NAME} PROPERTIES SOVERSION ${PROJECT_VERSION})
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} abstract-robot-dynamics)
+PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} jrl-mal)
ROSPACK_USE_DEPENDENCY(${LIBRARY_NAME} rbdl)
TARGET_LINK_LIBRARIES(${LIBRARY_NAME})
View
@@ -23,6 +23,7 @@
#include <stdexcept>
#include <ard/rbdl/tools/pointer-util.hh>
+#include <ard/rbdl/tools/math-util.hh>
#include <ard/rbdl/model/body.hh>
namespace ard
@@ -43,9 +44,12 @@ namespace ard
joint_t& joint) :
boost::enable_shared_from_this<Body> (),
name_ (),
- rbdlBody_ (mass, com, inertia),
+ rbdlBody_ (),
joint_ ()
{
+ rbdlBody_ = rbdlBody_t (mass,
+ toRbdlFromMal (com),
+ toRbdlFromMal (inertia));
jointShPtr_t shPtr = joint.shared_from_this ();
joint_ = jointWkPtr_t (shPtr);
}
@@ -88,7 +92,7 @@ namespace ard
const vector3d& Body::localCenterOfMass () const
{
- return rbdlBody_.mCenterOfMass;
+ return toMalFromRbdl (rbdlBody_.mCenterOfMass);
}
void Body::localCenterOfMass (const vector3d& localCenterOfMass)
@@ -111,7 +115,7 @@ namespace ard
// Recreate rbdl body as spatial inertia will be modified.
rbdlBody_ = rbdlBody_t (rbdlBody_.mMass,
rbdlBody_.mCenterOfMass,
- inertiaMatrix);
+ toRbdlFromMal (inertiaMatrix));
}
double Body::mass () const
View
@@ -24,6 +24,7 @@
#include <boost/foreach.hpp>
#include <ard/rbdl/tools/pointer-util.hh>
+#include <ard/rbdl/tools/math-util.hh>
#include <ard/rbdl/model/dynamic-robot.hh>
namespace ard
@@ -69,7 +70,7 @@ namespace ard
acceleration_ = robot.currentAcceleration ();
forces_ = robot.currentForces ();
torques_ = robot.currentTorques ();
- jointTorques_ = matrixNxP (robot.currentJointTorques ());
+ jointTorques_ = robot.currentJointTorques ();
positionCenterOfMass_ = robot.positionCenterOfMass ();
velocityCenterOfMass_ = robot.velocityCenterOfMass ();
accelerationCenterOfMass_ = robot.accelerationCenterOfMass ();
@@ -90,7 +91,7 @@ namespace ard
acceleration_ = robot.currentAcceleration ();
forces_ = robot.currentForces ();
torques_ = robot.currentTorques ();
- jointTorques_ = matrixNxP (robot.currentJointTorques ());
+ jointTorques_ = robot.currentJointTorques ();
positionCenterOfMass_ = robot.positionCenterOfMass ();
velocityCenterOfMass_ = robot.velocityCenterOfMass ();
accelerationCenterOfMass_ = robot.accelerationCenterOfMass ();
@@ -112,7 +113,7 @@ namespace ard
// Initialize rbdl model. This method is called before building
// the model.
rbdlModel_.Init ();
- rbdlModel_.gravity = gravity;
+ toRbdlFromMal (rbdlModel_.gravity, gravity);
// Build rbdl model.
if (!buildRbdlModel ())
@@ -469,12 +470,14 @@ namespace ard
= rbdlModel_.GetBodyId (parentBodyName.c_str ());
// Compute parent joint transformation in joint frame.
- matrix4d world_T_joint = joint->initialPosition ();
- matrix4d world_T_pJoint = parentJoint->initialPosition ();
- matrix4d joint_T_world = world_T_joint.inverse ();
- matrix4d joint_T_pJoint = joint_T_world * world_T_pJoint;
- matrix3d E = joint_T_pJoint.block<3,3> (0,0);
- vector3d r = - joint_T_pJoint.block<3,1> (0,3);
+ rbdlMatrix4d_t world_T_joint
+ = toRbdlFromMal (joint->initialPosition ());
+ rbdlMatrix4d_t world_T_pJoint
+ = toRbdlFromMal (parentJoint->initialPosition ());
+ rbdlMatrix4d_t joint_T_world = world_T_joint.inverse ();
+ rbdlMatrix4d_t joint_T_pJoint = joint_T_world * world_T_pJoint;
+ rbdlMatrix3d_t E = joint_T_pJoint.block<3,3> (0,0);
+ rbdlVector3d_t r = - joint_T_pJoint.block<3,1> (0,3);
rbdlSpatialTransform_t joint_X_pjoint (E, r);
// Retrieve rbdl body.
View
@@ -83,13 +83,13 @@ namespace ard
case JOINT_TYPE_REVOLUTE:
{
rbdlJoint_ = rbdlJoint_t (RigidBodyDynamics::JointTypeRevolute,
- vector3d (1, 0, 0));
+ rbdlVector3d_t (1, 0, 0));
break;
}
case JOINT_TYPE_PRISMATIC:
{
rbdlJoint_ = rbdlJoint_t (RigidBodyDynamics::JointTypePrismatic,
- vector3d (1, 0, 0));
+ rbdlVector3d_t (1, 0, 0));
break;
}
case JOINT_TYPE_FREEFLYER:
Oops, something went wrong.

0 comments on commit fa65bef

Please sign in to comment.