Skip to content
Snippets Groups Projects
Commit efa10c24 authored by olivier stasse's avatar olivier stasse
Browse files

Merge pull request #15 from bcoudrin/groovy 

Modify RosRobotModel to derive from sot::Dynamic
parents c4b44af9 42d0b25c
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/robot_model.cpp
+ 144
− 283
View file @ efa10c24
#include <limits> #include "robot_model.hh"
#include <boost/bind.hpp>
#include <ros/ros.h>
#include <dynamic-graph/command-getter.h> #include <dynamic-graph/all-commands.h>
#include <dynamic-graph/factory.h> #include <dynamic-graph/factory.h>
#include <dynamic-graph/null-ptr.hh>
#include <jrl/dynamics/urdf/parser.hh> #include <jrl/dynamics/urdf/parser.hh>
#include "dynamic_graph_bridge/ros_init.hh" #include "dynamic_graph_bridge/ros_init.hh"
#include "robot_model.hh"
namespace dynamicgraph 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) RosRobotModel::RosRobotModel(const std::string& name)
: Command (entity, std::vector<Value::Type>(), docstring) : Dynamic(name,false),
{} jointsParameterName_("jrl_map"),
ns_("sot_controller")
Value {
LoadUrdf::doExecute() std::string docstring;
{
RosRobotModel& entity =
static_cast<RosRobotModel&>(owner ());
const std::vector<Value>& values = getParameterValues (); docstring =
std::string resourceName = values[0].value (); "\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); docstring =
return Value (); "\n"
} " Load the robot model from an URDF file.\n"
} // end of namespace command. "\n";
addCommand("loadUrdf", command::makeCommandVoid1(*this,&RosRobotModel::loadUrdf,docstring));
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_);
std::string docstring; docstring =
"\n"
" Set the controller namespace."
"\n";
addCommand("setNamespace", command::makeCommandVoid1(*this,&RosRobotModel::setNamespace,docstring));
docstring = docstring =
"\n" "\n"
" Load the robot model from the parameter server.\n" " Get current configuration of the robot.\n"
"\n" "\n";
" This is the recommended method.\n" addCommand ("curConf", new command::Getter<RosRobotModel,Vector> (*this,&RosRobotModel::curConf,docstring));
"\n";
addCommand ("loadFromParameterServer",
new command::LoadFromParameterServer (*this, docstring));
docstring = docstring =
"\n" "\n"
" Load the robot model from an URDF file.\n" " Maps a link name in the URDF parser to actual robot link name.\n"
"\n"; "\n";
addCommand ("loadUrdf", new command::LoadUrdf (*this, docstring)); addCommand ("addJointMapping", command::makeCommandVoid2(*this,&RosRobotModel::addJointMapping,docstring));
}
docstring = " \n"
" Get the dimension of the robot configuration.\n" RosRobotModel::~RosRobotModel()
" \n" {}
" Return:\n"
" an unsigned int: the dimension.\n" void RosRobotModel::loadUrdf (const std::string& filename)
" \n"; {
addCommand ("getDimension",
new Getter<RosRobotModel, unsigned>
(*this,
&RosRobotModel::getDimension, docstring));
}
RosRobotModel::~RosRobotModel ()
{}
void
RosRobotModel::loadUrdf (const std::string& filename)
{
jrl::dynamics::urdf::Parser parser; jrl::dynamics::urdf::Parser parser;
robot_ = parser.parse(filename);
buildSignals ();
}
void std::map<std::string, std::string>::const_iterator it = specialJoints_.begin();
RosRobotModel::loadFromParameterServer () 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; 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); rosInit (false);
std::string robotDescription; std::string robotDescription;
ros::param::param<std::string> ros::param::param<std::string> ("/robot_description", robotDescription, "");
("robot_description", robotDescription, "");
if (robotDescription.empty ()) if (robotDescription.empty ())
throw std::runtime_error throw std::runtime_error("No model available as ROS parameter. Fail.");
("No model available as ROS parameter. Fail.");
robot_ = parser.parseStream (robotDescription); m_HDR = parser.parseStream (robotDescription);
buildSignals ();
} ros::NodeHandle nh(ns_);
void nh.setParam(jointsParameterName_, parser.JointsNamesByRank_);
RosRobotModel::buildSignals ()
{ }
// iterate on tree nodes and add signals
typedef dg::SignalTimeDependent<MatrixHomogeneous, int> signal_t; namespace
::std::vector<CjrlJoint*> jointsVect = robot_->jointVector(); {
for (uint i=0; i<jointsVect.size(); ++i) vectorN convertVector(const ml::Vector& v)
{ {
CjrlJoint* currentJoint = jointsVect[i]; vectorN res (v.size());
std::string signame = currentJoint->getName(); for (unsigned i = 0; i < v.size(); ++i)
res[i] = v(i);
signal_t* sig return res;
= new signal_t }
(boost::bind
(&RosRobotModel::computeBodyPosition, this, currentJoint, _1, _2), ml::Vector convertVector(const vectorN& v)
0, {
"RosRobotModel(" + getName () + ")::output(matrix)::" + signame); ml::Vector res;
sig->addDependency (q_); res.resize(v.size());
sig->addDependency (dq_); for (unsigned i = 0; i < v.size(); ++i)
sig->addDependency (ddq_); res(i) = v[i];
genericSignalRefs_.push_back (sig); return res;
signalRegistration (*sig); }
}
// force recomputation as the model has changed. } // end of anonymous namespace.
lastComputation_ = std::numeric_limits<int>::min();
} Vector RosRobotModel::curConf() const
{
RosRobotModel::MatrixHomogeneous&
RosRobotModel::computeBodyPosition (CjrlJoint* joint, // The first 6 dofs are associated to the Freeflyer frame
MatrixHomogeneous& position, // Freeflyer reference frame should be the same as global
int t) // frame so that operational point positions correspond to
{ // position in freeflyer frame.
update (t);
for(unsigned i = 0; i < position.nbRows(); ++i) XmlRpc::XmlRpcValue ffpose;
for(unsigned j = 0; j < position.nbCols(); ++j) ros::NodeHandle nh(ns_);
position.elementAt(i,j) = std::string param_name = "ffpose";
joint->currentTransformation().m[i * position.nbCols() + j]; if (nh.hasParam(param_name)){
return position; nh.getParam(param_name, ffpose);
} ROS_ASSERT(ffpose.getType() == XmlRpc::XmlRpcValue::TypeArray);
ROS_ASSERT(ffpose.size() == 6);
namespace for (int32_t i = 0; i < ffpose.size(); ++i)
{ {
vectorN convertVector (const ml::Vector& v) ROS_ASSERT(ffpose[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
}
}
else
{ {
vectorN res (v.size()); ffpose.setSize(6);
for (unsigned i = 0; i < v.size(); ++i) for (int32_t i = 0; i < ffpose.size(); ++i)
res[i] = v(i); ffpose[i] = 0.0;
return res;
} }
} // end of anonymous namespace.
if (!m_HDR )
void throw std::runtime_error ("no robot loaded");
RosRobotModel::update (int t) else {
{ vectorN currConf = m_HDR->currentConfiguration();
if (t <= lastComputation_) Vector res;
return; res = convertVector(currConf);
vectorN q = convertVector (q_ (t)); for (int32_t i = 0; i < ffpose.size(); ++i)
vectorN dq = convertVector (dq_ (t)); res(i) = static_cast<double>(ffpose[i]);
vectorN ddq = convertVector (dq_ (t));
return res;
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"); void
if (!robot_->currentAcceleration(ddq)) RosRobotModel::addJointMapping(const std::string &link, const std::string &repName)
throw std::runtime_error ("failed to update current acceleration"); {
robot_->computeForwardKinematics(); specialJoints_[link] = repName;
lastComputation_ = t; }
}
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosRobotModel, "RosRobotModel");
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");
} // end of namespace dynamicgraph. } // end of namespace dynamicgraph.
src/robot_model.hh
+ 19
− 92
View file @ efa10c24
#ifndef DYNAMIC_GRAPH_BRIDGE_ROBOT_MODEL_HH #ifndef DYNAMIC_GRAPH_BRIDGE_ROBOT_MODEL_HH
# define 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 <string>
# include <jrl/dynamics/dynamicsfactory.hh>
//# include <sot/core/flags.hh> #include <sot-dynamic/dynamic.h>
# include <dynamic-graph/command.h> #include <dynamic-graph/linear-algebra.h>
# include <dynamic-graph/entity.h> #include "XmlRpcValue.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>
namespace dynamicgraph namespace dynamicgraph
{ {
class RosRobotModel; 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 /// \brief This entity load either the current model available in
/// the robot_description parameter or a specified file and provides /// the robot_description parameter or a specified file and build
/// various data such as body positions, jacobians, etc. /// a Dynamic entity
/// ///
/// This relies on jrl_dynamics_urdf to load the model and jrl-dynamics /// This relies on jrl_dynamics_urdf to load the model and jrl-dynamics
/// to realize the computation. /// to realize the computation.
class RosRobotModel : public dynamicgraph::Entity class RosRobotModel : public sot::Dynamic
{ {
DYNAMIC_GRAPH_ENTITY_DECL(); DYNAMIC_GRAPH_ENTITY_DECL();
public: public:
typedef ::dynamicgraph::sot::MatrixHomogeneous MatrixHomogeneous;
RosRobotModel(const std::string& n); RosRobotModel(const std::string& n);
virtual ~RosRobotModel(); virtual ~RosRobotModel();
void loadUrdf(const std::string& filename); void loadUrdf(const std::string& filename);
void setNamespace (const std::string& ns);
void loadFromParameterServer(); void loadFromParameterServer();
Vector curConf() const;
protected: void addJointMapping(const std::string& link, const std::string& repName);
void buildSignals();
/// \brief Callback Computing the position of the body attached
/// to the provided joint.
///
MatrixHomogeneous&
computeBodyPosition (CjrlJoint* joint,
MatrixHomogeneous& position,
int t);
/// \brief Update data if necessary by updating the robot protected:
/// configuration/velocity/acceleration.
///
/// \param t current time
void update (int t);
unsigned getDimension () const unsigned getDimension () const
{ {
if (!robot_) if (!m_HDR)
throw std::runtime_error ("no robot loaded"); 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: private:
CjrlHumanoidDynamicRobot* robot_; /// \brief Name of the parameter where the joints list will be published
std::list< ::dynamicgraph::SignalBase<int>* > genericSignalRefs_; std::string jointsParameterName_;
/// \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 Zero Momentum Point /// \brief Name of the controller namespace
dynamicgraph::SignalTimeDependent<ml::Vector,int> zmp_; std::string ns_;
/// \brief Center of mass
dynamicgraph::SignalTimeDependent<ml::Vector,int> com_;
/// \brief Center of mass jacobian
dynamicgraph::SignalTimeDependent<ml::Matrix,int> jcom_;
/// \brief Lower joints limits /// \brief Special joints map for the parser
dynamicgraph::SignalTimeDependent<ml::Vector,int> lowerJointLimits_; std::map<std::string, std::string> specialJoints_;
/// \brief Upper joints limits
dynamicgraph::SignalTimeDependent<ml::Vector,int> upperJointLimits_;
}; };
} // end of namespace dynamicgraph. } // end of namespace dynamicgraph.
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment