diff --git a/src/robot_model.cpp b/src/robot_model.cpp
deleted file mode 100644
index 986fb3931988ff5472dcc202fc85030d1d0e7d4b..0000000000000000000000000000000000000000
--- a/src/robot_model.cpp
+++ /dev/null
@@ -1,138 +0,0 @@
-#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.
diff --git a/src/robot_model.hh b/src/robot_model.hh
deleted file mode 100644
index 59e68fdcaebe4fc69368d6562f68f3eb0e866075..0000000000000000000000000000000000000000
--- a/src/robot_model.hh
+++ /dev/null
@@ -1,53 +0,0 @@
-#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