diff --git a/src/robot_model.cpp b/src/robot_model.cpp
index e2e8d31768b03f70d464add19a4a79f6ebd1831d..d849dfea36f57711a4532c0a39c4f968bddc690f 100644
--- a/src/robot_model.cpp
+++ b/src/robot_model.cpp
@@ -1,309 +1,170 @@
-#include <limits>
-
-#include <boost/bind.hpp>
-
-#include <ros/ros.h>
+#include "robot_model.hh"
-#include <dynamic-graph/command-getter.h>
+#include <dynamic-graph/all-commands.h>
#include <dynamic-graph/factory.h>
-#include <dynamic-graph/null-ptr.hh>
#include <jrl/dynamics/urdf/parser.hh>
#include "dynamic_graph_bridge/ros_init.hh"
-#include "robot_model.hh"
namespace dynamicgraph
{
- namespace dg = dynamicgraph;
- using ::dynamicgraph::command::Getter;
-
- namespace
- {
- void convert (ml::Vector& dst, const vector3d& src)
- {
- dst(0) = src[0];
- dst(1) = src[1];
- dst(2) = src[2];
- }
- } // end of anonymous namespace.
-
- namespace command
- {
- LoadFromParameterServer::LoadFromParameterServer
- (RosRobotModel& entity,
- const std::string& docstring)
- : Command (entity, std::vector<Value::Type>(), docstring)
- {}
-
- Value
- LoadFromParameterServer::doExecute()
- {
- RosRobotModel& entity =
- static_cast<RosRobotModel&>(owner ());
- entity.loadFromParameterServer ();
- return Value ();
- }
- LoadUrdf::LoadUrdf(RosRobotModel& entity, const std::string& docstring)
- : Command (entity, std::vector<Value::Type>(), docstring)
- {}
-
- Value
- LoadUrdf::doExecute()
- {
- RosRobotModel& entity =
- static_cast<RosRobotModel&>(owner ());
+RosRobotModel::RosRobotModel(const std::string& name)
+ : Dynamic(name,false),
+ jointsParameterName_("jrl_map"),
+ ns_("sot_controller")
+{
+ std::string docstring;
- const std::vector<Value>& values = getParameterValues ();
- std::string resourceName = values[0].value ();
+ docstring =
+ "\n"
+ " Load the robot model from the parameter server.\n"
+ "\n"
+ " This is the recommended method.\n"
+ "\n";
+ addCommand("loadFromParameterServer", command::makeCommandVoid0(*this,&RosRobotModel::loadFromParameterServer,docstring));
- entity.loadUrdf (resourceName);
- return Value ();
- }
- } // end of namespace command.
-
- RosRobotModel::RosRobotModel (const std::string& name)
- : Entity(name),
- robot_ (0),
- lastComputation_ (std::numeric_limits<int>::min()),
- q_ (dynamicgraph::nullptr,
- "RosRobotModel(" + name + ")::input(vector)::position"),
- dq_ (dynamicgraph::nullptr,
- "RosRobotModel(" + name + ")::input(vector)::velocity"),
- ddq_ (dynamicgraph::nullptr,
- "RosRobotModel(" + name + ")::input(vector)::acceleration"),
- zmp_ (boost::bind (&RosRobotModel::computeZmp, this, _1, _2),
- 0,
- "RosRobotModel(" + name + ")::output(vector)::zmp"),
- com_ (boost::bind (&RosRobotModel::computeCom, this, _1, _2),
- 0,
- "RosRobotModel(" + name + ")::output(vector)::com"),
- jcom_ (boost::bind (&RosRobotModel::computeJCom, this, _1, _2),
- 0,
- "RosRobotModel(" + name + ")::output(vector)::Jcom"),
- lowerJointLimits_
- (boost::bind
- (&RosRobotModel::computeLowerJointLimits, this, _1, _2),
- 0,
- "RosRobotModel(" + name + ")::output(vector)::lowerJl"),
- upperJointLimits_
- (boost::bind
- (&RosRobotModel::computeUpperJointLimits, this, _1, _2),
- 0,
- "RosRobotModel(" + name + ")::output(vector)::upperJl")
- {
- signalRegistration(q_);
- signalRegistration(dq_);
- signalRegistration(ddq_);
- signalRegistration(zmp_);
- signalRegistration(com_);
- signalRegistration(jcom_);
- signalRegistration(lowerJointLimits_);
- signalRegistration(upperJointLimits_);
-
- zmp_.addDependency (q_);
- zmp_.addDependency (dq_);
- zmp_.addDependency (ddq_);
- com_.addDependency (q_);
- com_.addDependency (dq_);
- com_.addDependency (ddq_);
- jcom_.addDependency (q_);
- jcom_.addDependency (dq_);
- jcom_.addDependency (ddq_);
+ docstring =
+ "\n"
+ " Load the robot model from an URDF file.\n"
+ "\n";
+ addCommand("loadUrdf", command::makeCommandVoid1(*this,&RosRobotModel::loadUrdf,docstring));
- std::string docstring;
+ docstring =
+ "\n"
+ " Set the controller namespace."
+ "\n";
+ addCommand("setNamespace", command::makeCommandVoid1(*this,&RosRobotModel::setNamespace,docstring));
docstring =
- "\n"
- " Load the robot model from the parameter server.\n"
- "\n"
- " This is the recommended method.\n"
- "\n";
- addCommand ("loadFromParameterServer",
- new command::LoadFromParameterServer (*this, docstring));
+ "\n"
+ " Get current configuration of the robot.\n"
+ "\n";
+ addCommand ("curConf", new command::Getter<RosRobotModel,Vector> (*this,&RosRobotModel::curConf,docstring));
docstring =
- "\n"
- " Load the robot model from an URDF file.\n"
- "\n";
- addCommand ("loadUrdf", new command::LoadUrdf (*this, docstring));
-
- docstring = " \n"
- " Get the dimension of the robot configuration.\n"
- " \n"
- " Return:\n"
- " an unsigned int: the dimension.\n"
- " \n";
- addCommand ("getDimension",
- new Getter<RosRobotModel, unsigned>
- (*this,
- &RosRobotModel::getDimension, docstring));
- }
-
- RosRobotModel::~RosRobotModel ()
- {}
-
- void
- RosRobotModel::loadUrdf (const std::string& filename)
- {
+ "\n"
+ " Maps a link name in the URDF parser to actual robot link name.\n"
+ "\n";
+ addCommand ("addJointMapping", command::makeCommandVoid2(*this,&RosRobotModel::addJointMapping,docstring));
+}
+
+RosRobotModel::~RosRobotModel()
+{}
+
+void RosRobotModel::loadUrdf (const std::string& filename)
+{
jrl::dynamics::urdf::Parser parser;
- robot_ = parser.parse(filename);
- buildSignals ();
- }
- void
- RosRobotModel::loadFromParameterServer ()
- {
+ std::map<std::string, std::string>::const_iterator it = specialJoints_.begin();
+ for (;it!=specialJoints_.end();++it) {
+ parser.specifyREPName(it->first, it->second);
+ }
+
+ m_HDR = parser.parse(filename);
+
+ ros::NodeHandle nh(ns_);
+
+ nh.setParam(jointsParameterName_, parser.JointsNamesByRank_);
+}
+
+void RosRobotModel::setNamespace (const std::string& ns)
+{
+ ns_ = 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);
+ }
+
rosInit (false);
std::string robotDescription;
- ros::param::param<std::string>
- ("robot_description", robotDescription, "");
+ ros::param::param<std::string> ("/robot_description", robotDescription, "");
+
if (robotDescription.empty ())
- throw std::runtime_error
- ("No model available as ROS parameter. Fail.");
- robot_ = parser.parseStream (robotDescription);
- buildSignals ();
- }
-
- void
- RosRobotModel::buildSignals ()
- {
- // iterate on tree nodes and add signals
- typedef dg::SignalTimeDependent<MatrixHomogeneous, int> signal_t;
- ::std::vector<CjrlJoint*> jointsVect = robot_->jointVector();
-
- for (uint i=0; i<jointsVect.size(); ++i)
- {
- CjrlJoint* currentJoint = jointsVect[i];
- std::string signame = currentJoint->getName();
-
- signal_t* sig
- = new signal_t
- (boost::bind
- (&RosRobotModel::computeBodyPosition, this, currentJoint, _1, _2),
- 0,
- "RosRobotModel(" + getName () + ")::output(matrix)::" + signame);
- sig->addDependency (q_);
- sig->addDependency (dq_);
- sig->addDependency (ddq_);
- genericSignalRefs_.push_back (sig);
- signalRegistration (*sig);
- }
- // force recomputation as the model has changed.
- lastComputation_ = std::numeric_limits<int>::min();
- }
-
- RosRobotModel::MatrixHomogeneous&
- RosRobotModel::computeBodyPosition (CjrlJoint* joint,
- MatrixHomogeneous& position,
- int t)
- {
- update (t);
- for(unsigned i = 0; i < position.nbRows(); ++i)
- for(unsigned j = 0; j < position.nbCols(); ++j)
- position.elementAt(i,j) =
- joint->currentTransformation().m[i * position.nbCols() + j];
- return position;
- }
-
- namespace
- {
- vectorN convertVector (const ml::Vector& v)
+ throw std::runtime_error("No model available as ROS parameter. Fail.");
+
+ m_HDR = parser.parseStream (robotDescription);
+
+ ros::NodeHandle nh(ns_);
+
+ nh.setParam(jointsParameterName_, parser.JointsNamesByRank_);
+
+}
+
+namespace
+{
+
+vectorN convertVector(const ml::Vector& v)
+{
+ vectorN res (v.size());
+ for (unsigned i = 0; i < v.size(); ++i)
+ res[i] = v(i);
+ return res;
+}
+
+ml::Vector convertVector(const vectorN& v)
+{
+ ml::Vector res;
+ res.resize(v.size());
+ for (unsigned i = 0; i < v.size(); ++i)
+ res(i) = v[i];
+ return res;
+}
+
+} // end of anonymous namespace.
+
+Vector RosRobotModel::curConf() const
+{
+
+ // The first 6 dofs are associated to the Freeflyer frame
+ // 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";
+ if (nh.hasParam(param_name)){
+ nh.getParam(param_name, ffpose);
+ ROS_ASSERT(ffpose.getType() == XmlRpc::XmlRpcValue::TypeArray);
+ ROS_ASSERT(ffpose.size() == 6);
+ for (int32_t i = 0; i < ffpose.size(); ++i)
+ {
+ ROS_ASSERT(ffpose[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
+ }
+ }
+ else
{
- vectorN res (v.size());
- for (unsigned i = 0; i < v.size(); ++i)
- res[i] = v(i);
- return res;
+ ffpose.setSize(6);
+ for (int32_t i = 0; i < ffpose.size(); ++i)
+ ffpose[i] = 0.0;
}
- } // end of anonymous namespace.
-
- void
- RosRobotModel::update (int t)
- {
- if (t <= lastComputation_)
- return;
-
- vectorN q = convertVector (q_ (t));
- vectorN dq = convertVector (dq_ (t));
- vectorN ddq = convertVector (dq_ (t));
-
- if (!robot_->currentConfiguration(q))
- throw std::runtime_error ("failed to update current configuration");
- if (!robot_->currentVelocity(dq))
- throw std::runtime_error ("failed to update current velocity");
- if (!robot_->currentAcceleration(ddq))
- throw std::runtime_error ("failed to update current acceleration");
- robot_->computeForwardKinematics();
- lastComputation_ = t;
- }
-
- ml::Vector&
- RosRobotModel::computeZmp (ml::Vector& zmp, int t)
- {
- zmp.resize(3);
- if (!robot_)
- throw std::runtime_error ("no robot");
- update (t);
- convert (zmp, robot_->zeroMomentumPoint());
- return zmp;
- }
-
- ml::Vector&
- RosRobotModel::computeCom (ml::Vector& com, int t)
- {
- com.resize(3);
- if (!robot_)
- throw std::runtime_error ("no robot");
- update (t);
- convert (com, robot_->positionCenterOfMass());
- return com;
- }
-
- ml::Matrix&
- RosRobotModel::computeJCom (ml::Matrix& jcom, int t)
- {
- jcom.resize(3, getDimension ());
- if (!robot_)
- throw std::runtime_error ("no robot");
- update (t);
- CjrlJoint* rootJoint = robot_->rootJoint();
- if (!rootJoint)
- throw std::runtime_error ("no root joint");
- matrixNxP jacobian;
- robot_->getJacobianCenterOfMass (*rootJoint, jacobian);
- jcom.initFromMotherLib (jacobian);
- return jcom;
- }
-
- ml::Vector&
- RosRobotModel::computeLowerJointLimits (ml::Vector& lowerJointLimits, int t)
- {
- if (!robot_)
- throw std::runtime_error ("no robot");
-
- const unsigned int& ndofs = robot_->numberDof ();
- lowerJointLimits.resize (ndofs);
-
- for (unsigned int i = 0; i< ndofs; ++i)
- lowerJointLimits (i) = robot_->lowerBoundDof (i);
- return lowerJointLimits;
- }
-
- ml::Vector&
- RosRobotModel::computeUpperJointLimits (ml::Vector& upperJointLimits, int t)
- {
- if (!robot_)
- throw std::runtime_error ("no robot");
-
- const unsigned int& ndofs = robot_->numberDof ();
- upperJointLimits.resize (ndofs);
-
- for (unsigned int i = 0; i< ndofs; ++i)
- upperJointLimits (i) = robot_->upperBoundDof (i);
- return upperJointLimits;
- }
-
- DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosRobotModel, "RosRobotModel");
+
+ if (!m_HDR )
+ 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;
+ }
+}
+
+void
+RosRobotModel::addJointMapping(const std::string &link, const std::string &repName)
+{
+ specialJoints_[link] = repName;
+}
+
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosRobotModel, "RosRobotModel");
} // end of namespace dynamicgraph.
diff --git a/src/robot_model.hh b/src/robot_model.hh
index 1bd498d1fca1eb342e0e184db28649d8570c07cd..b456b4e4f78a3b93d97e7ea416d8f045fde65b5d 100644
--- a/src/robot_model.hh
+++ b/src/robot_model.hh
@@ -1,128 +1,55 @@
#ifndef DYNAMIC_GRAPH_BRIDGE_ROBOT_MODEL_HH
# define DYNAMIC_GRAPH_BRIDGE_ROBOT_MODEL_HH
-# include <list>
-# include <string>
-
-# include <jrl/mal/boost.hh>
-# include "jrl/mal/matrixabstractlayer.hh"
-namespace ml = maal::boost;
-
-# include <jrl/dynamics/dynamicsfactory.hh>
+# include <string>
-//# include <sot/core/flags.hh>
-# include <dynamic-graph/command.h>
-# include <dynamic-graph/entity.h>
-//# include <dynamic-graph/pool.h>
-# include <dynamic-graph/signal-ptr.h>
-# include <dynamic-graph/signal-time-dependent.h>
-//# include <sot/core/exception-dynamic.hh>
-# include <sot/core/matrix-homogeneous.hh>
+#include <sot-dynamic/dynamic.h>
+#include <dynamic-graph/linear-algebra.h>
+#include "XmlRpcValue.h"
namespace dynamicgraph
{
class RosRobotModel;
- namespace command
- {
- using ::dynamicgraph::command::Command;
- using ::dynamicgraph::command::Value;
-
- /// \brief Load from the robot_description parameter of the
- /// parameter server.
- ///
- /// This is the recommended method as it ensures model consistency
- /// between the control and the other nodes.
- class LoadFromParameterServer : public Command
- {
- public:
- explicit LoadFromParameterServer(RosRobotModel& entity,
- const std::string& docstring);
- Value doExecute();
- };
-
- /// \brief Load model from an URDF file.
- class LoadUrdf : public Command
- {
- public:
- explicit LoadUrdf(RosRobotModel& entity, const std::string& docstring);
- Value doExecute();
- };
- } // end of namespace command.
-
/// \brief This entity load either the current model available in
- /// the robot_description parameter or a specified file and provides
- /// various data such as body positions, jacobians, etc.
+ /// 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
/// to realize the computation.
- class RosRobotModel : public dynamicgraph::Entity
+ class RosRobotModel : public sot::Dynamic
{
DYNAMIC_GRAPH_ENTITY_DECL();
public:
- typedef ::dynamicgraph::sot::MatrixHomogeneous MatrixHomogeneous;
RosRobotModel(const std::string& n);
virtual ~RosRobotModel();
void loadUrdf(const std::string& filename);
+ void setNamespace (const std::string& ns);
void loadFromParameterServer();
+ Vector curConf() const;
- protected:
- void buildSignals();
-
- /// \brief Callback Computing the position of the body attached
- /// to the provided joint.
- ///
- MatrixHomogeneous&
- computeBodyPosition (CjrlJoint* joint,
- MatrixHomogeneous& position,
- int t);
+ void addJointMapping(const std::string& link, const std::string& repName);
- /// \brief Update data if necessary by updating the robot
- /// configuration/velocity/acceleration.
- ///
- /// \param t current time
- void update (int t);
+ protected:
unsigned getDimension () const
{
- if (!robot_)
+ if (!m_HDR)
throw std::runtime_error ("no robot loaded");
- return robot_->numberDof();
+ return m_HDR->numberDof();
}
- ml::Vector& computeZmp (ml::Vector& zmp, int time);
- ml::Vector& computeCom (ml::Vector& com, int time);
- ml::Matrix& computeJCom (ml::Matrix& jcom, int time);
-
- ml::Vector& computeLowerJointLimits (ml::Vector&, int time);
- ml::Vector& computeUpperJointLimits (ml::Vector&, int time);
private:
- CjrlHumanoidDynamicRobot* robot_;
- std::list< ::dynamicgraph::SignalBase<int>* > genericSignalRefs_;
-
- /// \brief When did the last computation occur?
- int lastComputation_;
-
- /// \brief Robot current configuration.
- dynamicgraph::SignalPtr<ml::Vector,int> q_;
- /// \brief Robot current velocity.
- dynamicgraph::SignalPtr<ml::Vector,int> dq_;
- /// \brief Robot current acceleration.
- dynamicgraph::SignalPtr<ml::Vector,int> ddq_;
+ /// \brief Name of the parameter where the joints list will be published
+ std::string jointsParameterName_;
- /// \brief Zero Momentum Point
- dynamicgraph::SignalTimeDependent<ml::Vector,int> zmp_;
- /// \brief Center of mass
- dynamicgraph::SignalTimeDependent<ml::Vector,int> com_;
- /// \brief Center of mass jacobian
- dynamicgraph::SignalTimeDependent<ml::Matrix,int> jcom_;
+ /// \brief Name of the controller namespace
+ std::string ns_;
- /// \brief Lower joints limits
- dynamicgraph::SignalTimeDependent<ml::Vector,int> lowerJointLimits_;
- /// \brief Upper joints limits
- dynamicgraph::SignalTimeDependent<ml::Vector,int> upperJointLimits_;
+ /// \brief Special joints map for the parser
+ std::map<std::string, std::string> specialJoints_;
};
} // end of namespace dynamicgraph.
diff --git a/src/ros_publish.cpp b/src/ros_publish.cpp
index 5ef54875e81c8365e1e8ba2336b2950102f000c6..489f5962ec5f0356c7611c8aa901aa5c109bb8c3 100644
--- a/src/ros_publish.cpp
+++ b/src/ros_publish.cpp
@@ -20,7 +20,6 @@
namespace dynamicgraph
{
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosPublish, "RosPublish");
-
const double RosPublish::ROS_JOINT_STATE_PUBLISHER_RATE = 0.01;
namespace command