Remove old files.

src/robot_model.cpp

-#include "robot_model.hh"
-
-#include <dynamic-graph/all-commands.h>
-#include <dynamic-graph/factory.h>
-#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), jointsParameterName_("jrl_map"), ns_("sot_controller") {
-  std::string docstring;
-
-  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));
-
-  docstring =
-      "\n"
-      "  Load the robot model from an URDF file.\n"
-      "\n";
-  addCommand("loadUrdf", command::makeCommandVoid1(*this, &RosRobotModel::loadUrdf, docstring));
-
-  docstring =
-      "\n"
-      "  Set the controller namespace."
-      "\n";
-  addCommand("setNamespace", command::makeCommandVoid1(*this, &RosRobotModel::setNamespace, docstring));
-
-  docstring =
-      "\n"
-      "  Get current configuration of the robot.\n"
-      "\n";
-  addCommand("curConf", new command::Getter<RosRobotModel, Vector>(*this, &RosRobotModel::curConf, docstring));
-
-  docstring =
-      "\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) {
-  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() + 2;  // first joint is universe, second is freeflyer
-  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) { ns_ = ns; }
-
-void RosRobotModel::loadFromParameterServer() {
-  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);
-  }
-
-  this->m_urdfPath = "";
-  if (m_data) delete m_data;
-  m_data = new se3::Data(m_model);
-  init = true;
-  ros::NodeHandle nh(ns_);
-
-  XmlRpc::XmlRpcValue JointsNamesByRank_;
-  JointsNamesByRank_.setSize(m_model.names.size());
-  // first joint is universe, second is freeflyer
-  std::vector<std::string>::const_iterator it = m_model.names.begin() + 2;
-  for (int i = 0; it != m_model.names.end(); ++it, ++i) JointsNamesByRank_[i] = (*it);
-  nh.setParam(jointsParameterName_, JointsNamesByRank_);
-}
-
-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 {
-    ffpose.setSize(6);
-    for (int32_t i = 0; i < ffpose.size(); ++i) ffpose[i] = 0.0;
-  }
-
-  if (!m_data)
-    throw std::runtime_error("no robot loaded");
-  else {
-    // 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;
-  }
-}
-
-void RosRobotModel::addJointMapping(const std::string& link, const std::string& repName) {
-  specialJoints_[link] = repName;
-}
-
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosRobotModel, "RosRobotModel");
-}  // end of namespace dynamicgraph.

src/robot_model.hh

-#ifndef DYNAMIC_GRAPH_BRIDGE_ROBOT_MODEL_HH
-#define DYNAMIC_GRAPH_BRIDGE_ROBOT_MODEL_HH
-
-#include <string>
-
-#include <sot-dynamic/dynamic.h>
-#include <dynamic-graph/linear-algebra.h>
-#include "XmlRpcValue.h"
-
-namespace dynamicgraph {
-class RosRobotModel;
-
-/// \brief This entity load either the current model available in
-/// the robot_description parameter or a specified file and build
-/// a Dynamic entity
-///
-/// This relies on pinocchio urdf parser to load the model and pinocchio
-/// to realize the computation.
-class RosRobotModel : public sot::Dynamic {
-  DYNAMIC_GRAPH_ENTITY_DECL();
-
- public:
-  RosRobotModel(const std::string& n);
-  virtual ~RosRobotModel();
-
-  void loadUrdf(const std::string& filename);
-  void setNamespace(const std::string& ns);
-  void loadFromParameterServer();
-  Vector curConf() const;
-
-  void addJointMapping(const std::string& link, const std::string& repName);
-
- protected:
-  unsigned getDimension() const {
-    if (!m_data) throw std::runtime_error("no robot loaded");
-    // TODO: Configuration vector dimension or the dof?
-    return m_model.nv;
-    // return m_model.nq;
-  }
-
- private:
-  /// \brief Name of the parameter where the joints list will be published
-  std::string jointsParameterName_;
-
-  /// \brief Name of the controller namespace
-  std::string ns_;
-
-  /// \brief Special joints map for the parser
-  std::map<std::string, std::string> specialJoints_;
-};
-}  // end of namespace dynamicgraph.
-
-#endif  //! DYNAMIC_GRAPH_BRIDGE_ROBOT_MODEL_HH