From 11f6b32c35f1ea1d8d8645467c958c708226740c Mon Sep 17 00:00:00 2001
From: Rohan Budhiraja <budhiraja@laas.fr>
Date: Fri, 18 Mar 2016 20:00:20 +0100
Subject: [PATCH] ros bindings with pinocchio and eigen.
---
CMakeLists.txt | 54 +++++++++++++--------
src/converter.hh | 102 +++++++++++++++++++--------------------
src/geometric_simu.cpp | 7 +--
src/robot_model.cpp | 103 +++++++++++++++++++++++++---------------
src/robot_model.hh | 8 ++--
src/ros_joint_state.cpp | 59 ++++++++++-------------
src/ros_joint_state.hh | 2 +-
src/ros_publish.cpp | 15 +++---
src/ros_publish.hxx | 8 ++--
src/ros_subscribe.cpp | 10 ++--
src/ros_subscribe.hh | 2 +-
src/ros_subscribe.hxx | 8 ++--
src/sot_loader.cpp | 15 +++---
src/sot_to_ros.cpp | 10 ++--
src/sot_to_ros.hh | 35 +++++++-------
15 files changed, 233 insertions(+), 205 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 7f5130e..47df55e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -17,14 +17,20 @@
# Catkin part
+
cmake_minimum_required(VERSION 2.4.6)
+include(cmake/base.cmake)
+INCLUDE(cmake/boost.cmake)
+INCLUDE(cmake/eigen.cmake)
+include(cmake/ros.cmake)
+include(cmake/GNUInstallDirs.cmake)
+include(cmake/python.cmake)
+
project(dynamic_graph_bridge)
find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs message_generation std_srvs geometry_msgs sensor_msgs tf)
find_package(realtime_tools)
-find_package(Boost REQUIRED COMPONENTS program_options)
-
## LAAS cmake submodule part
set(PROJECT_DESCRIPTION "Dynamic graph bridge library")
@@ -37,10 +43,8 @@ set(${PROJECT_NAME}_HEADERS
include/dynamic_graph_bridge/ros_init.hh
include/dynamic_graph_bridge/ros_interpreter.hh
)
-include(cmake/base.cmake)
-include(cmake/ros.cmake)
-include(cmake/GNUInstallDirs.cmake)
-include(cmake/python.cmake)
+SEARCH_FOR_EIGEN()
+SEARCH_FOR_BOOST()
SETUP_PROJECT()
@@ -57,12 +61,19 @@ set(PKG_CONFIG_ADDITIONAL_VARIABLES
add_required_dependency(roscpp)
add_required_dependency("realtime_tools >= 1.8")
add_required_dependency(bullet)
-add_required_dependency(jrl-mal)
-add_required_dependency(dynamic-graph)
-add_required_dependency(dynamic-graph-python)
-add_required_dependency(sot-core)
-add_required_dependency(sot-dynamic)
-add_required_dependency(jrl-dynamics-urdf)
+ADD_REQUIRED_DEPENDENCY("pinocchio")
+ADD_REQUIRED_DEPENDENCY("dynamic-graph >= 3.0.0")
+ADD_REQUIRED_DEPENDENCY("dynamic-graph-python >= 3.0.0")
+ADD_REQUIRED_DEPENDENCY("sot-core >= 3.0.0")
+ADD_REQUIRED_DEPENDENCY("sot-tools >= 2.0.0")
+ADD_REQUIRED_DEPENDENCY("sot-dynamic >= 3.1.0")
+#add_required_dependency(jrl-mal)
+#add_required_dependency(dynamic-graph)
+#add_required_dependency(dynamic-graph-python)
+#add_required_dependency(sot-core)
+#add_required_dependency(sot-dynamic)
+#add_required_dependency(jrl-dynamics-urdf)
+
add_required_dependency(dynamic_graph_bridge_msgs)
add_library(ros_bridge
@@ -70,7 +81,8 @@ add_library(ros_bridge
include/dynamic_graph_bridge/ros_init.hh src/ros_init.cpp
src/sot_to_ros.hh src/sot_to_ros.cpp
)
-pkg_config_use_dependency(ros_bridge jrl-mal)
+#pkg_config_use_dependency(ros_bridge jrl-mal)
+#PKG_CONFIG_USE_COMPILE_DEPENDENCY(ros_bridge eigen3)
pkg_config_use_dependency(ros_bridge bullet)
pkg_config_use_dependency(ros_bridge dynamic_graph_bridge_msgs)
install(TARGETS ros_bridge DESTINATION lib)
@@ -84,10 +96,11 @@ macro(compile_plugin NAME)
message(lib path ${LIBRARY_OUTPUT_PATH})
file(MAKE_DIRECTORY "${LIBRARY_OUTPUT_PATH}/dynamic_graph/ros/${NAME}")
add_library(${NAME} src/${NAME}.cpp src/${NAME}.hh)
- pkg_config_use_dependency(${NAME} jrl-mal)
+ #pkg_config_use_dependency(${NAME} jrl-mal)
+ #kg_config_use_dependency(${NAME} eigen3)
pkg_config_use_dependency(${NAME} dynamic-graph)
pkg_config_use_dependency(${NAME} sot-core)
- pkg_config_use_dependency(${NAME} jrl-dynamics-urdf)
+ #pkg_config_use_dependency(${NAME} jrl-dynamics-urdf)
pkg_config_use_dependency(${NAME} dynamic_graph_bridge_msgs)
add_dependencies(${NAME} ros_bridge)
target_link_libraries(${NAME} ros_bridge)
@@ -101,14 +114,13 @@ macro(compile_plugin NAME)
)
PKG_CONFIG_USE_DEPENDENCY(ros/${NAME}/wrap realtime_tools)
- PKG_CONFIG_USE_DEPENDENCY(ros/${NAME}/wrap jrl-mal)
+ #KG_CONFIG_USE_DEPENDENCY(ros/${NAME}/wrap eigen3)
+ #PKG_CONFIG_USE_DEPENDENCY(ros/${NAME}/wrap jrl-mal)
PKG_CONFIG_USE_DEPENDENCY(ros/${NAME}/wrap dynamic_graph)
PKG_CONFIG_USE_DEPENDENCY(ros/${NAME}/wrap sot-core)
PKG_CONFIG_USE_DEPENDENCY(ros/${NAME}/wrap dynamic_graph_bridge_msgs)
endmacro()
-include(cmake/python.cmake)
-
compile_plugin(ros_publish)
compile_plugin(ros_subscribe)
compile_plugin(ros_time)
@@ -120,7 +132,8 @@ compile_plugin(robot_model)
# ros_interperter library.
add_library(ros_interpreter src/ros_interpreter.cpp)
-pkg_config_use_dependency(ros_interpreter jrl-mal)
+#kg_config_use_dependency(ros_interpreter eigen3)
+#pkg_config_use_dependency(ros_interpreter jrl-mal)
pkg_config_use_dependency(ros_interpreter dynamic-graph)
pkg_config_use_dependency(ros_interpreter sot-core)
pkg_config_use_dependency(ros_interpreter roscpp)
@@ -137,7 +150,8 @@ install(TARGETS ros_interpreter DESTINATION lib)
add_executable(interpreter src/interpreter.cpp)
add_dependencies(interpreter ros_interpreter)
target_link_libraries(interpreter ros_interpreter)
-pkg_config_use_dependency(interpreter jrl-mal)
+#kg_config_use_dependency(interpreter eigen3)
+#pkg_config_use_dependency(interpreter jrl-mal)
pkg_config_use_dependency(interpreter dynamic-graph)
pkg_config_use_dependency(interpreter sot-core)
pkg_config_use_dependency(interpreter sot-dynamic)
diff --git a/src/converter.hh b/src/converter.hh
index e53cbf5..79282ba 100644
--- a/src/converter.hh
+++ b/src/converter.hh
@@ -67,18 +67,18 @@ namespace dynamicgraph
}
// Vector
- SOT_TO_ROS_IMPL(ml::Vector)
+ SOT_TO_ROS_IMPL(Vector)
{
dst.data.resize (src.size ());
- for (unsigned i = 0; i < src.size (); ++i)
- dst.data[i] = src.elementAt (i);
+ for (int i = 0; i < src.size (); ++i)
+ dst.data[i] = src (i);
}
- ROS_TO_SOT_IMPL(ml::Vector)
+ ROS_TO_SOT_IMPL(Vector)
{
dst.resize (src.data.size ());
- for (unsigned i = 0; i < src.data.size (); ++i)
- dst.elementAt (i) = src.data[i];
+ for (unsigned int i = 0; i < src.data.size (); ++i)
+ dst (i) = src.data[i];
}
// Vector3
@@ -86,12 +86,12 @@ namespace dynamicgraph
{
if (src.size () > 0)
{
- dst.x = src.elementAt (0);
+ dst.x = src (0);
if (src.size () > 1)
{
- dst.y = src.elementAt (1);
+ dst.y = src (1);
if (src.size () > 2)
- dst.z = src.elementAt (2);
+ dst.z = src (2);
}
}
}
@@ -99,62 +99,56 @@ namespace dynamicgraph
ROS_TO_SOT_IMPL(specific::Vector3)
{
dst.resize (3);
- dst.elementAt (0) = src.x;
- dst.elementAt (1) = src.y;
- dst.elementAt (2) = src.z;
+ dst (0) = src.x;
+ dst (1) = src.y;
+ dst (2) = src.z;
}
// Matrix
- SOT_TO_ROS_IMPL(ml::Matrix)
+ SOT_TO_ROS_IMPL(Matrix)
{
- dst.width = src.nbRows ();
- dst.data.resize (src.nbCols () * src.nbRows ());
- for (unsigned i = 0; i < src.nbCols () * src.nbRows (); ++i)
- dst.data[i] = src.elementAt (i);
- }
- ROS_TO_SOT_IMPL(ml::Matrix)
+ //TODO: Confirm Ros Matrix Storage order. It changes the RosMatrix to ColMajor.
+ dst.width = src.rows ();
+ dst.data.resize (src.cols () * src.rows ());
+ for (int i = 0; i < src.cols () * src.rows (); ++i)
+ dst.data[i] = src.data()[i];
+ }
+
+ ROS_TO_SOT_IMPL(Matrix)
{
+ //TODO: Confirm Ros Matrix Storage order.
dst.resize (src.width, src.data.size () / src.width);
for (unsigned i = 0; i < src.data.size (); ++i)
- dst.elementAt (i) = src.data[i];
+ dst.data()[i] = src.data[i];
}
// Homogeneous matrix.
SOT_TO_ROS_IMPL(sot::MatrixHomogeneous)
{
- btMatrix3x3 rotation;
- btQuaternion quaternion;
- for(unsigned i = 0; i < 3; ++i)
- for(unsigned j = 0; j < 3; ++j)
- rotation[i][j] = src (i, j);
- rotation.getRotation (quaternion);
-
- dst.translation.x = src (0, 3);
- dst.translation.y = src (1, 3);
- dst.translation.z = src (2, 3);
-
- dst.rotation.x = quaternion.x ();
- dst.rotation.y = quaternion.y ();
- dst.rotation.z = quaternion.z ();
- dst.rotation.w = quaternion.w ();
+ sot::VectorQuaternion q(src.linear());
+ dst.translation.x = src.translation()(0);
+ dst.translation.y = src.translation()(1);
+ dst.translation.z = src.translation()(2);
+
+ dst.rotation.x = q.x();
+ dst.rotation.y = q.y();
+ dst.rotation.z = q.z();
+ dst.rotation.w = q.w();
}
ROS_TO_SOT_IMPL(sot::MatrixHomogeneous)
{
- btQuaternion quaternion
- (src.rotation.x, src.rotation.y, src.rotation.z, src.rotation.w);
- btMatrix3x3 rotation (quaternion);
-
- // Copy the rotation component.
- for(unsigned i = 0; i < 3; ++i)
- for(unsigned j = 0; j < 3; ++j)
- dst (i, j) = rotation[i][j];
+ sot::VectorQuaternion q(src.rotation.w,
+ src.rotation.x,
+ src.rotation.y,
+ src.rotation.z);
+ dst.linear() = q.matrix();
// Copy the translation component.
- dst(0, 3) = src.translation.x;
- dst(1, 3) = src.translation.y;
- dst(2, 3) = src.translation.z;
+ dst.translation()(0) = src.translation.x;
+ dst.translation()(1) = src.translation.y;
+ dst.translation()(2) = src.translation.z;
}
@@ -189,8 +183,8 @@ namespace dynamicgraph
# define DG_BRIDGE_TO_ROS_MAKE_STAMPED_IMPL(T, ATTRIBUTE, EXTRA) \
template <> \
inline void converter \
- (SotToRos<std::pair<T, ml::Vector> >::ros_t& dst, \
- const SotToRos<std::pair<T, ml::Vector> >::sot_t& src) \
+ (SotToRos<std::pair<T, Vector> >::ros_t& dst, \
+ const SotToRos<std::pair<T, Vector> >::sot_t& src) \
{ \
makeHeader(dst.header); \
converter<SotToRos<T>::ros_t, SotToRos<T>::sot_t> (dst.ATTRIBUTE, src); \
@@ -222,9 +216,9 @@ namespace dynamicgraph
DG_BRIDGE_MAKE_SHPTR_IMPL(double);
DG_BRIDGE_MAKE_SHPTR_IMPL(unsigned int);
- DG_BRIDGE_MAKE_SHPTR_IMPL(ml::Vector);
+ DG_BRIDGE_MAKE_SHPTR_IMPL(Vector);
DG_BRIDGE_MAKE_SHPTR_IMPL(specific::Vector3);
- DG_BRIDGE_MAKE_SHPTR_IMPL(ml::Matrix);
+ DG_BRIDGE_MAKE_SHPTR_IMPL(Matrix);
DG_BRIDGE_MAKE_SHPTR_IMPL(sot::MatrixHomogeneous);
DG_BRIDGE_MAKE_SHPTR_IMPL(specific::Twist);
@@ -235,8 +229,8 @@ namespace dynamicgraph
# define DG_BRIDGE_MAKE_STAMPED_IMPL(T, ATTRIBUTE, EXTRA) \
template <> \
inline void converter \
- (SotToRos<std::pair<T, ml::Vector> >::sot_t& dst, \
- const SotToRos<std::pair<T, ml::Vector> >::ros_t& src) \
+ (SotToRos<std::pair<T, Vector> >::sot_t& dst, \
+ const SotToRos<std::pair<T, Vector> >::ros_t& src) \
{ \
converter<SotToRos<T>::sot_t, SotToRos<T>::ros_t> (dst, src.ATTRIBUTE); \
do { EXTRA } while (0); \
@@ -254,9 +248,9 @@ namespace dynamicgraph
# define DG_BRIDGE_MAKE_STAMPED_SHPTR_IMPL(T, ATTRIBUTE, EXTRA) \
template <> \
inline void converter \
- (SotToRos<std::pair<T, ml::Vector> >::sot_t& dst, \
+ (SotToRos<std::pair<T, Vector> >::sot_t& dst, \
const boost::shared_ptr \
- <SotToRos<std::pair<T, ml::Vector> >::ros_t const>& src) \
+ <SotToRos<std::pair<T, Vector> >::ros_t const>& src) \
{ \
converter<SotToRos<T>::sot_t, SotToRos<T>::ros_t> (dst, src->ATTRIBUTE); \
do { EXTRA } while (0); \
diff --git a/src/geometric_simu.cpp b/src/geometric_simu.cpp
index 56f31cc..5a6a1ff 100644
--- a/src/geometric_simu.cpp
+++ b/src/geometric_simu.cpp
@@ -46,21 +46,16 @@ void workThread(SotLoader *aSotLoader)
int main(int argc, char *argv[])
{
-
ros::init(argc, argv, "sot_ros_encapsulator");
-
SotLoader aSotLoader;
if (aSotLoader.parseOptions(argc,argv)<0)
return -1;
-
ros::NodeHandle n;
ros::ServiceServer service = n.advertiseService("start_dynamic_graph",
&SotLoader::start_dg,
- &aSotLoader);
+ &aSotLoader);
ROS_INFO("Ready to start dynamic graph.");
-
boost::thread thr(workThread,&aSotLoader);
-
boost::unique_lock<boost::mutex> lock(mut);
cond.wait(lock);
ros::spin();
diff --git a/src/robot_model.cpp b/src/robot_model.cpp
index 44911ea..e4afa19 100644
--- a/src/robot_model.cpp
+++ b/src/robot_model.cpp
@@ -2,18 +2,22 @@
#include <dynamic-graph/all-commands.h>
#include <dynamic-graph/factory.h>
-#include <jrl/dynamics/urdf/parser.hh>
+#include <pinocchio/multibody/parser/urdf.hpp>
+#include <pinocchio/multibody/model.hpp>
#include "dynamic_graph_bridge/ros_init.hh"
+#include <ros/package.h>
+
namespace dynamicgraph
{
RosRobotModel::RosRobotModel(const std::string& name)
- : Dynamic(name,false),
+ : Dynamic(name),
jointsParameterName_("jrl_map"),
ns_("sot_controller")
{
+
std::string docstring;
docstring =
@@ -54,19 +58,28 @@ RosRobotModel::~RosRobotModel()
void RosRobotModel::loadUrdf (const std::string& filename)
{
- jrl::dynamics::urdf::Parser parser;
-
- std::map<std::string, std::string>::const_iterator it = specialJoints_.begin();
- for (;it!=specialJoints_.end();++it) {
- parser.specifyREPName(it->first, it->second);
- }
- rosInit (false);
-
- m_HDR = parser.parse(filename);
-
- ros::NodeHandle nh(ns_);
-
- nh.setParam(jointsParameterName_, parser.JointsNamesByRank_);
+ // jrl::dynamics::urdf::Parser parser;
+
+ //TODO: Specific rep name. link them to the operational frames in pinocchio
+ // std::map<std::string, std::string>::const_iterator it = specialJoints_.begin();
+ // for (;it!=specialJoints_.end();++it) {
+ // parser.specifyREPName(it->first, it->second);
+ // }
+ rosInit (false);
+ m_model = se3::urdf::buildModel(filename);
+ this->m_urdfPath = filename;
+ if (m_data) delete m_data;
+ m_data = new se3::Data(m_model);
+ init=true;
+
+ // m_HDR = parser.parse(filename);
+ ros::NodeHandle nh(ns_);
+
+ XmlRpc::XmlRpcValue JointsNamesByRank_;
+ JointsNamesByRank_.setSize(m_model.names.size());
+ std::vector<std::string>::const_iterator it = m_model.names.begin();
+ for (int i=0;it!=m_model.names.end();++it, ++i) JointsNamesByRank_[i]= (*it);
+ nh.setParam(jointsParameterName_, JointsNamesByRank_);
}
void RosRobotModel::setNamespace (const std::string& ns)
@@ -76,28 +89,41 @@ void RosRobotModel::setNamespace (const std::string& ns)
void RosRobotModel::loadFromParameterServer()
{
- jrl::dynamics::urdf::Parser parser;
-
- std::map<std::string, std::string>::const_iterator it = specialJoints_.begin();
- for (;it!=specialJoints_.end();++it) {
- parser.specifyREPName(it->first, it->second);
- }
+ //jrl::dynamics::urdf::Parser parser;
+ //TODO: Specific rep name. link them to the operational frames in pinocchio
+ // std::map<std::string, std::string>::const_iterator it = specialJoints_.begin();
+ // for (;it!=specialJoints_.end();++it) {
+ // parser.specifyREPName(it->first, it->second);
+ // }
rosInit (false);
std::string robotDescription;
ros::param::param<std::string> ("/robot_description", robotDescription, "");
-
if (robotDescription.empty ())
throw std::runtime_error("No model available as ROS parameter. Fail.");
+ ::urdf::ModelInterfacePtr urdfTree = ::urdf::parseURDF (robotDescription);
+ if (urdfTree)
+ se3::urdf::parseTree(urdfTree->getRoot(),
+ this->m_model, se3::SE3::Identity(), false);
+ else {
+ const std::string exception_message
+ ("robot_description not parsed correctly.");
+ throw std::invalid_argument(exception_message);
+ }
- m_HDR = parser.parseStream (robotDescription);
-
+ this->m_urdfPath = "";
+ if (m_data) delete m_data;
+ m_data = new se3::Data(m_model);
+ init=true;
ros::NodeHandle nh(ns_);
-
- nh.setParam(jointsParameterName_, parser.JointsNamesByRank_);
-
+
+ XmlRpc::XmlRpcValue JointsNamesByRank_;
+ JointsNamesByRank_.setSize(m_model.names.size());
+ std::vector<std::string>::const_iterator it = m_model.names.begin();
+ for (int i=0;it!=m_model.names.end();++it, ++i) JointsNamesByRank_[i]= (*it);
+ nh.setParam(jointsParameterName_, JointsNamesByRank_);
}
-
+ /*
namespace
{
@@ -119,7 +145,7 @@ ml::Vector convertVector(const vectorN& v)
}
} // end of anonymous namespace.
-
+*/
Vector RosRobotModel::curConf() const
{
@@ -127,7 +153,6 @@ Vector RosRobotModel::curConf() const
// Freeflyer reference frame should be the same as global
// frame so that operational point positions correspond to
// position in freeflyer frame.
-
XmlRpc::XmlRpcValue ffpose;
ros::NodeHandle nh(ns_);
std::string param_name = "ffpose";
@@ -147,17 +172,17 @@ Vector RosRobotModel::curConf() const
ffpose[i] = 0.0;
}
- if (!m_HDR )
+ if (!m_data )
throw std::runtime_error ("no robot loaded");
else {
- vectorN currConf = m_HDR->currentConfiguration();
- Vector res;
- res = convertVector(currConf);
-
- for (int32_t i = 0; i < ffpose.size(); ++i)
- res(i) = static_cast<double>(ffpose[i]);
-
- return res;
+ //TODO: confirm accesscopy is for asynchronous commands
+ Vector currConf = jointPositionSIN.accessCopy();
+
+ for (int32_t i = 0; i < ffpose.size(); ++i)
+ currConf(i) = static_cast<double>(ffpose[i]);
+
+ return currConf;
+
}
}
diff --git a/src/robot_model.hh b/src/robot_model.hh
index b456b4e..646a894 100644
--- a/src/robot_model.hh
+++ b/src/robot_model.hh
@@ -15,7 +15,7 @@ namespace dynamicgraph
/// the robot_description parameter or a specified file and build
/// a Dynamic entity
///
- /// This relies on jrl_dynamics_urdf to load the model and jrl-dynamics
+ /// This relies on pinocchio urdf parser to load the model and pinocchio
/// to realize the computation.
class RosRobotModel : public sot::Dynamic
{
@@ -35,9 +35,11 @@ namespace dynamicgraph
unsigned getDimension () const
{
- if (!m_HDR)
+ if (!m_data)
throw std::runtime_error ("no robot loaded");
- return m_HDR->numberDof();
+ //TODO: Configuration vector dimension or the dof?
+ return m_model.nv;
+ //return m_model.nq;
}
diff --git a/src/ros_joint_state.cpp b/src/ros_joint_state.cpp
index dccc85e..df04d8c 100644
--- a/src/ros_joint_state.cpp
+++ b/src/ros_joint_state.cpp
@@ -32,41 +32,34 @@ namespace dynamicgraph
RetrieveJointNames::RetrieveJointNames
(RosJointState& entity, const std::string& docstring)
: Command (entity, boost::assign::list_of (Value::STRING), docstring)
- {}
-
- namespace
{
- void
- buildJointNames (sensor_msgs::JointState& jointState, CjrlJoint* joint)
- {
- if (!joint)
- return;
- // Ignore anchors.
- if (joint->numberDof() != 0)
- {
+}
+
+ namespace {
+ void buildJointNames (sensor_msgs::JointState& jointState, se3::Model& robot_model) {
+ for (int i=0;i<robot_model.nbody-1;i++) {
+ // Ignore anchors.
+ if (se3::nv(robot_model.joints[i]) != 0) {
// If we only have one dof, the dof name is the joint name.
- if (joint->numberDof() == 1)
- {
- jointState.name[joint->rankInConfiguration()] =
- joint->getName();
+ if (se3::nv(robot_model.joints[i]) == 1) {
+ jointState.name[i] = robot_model.names[i];
+ }
+ else {
+ // ...otherwise, the dof name is the joint name on which
+ // the dof id is appended.
+ int joint_dof = se3::nv(robot_model.joints[i]);
+ for(int j = 0; j<joint_dof; j++) {
+ boost::format fmt("%1%_%2%");
+ fmt % robot_model.names[i];
+ fmt % j;
+ jointState.name[i + j] = fmt.str();
}
- // ...otherwise, the dof name is the joint name on which
- // the dof id is appended.
- else
- for (unsigned i = 0; i < joint->numberDof(); ++i)
- {
- boost::format fmt("%1%_%2%");
- fmt % joint->getName();
- fmt % i;
- jointState.name[joint->rankInConfiguration() + i] =
- fmt.str();
- }
+ }
}
- for (unsigned i = 0; i < joint->countChildJoints (); ++i)
- buildJointNames (jointState, joint->childJoint (i));
+ }
}
} // end of anonymous namespace
-
+
Value RetrieveJointNames::doExecute ()
{
RosJointState& entity = static_cast<RosJointState&> (owner ());
@@ -89,15 +82,15 @@ namespace dynamicgraph
return Value ();
}
- CjrlHumanoidDynamicRobot* robot = dynamic->m_HDR;
- if (!robot)
+ se3::Model& robot_model = dynamic->m_model;
+ if (robot_model.nbody == 1)
{
std::cerr << "no robot in the dynamic entity" << std::endl;
return Value ();
}
- entity.jointState ().name.resize (robot->numberDof());
- buildJointNames (entity.jointState (), robot->rootJoint());
+ entity.jointState ().name.resize (robot_model.nv);
+ buildJointNames (entity.jointState (), robot_model);
return Value ();
}
} // end of namespace command.
diff --git a/src/ros_joint_state.hh b/src/ros_joint_state.hh
index cb21157..10ee53a 100644
--- a/src/ros_joint_state.hh
+++ b/src/ros_joint_state.hh
@@ -19,7 +19,7 @@ namespace dynamicgraph
DYNAMIC_GRAPH_ENTITY_DECL();
public:
/// \brief Vector input signal.
- typedef SignalPtr<ml::Vector, int> signalVectorIn_t;
+ typedef SignalPtr<Vector, int> signalVectorIn_t;
static const double ROS_JOINT_STATE_PUBLISHER_RATE;
diff --git a/src/ros_publish.cpp b/src/ros_publish.cpp
index 3bcf22b..20c4b9d 100644
--- a/src/ros_publish.cpp
+++ b/src/ros_publish.cpp
@@ -13,6 +13,7 @@
#include <dynamic-graph/factory.h>
#include <dynamic-graph/command.h>
+#include <dynamic-graph/linear-algebra.h>
#include "dynamic_graph_bridge/ros_init.hh"
#include "ros_publish.hh"
@@ -82,22 +83,22 @@ namespace dynamicgraph
else if (type == "unsigned")
entity.add<unsigned int> (signal, topic);
else if (type == "matrix")
- entity.add<ml::Matrix> (signal, topic);
+ entity.add<Matrix> (signal, topic);
else if (type == "vector")
- entity.add<ml::Vector> (signal, topic);
+ entity.add<Vector> (signal, topic);
else if (type == "vector3")
entity.add<specific::Vector3> (signal, topic);
else if (type == "vector3Stamped")
- entity.add<std::pair<specific::Vector3, ml::Vector> > (signal, topic);
+ entity.add<std::pair<specific::Vector3, Vector> > (signal, topic);
else if (type == "matrixHomo")
entity.add<sot::MatrixHomogeneous> (signal, topic);
else if (type == "matrixHomoStamped")
- entity.add<std::pair<sot::MatrixHomogeneous, ml::Vector> >
+ entity.add<std::pair<sot::MatrixHomogeneous, Vector> >
(signal, topic);
else if (type == "twist")
entity.add<specific::Twist> (signal, topic);
else if (type == "twistStamped")
- entity.add<std::pair<specific::Twist, ml::Vector> > (signal, topic);
+ entity.add<std::pair<specific::Twist, Vector> > (signal, topic);
else
throw std::runtime_error("bad type");
return Value ();
@@ -109,7 +110,8 @@ namespace dynamicgraph
(entity,
boost::assign::list_of (Value::STRING),
docstring)
- {}
+ {
+}
Value Rm::doExecute ()
{
@@ -139,6 +141,7 @@ namespace dynamicgraph
rate_ (ROS_JOINT_STATE_PUBLISHER_RATE),
lastPublicated_ ()
{
+
try {
lastPublicated_ = ros::Time::now ();
} catch (const std::exception& exc) {
diff --git a/src/ros_publish.hxx b/src/ros_publish.hxx
index 34f45fc..deff4cf 100644
--- a/src/ros_publish.hxx
+++ b/src/ros_publish.hxx
@@ -15,19 +15,19 @@ namespace dynamicgraph
template <>
inline void
RosPublish::sendData
- <std::pair<sot::MatrixHomogeneous, ml::Vector> >
+ <std::pair<sot::MatrixHomogeneous, Vector> >
(boost::shared_ptr
<realtime_tools::RealtimePublisher
<SotToRos
- <std::pair<sot::MatrixHomogeneous, ml::Vector> >::ros_t> > publisher,
+ <std::pair<sot::MatrixHomogeneous, Vector> >::ros_t> > publisher,
boost::shared_ptr
<SotToRos
- <std::pair<sot::MatrixHomogeneous, ml::Vector> >::signalIn_t> signal,
+ <std::pair<sot::MatrixHomogeneous, Vector> >::signalIn_t> signal,
int time)
{
SotToRos
<std::pair
- <sot::MatrixHomogeneous, ml::Vector> >::ros_t result;
+ <sot::MatrixHomogeneous, Vector> >::ros_t result;
if (publisher->trylock ())
{
publisher->msg_.child_frame_id = "/dynamic_graph/world";
diff --git a/src/ros_subscribe.cpp b/src/ros_subscribe.cpp
index 4c1f02c..8923ed7 100644
--- a/src/ros_subscribe.cpp
+++ b/src/ros_subscribe.cpp
@@ -77,22 +77,22 @@ namespace dynamicgraph
else if (type == "unsigned")
entity.add<unsigned int> (signal, topic);
else if (type == "matrix")
- entity.add<ml::Matrix> (signal, topic);
+ entity.add<dg::Matrix> (signal, topic);
else if (type == "vector")
- entity.add<ml::Vector> (signal, topic);
+ entity.add<dg::Vector> (signal, topic);
else if (type == "vector3")
entity.add<specific::Vector3> (signal, topic);
else if (type == "vector3Stamped")
- entity.add<std::pair<specific::Vector3, ml::Vector> > (signal, topic);
+ entity.add<std::pair<specific::Vector3, dg::Vector> > (signal, topic);
else if (type == "matrixHomo")
entity.add<sot::MatrixHomogeneous> (signal, topic);
else if (type == "matrixHomoStamped")
- entity.add<std::pair<sot::MatrixHomogeneous, ml::Vector> >
+ entity.add<std::pair<sot::MatrixHomogeneous, dg::Vector> >
(signal, topic);
else if (type == "twist")
entity.add<specific::Twist> (signal, topic);
else if (type == "twistStamped")
- entity.add<std::pair<specific::Twist, ml::Vector> >
+ entity.add<std::pair<specific::Twist, dg::Vector> >
(signal, topic);
else
throw std::runtime_error("bad type");
diff --git a/src/ros_subscribe.hh b/src/ros_subscribe.hh
index 480686c..17b47e5 100644
--- a/src/ros_subscribe.hh
+++ b/src/ros_subscribe.hh
@@ -9,7 +9,7 @@
# include <dynamic-graph/signal-time-dependent.h>
# include <dynamic-graph/signal-ptr.h>
# include <dynamic-graph/command.h>
-# include <sot/core/matrix-homogeneous.hh>
+# include <sot/core/matrix-geometry.hh>
# include <ros/ros.h>
diff --git a/src/ros_subscribe.hxx b/src/ros_subscribe.hxx
index 12819e6..ded5de1 100644
--- a/src/ros_subscribe.hxx
+++ b/src/ros_subscribe.hxx
@@ -4,14 +4,14 @@
# include <boost/bind.hpp>
# include <boost/date_time/posix_time/posix_time.hpp>
# include <dynamic-graph/signal-caster.h>
+# include <dynamic-graph/linear-algebra.h>
# include <dynamic-graph/signal-cast-helper.h>
-# include <jrl/mal/boost.hh>
# include <std_msgs/Float64.h>
# include "dynamic_graph_bridge_msgs/Matrix.h"
# include "dynamic_graph_bridge_msgs/Vector.h"
# include "ros_time.hh"
-namespace ml = ::maal::boost;
+namespace dg = dynamicgraph;
namespace dynamicgraph
{
@@ -79,13 +79,13 @@ namespace dynamicgraph
};
template <typename T>
- struct Add<std::pair<T, ml::Vector> >
+ struct Add<std::pair<T, dg::Vector> >
{
void operator () (RosSubscribe& RosSubscribe,
const std::string& signal,
const std::string& topic)
{
- typedef std::pair<T, ml::Vector> type_t;
+ typedef std::pair<T, dg::Vector> type_t;
typedef typename SotToRos<type_t>::sot_t sot_t;
typedef typename SotToRos<type_t>::ros_const_ptr_t ros_const_ptr_t;
diff --git a/src/sot_loader.cpp b/src/sot_loader.cpp
index d347064..96cf677 100644
--- a/src/sot_loader.cpp
+++ b/src/sot_loader.cpp
@@ -65,13 +65,14 @@ int SotLoader::readSotVectorStateParam()
if (!ros::param::has("/sot/state_vector_map"))
{
std::cerr<< " Read Sot Vector State Param " << std::endl;
+ std::cerr<< " Can't read state_vector_map. Does not exist. "<<std::endl;
return 1;
}
n.getParam("/sot/state_vector_map", stateVectorMap_);
ROS_ASSERT(stateVectorMap_.getType() == XmlRpc::XmlRpcValue::TypeArray);
nbOfJoints_ = stateVectorMap_.size();
-
+ nbOfParallelJoints_ = 0;
if (ros::param::has("/sot/joint_state_parallel"))
{
XmlRpc::XmlRpcValue joint_state_parallel;
@@ -105,7 +106,7 @@ int SotLoader::readSotVectorStateParam()
// Prepare joint_state according to robot description.
joint_state_.name.resize(nbOfJoints_+nbOfParallelJoints_);
joint_state_.position.resize(nbOfJoints_+nbOfParallelJoints_);
-
+
// Fill in the name of the joints from the state vector.
// and build local map from state name to state vector
for (int32_t i = 0; i < stateVectorMap_.size(); ++i)
@@ -134,7 +135,7 @@ int SotLoader::readSotVectorStateParam()
int SotLoader::parseOptions(int argc, char *argv[])
-{
+{
po::options_description desc("Allowed options");
desc.add_options()
("help", "produce help message")
@@ -162,8 +163,8 @@ int SotLoader::parseOptions(int argc, char *argv[])
void SotLoader::Initialization()
{
-
- // Load the SotRobotBipedController library.
+
+ // Load the SotRobotBipedController library.
void * SotRobotControllerLibrary = dlopen( dynamicLibraryName_.c_str(),
RTLD_LAZY | RTLD_GLOBAL );
if (!SotRobotControllerLibrary) {
@@ -214,7 +215,9 @@ SotLoader::readControl(map<string,dgs::ControlValues> &controlValues)
// Check if the size if coherent with the robot description.
if (angleControl_.size()!=(unsigned int)nbOfJoints_)
{
- std::cerr << " angleControl_ and nbOfJoints are different !"
+ std::cerr << " angleControl_"<<angleControl_.size()
+ << " and nbOfJoints"<<(unsigned int)nbOfJoints_
+ << " are different !"
<< std::endl;
exit(-1);
}
diff --git a/src/sot_to_ros.cpp b/src/sot_to_ros.cpp
index 701087c..e7a2075 100644
--- a/src/sot_to_ros.cpp
+++ b/src/sot_to_ros.cpp
@@ -5,19 +5,19 @@ namespace dynamicgraph
const char* SotToRos<double>::signalTypeName = "Double";
const char* SotToRos<unsigned int>::signalTypeName = "Unsigned";
- const char* SotToRos<ml::Matrix>::signalTypeName = "Matrix";
- const char* SotToRos<ml::Vector>::signalTypeName = "Vector";
+ const char* SotToRos<Matrix>::signalTypeName = "Matrix";
+ const char* SotToRos<Vector>::signalTypeName = "Vector";
const char* SotToRos<specific::Vector3>::signalTypeName = "Vector3";
const char* SotToRos<sot::MatrixHomogeneous>::signalTypeName = "MatrixHomo";
const char* SotToRos<specific::Twist>::signalTypeName = "Twist";
const char* SotToRos
- <std::pair<specific::Vector3, ml::Vector> >::signalTypeName
+ <std::pair<specific::Vector3, Vector> >::signalTypeName
= "Vector3Stamped";
const char* SotToRos
- <std::pair<sot::MatrixHomogeneous, ml::Vector> >::signalTypeName
+ <std::pair<sot::MatrixHomogeneous, Vector> >::signalTypeName
= "MatrixHomo";
const char* SotToRos
- <std::pair<specific::Twist, ml::Vector> >::signalTypeName
+ <std::pair<specific::Twist, Vector> >::signalTypeName
= "Twist";
} // end of namespace dynamicgraph.
diff --git a/src/sot_to_ros.hh b/src/sot_to_ros.hh
index d8a2cfa..a09f91c 100644
--- a/src/sot_to_ros.hh
+++ b/src/sot_to_ros.hh
@@ -5,7 +5,6 @@
# include <boost/format.hpp>
-# include <jrl/mal/boost.hh>
# include <std_msgs/Float64.h>
# include <std_msgs/UInt32.h>
# include "dynamic_graph_bridge_msgs/Matrix.h"
@@ -18,8 +17,9 @@
# include "geometry_msgs/Vector3Stamped.h"
# include <dynamic-graph/signal-time-dependent.h>
+# include <dynamic-graph/linear-algebra.h>
# include <dynamic-graph/signal-ptr.h>
-# include <sot/core/matrix-homogeneous.hh>
+# include <sot/core/matrix-geometry.hh>
# define MAKE_SIGNAL_STRING(NAME, IS_INPUT, OUTPUT_TYPE, SIGNAL_NAME) \
makeSignalString (typeid (*this).name (), NAME, \
@@ -27,7 +27,6 @@
namespace dynamicgraph
{
- namespace ml = maal::boost;
/// \brief Types dedicated to identify pairs of (dg,ros) data.
///
@@ -88,9 +87,9 @@ namespace dynamicgraph
};
template <>
- struct SotToRos<ml::Matrix>
+ struct SotToRos<Matrix>
{
- typedef ml::Matrix sot_t;
+ typedef Matrix sot_t;
typedef dynamic_graph_bridge_msgs::Matrix ros_t;
typedef dynamic_graph_bridge_msgs::MatrixConstPtr ros_const_ptr_t;
typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
@@ -102,16 +101,16 @@ namespace dynamicgraph
template <typename S>
static void setDefault(S& s)
{
- ml::Matrix m;
+ Matrix m;
m.resize(0, 0);
s.setConstant (m);
}
};
template <>
- struct SotToRos<ml::Vector>
+ struct SotToRos<Vector>
{
- typedef ml::Vector sot_t;
+ typedef Vector sot_t;
typedef dynamic_graph_bridge_msgs::Vector ros_t;
typedef dynamic_graph_bridge_msgs::VectorConstPtr ros_const_ptr_t;
typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
@@ -123,7 +122,7 @@ namespace dynamicgraph
template <typename S>
static void setDefault(S& s)
{
- ml::Vector v;
+ Vector v;
v.resize (0);
s.setConstant (v);
}
@@ -132,7 +131,7 @@ namespace dynamicgraph
template <>
struct SotToRos<specific::Vector3>
{
- typedef ml::Vector sot_t;
+ typedef Vector sot_t;
typedef geometry_msgs::Vector3 ros_t;
typedef geometry_msgs::Vector3ConstPtr ros_const_ptr_t;
typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
@@ -144,7 +143,7 @@ namespace dynamicgraph
template <typename S>
static void setDefault(S& s)
{
- ml::Vector v;
+ Vector v;
v.resize (0);
s.setConstant (v);
}
@@ -173,7 +172,7 @@ namespace dynamicgraph
template <>
struct SotToRos<specific::Twist>
{
- typedef ml::Vector sot_t;
+ typedef Vector sot_t;
typedef geometry_msgs::Twist ros_t;
typedef geometry_msgs::TwistConstPtr ros_const_ptr_t;
typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
@@ -185,7 +184,7 @@ namespace dynamicgraph
template <typename S>
static void setDefault(S& s)
{
- ml::Vector v (6);
+ Vector v (6);
v.setZero ();
s.setConstant (v);
}
@@ -193,9 +192,9 @@ namespace dynamicgraph
// Stamped vector3
template <>
- struct SotToRos<std::pair<specific::Vector3, ml::Vector> >
+ struct SotToRos<std::pair<specific::Vector3, Vector> >
{
- typedef ml::Vector sot_t;
+ typedef Vector sot_t;
typedef geometry_msgs::Vector3Stamped ros_t;
typedef geometry_msgs::Vector3StampedConstPtr ros_const_ptr_t;
typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
@@ -213,7 +212,7 @@ namespace dynamicgraph
// Stamped transformation
template <>
- struct SotToRos<std::pair<sot::MatrixHomogeneous, ml::Vector> >
+ struct SotToRos<std::pair<sot::MatrixHomogeneous, Vector> >
{
typedef sot::MatrixHomogeneous sot_t;
typedef geometry_msgs::TransformStamped ros_t;
@@ -233,9 +232,9 @@ namespace dynamicgraph
// Stamped twist
template <>
- struct SotToRos<std::pair<specific::Twist, ml::Vector> >
+ struct SotToRos<std::pair<specific::Twist, Vector> >
{
- typedef ml::Vector sot_t;
+ typedef Vector sot_t;
typedef geometry_msgs::TwistStamped ros_t;
typedef geometry_msgs::TwistStampedConstPtr ros_const_ptr_t;
typedef dynamicgraph::SignalTimeDependent<sot_t, int> signal_t;
--
GitLab