Commit 308426d7 authored by Rohan Budhiraja's avatar Rohan Budhiraja
Browse files

sot-dynamic with pinocchio dynamic library.

> TODO: Removed operational frames. Add via pinocchio
> TODO: Changed API
> TODO: Expose Pinocchio model class in python
parent 703347b2
...@@ -29,12 +29,10 @@ ...@@ -29,12 +29,10 @@
/* STD */ /* STD */
#include <string> #include <string>
#include <map> #include <map>
/* Matrix */ /* Matrix */
#include <dynamic-graph/linear-algebra.h> #include <dynamic-graph/linear-algebra.h>
/*Python API*/ /*Python API*/
#include <Python.h>
/* SOT */ /* SOT */
#include <sot/core/flags.hh> #include <sot/core/flags.hh>
...@@ -97,12 +95,11 @@ class SOTDYNAMIC_EXPORT Dynamic ...@@ -97,12 +95,11 @@ class SOTDYNAMIC_EXPORT Dynamic
DYNAMIC_GRAPH_ENTITY_DECL(); DYNAMIC_GRAPH_ENTITY_DECL();
/* --- MODEL ATRIBUTES --- */ /* --- MODEL ATRIBUTES --- */
se3::Model* m_model; se3::Model m_model;
se3::Data* m_data; se3::Data* m_data;
std::string m_urdfPath; std::string m_urdfPath;
/* --- MODEL ATRIBUTES --- */ /* --- MODEL ATRIBUTES --- */
PyObject* getPinocchioModel(PyObject*, PyObject* args);
public: public:
/* --- SIGNAL ACTIVATION --- */ /* --- SIGNAL ACTIVATION --- */
...@@ -182,13 +179,18 @@ class SOTDYNAMIC_EXPORT Dynamic ...@@ -182,13 +179,18 @@ class SOTDYNAMIC_EXPORT Dynamic
/// \note creates a pinocchio model. needs urdfdom libraries to parse. /// \note creates a pinocchio model. needs urdfdom libraries to parse.
void setUrdfFile( const std::string& path ); void setUrdfFile( const std::string& path );
//TODO:
// void parseAndAddFrames(se3::Model& pinocchio_model,
// const std::string& filename);
/// \name Construction of a robot by commands /// \name Construction of a robot by commands
///@{ ///@{
/// ///
/// \brief Create an empty device /// \brief Create an empty device
void createRobot(); void createRobot();
void displayModel(){ std::cout<<*m_model<<std::endl; }; void displayModel(){ std::cout<<m_model<<std::endl; };
/// \brief create a joint /// \brief create a joint
/// \param inJointName name of the joint, /// \param inJointName name of the joint,
...@@ -259,34 +261,54 @@ class SOTDYNAMIC_EXPORT Dynamic ...@@ -259,34 +261,54 @@ class SOTDYNAMIC_EXPORT Dynamic
const dg::Matrix& inertia3); const dg::Matrix& inertia3);
/// \brief Set the bounds of a joint degree of freedom
/// \param the name of the joint
/// \param non-negative integer: the dof id in the joint
/// \param the minimal value of the dof
/// \param: the maximal value of the dof
void setDofBounds(const std::string& inJointName,
const unsigned int inDofId,
const double inMinValue, double inMaxValue);
/// \brief Set lower bound of joint position /// \brief Set lower bound of joint position
/// ///
/// \param inJointName name of the joint, /// \param inJointName name of the joint,
/// \param lowPos vector containing lower limit bounds for all dofs of joint. /// \param vector containing lower limit bounds for all dofs of joint, or a double containing limits for a revolute joint.
void setLowerPositionLimit(const std::string& inJointName, void setLowerPositionLimit(const std::string& inJointName,
const dg::Vector& lowPos); const dg::Vector& lowPos);
void setLowerPositionLimit(const std::string& inJointName,
const double lowPos);
/// \brief Set upper bound of joint position /// \brief Set upper bound of joint position
/// ///
/// \param inJointName name of the joint, /// \param inJointName name of the joint,
/// \param upPos vector containing upper limit bounds for all dofs of joint. /// \param upPos vector containing upper limit bounds for all dofs of joint, or a double containing limits for a revolute joint.
void setUpperPositionLimit(const std::string& inJointName,
const dg::Vector& upPos);
void setUpperPositionLimit(const std::string& inJointName, void setUpperPositionLimit(const std::string& inJointName,
const dg::Vector& lowPos); const double upPos);
/// \brief Set upper bound of joint velocities /// \brief Set upper bound of joint velocities
/// ///
/// \param inJointName name of the joint, /// \param inJointName name of the joint,
/// \param maxVel vector containing upper limit bounds for all dofs of joint. /// \param maxVel vector containing upper limit bounds for all dofs of joint, or a double containing limits for a revolute joint.
void setMaxVelocityLimit(const std::string& inJointName, void setMaxVelocityLimit(const std::string& inJointName,
const dg::Vector& maxVel); const dg::Vector& maxVel);
void setMaxVelocityLimit(const std::string& inJointName,
const double maxVel);
/// \brief Set upper bound of joint effort /// \brief Set upper bound of joint effort
/// ///
/// \param inJointName name of the joint, /// \param inJointName name of the joint,
/// \param maxEffort vector containing upper limit bounds for all dofs of joint. /// \param maxEffort vector containing upper limit bounds for all dofs of joint, or a double containing limits for a revolute joint.
void setMaxEffortLimit(const std::string& inJointName, void setMaxEffortLimit(const std::string& inJointName,
const dg::Vector& maxEffort); const dg::Vector& maxEffort);
void setMaxEffortLimit(const std::string& inJointName,
const double maxEffort);
/* --- GETTERS --- */ /* --- GETTERS --- */
...@@ -353,6 +375,9 @@ class SOTDYNAMIC_EXPORT Dynamic ...@@ -353,6 +375,9 @@ class SOTDYNAMIC_EXPORT Dynamic
std::map<std::string, JointDetails> jointMap_; std::map<std::string, JointDetails> jointMap_;
std::vector<std::string> jointTypes; std::vector<std::string> jointTypes;
std::map<std::string,std::string> specificitiesMap;
}; };
// std::ostream& operator<<(std::ostream& os, const CjrlJoint& r); // std::ostream& operator<<(std::ostream& os, const CjrlJoint& r);
......
...@@ -29,13 +29,6 @@ ...@@ -29,13 +29,6 @@
#include <sot/core/matrix-geometry.hh> #include <sot/core/matrix-geometry.hh>
namespace dynamicsJRLJapan
{
class HumanoidDynamicMultiBody;
}
class CjrlHumanoidDynamicRobot;
class CjrlJoint;
/* --------------------------------------------------------------------- */ /* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */ /* --- API ------------------------------------------------------------- */
......
...@@ -35,7 +35,10 @@ namespace dynamicgraph { namespace sot { ...@@ -35,7 +35,10 @@ namespace dynamicgraph { namespace sot {
class FromSensor : public Command class FromSensor : public Command
{ {
public: public:
virtual ~FromSensor() {} virtual ~FromSensor() {
sotDEBUGIN(15);
sotDEBUGOUT(15);
}
/// Create command and store it in Entity /// Create command and store it in Entity
/// \param entity instance of Entity owning this command /// \param entity instance of Entity owning this command
/// \param docstring documentation of the command /// \param docstring documentation of the command
......
...@@ -36,7 +36,8 @@ namespace dynamicgraph { namespace sot { ...@@ -36,7 +36,8 @@ namespace dynamicgraph { namespace sot {
class SetFile : public Command class SetFile : public Command
{ {
public: public:
virtual ~SetFile() {} virtual ~SetFile() { sotDEBUGIN(15);
sotDEBUGOUT(15);}
/// Create command and store it in Entity /// Create command and store it in Entity
/// \param entity instance of Entity owning this command /// \param entity instance of Entity owning this command
/// \param docstring documentation of the command /// \param docstring documentation of the command
...@@ -58,7 +59,8 @@ namespace dynamicgraph { namespace sot { ...@@ -58,7 +59,8 @@ namespace dynamicgraph { namespace sot {
class CreateRobot : public Command class CreateRobot : public Command
{ {
public: public:
virtual ~CreateRobot() {} virtual ~CreateRobot() { sotDEBUGIN(15);
sotDEBUGOUT(15);}
/// Create command and store it in Entity /// Create command and store it in Entity
/// \param entity instance of Entity owning this command /// \param entity instance of Entity owning this command
/// \param docstring documentation of the command /// \param docstring documentation of the command
...@@ -79,7 +81,8 @@ namespace dynamicgraph { namespace sot { ...@@ -79,7 +81,8 @@ namespace dynamicgraph { namespace sot {
class DisplayModel : public Command class DisplayModel : public Command
{ {
public: public:
virtual ~DisplayModel() {} virtual ~DisplayModel() { sotDEBUGIN(15);
sotDEBUGOUT(15);}
/// Create command and store it in Entity /// Create command and store it in Entity
/// \param entity instance of Entity owning this command /// \param entity instance of Entity owning this command
/// \param docstring documentation of the command /// \param docstring documentation of the command
...@@ -100,7 +103,8 @@ namespace dynamicgraph { namespace sot { ...@@ -100,7 +103,8 @@ namespace dynamicgraph { namespace sot {
class CreateJoint : public Command class CreateJoint : public Command
{ {
public: public:
virtual ~CreateJoint() {} virtual ~CreateJoint() { sotDEBUGIN(15);
sotDEBUGOUT(15);}
/// Create command and store it in Entity /// Create command and store it in Entity
/// \param entity instance of Entity owning this command /// \param entity instance of Entity owning this command
/// \param docstring documentation of the command /// \param docstring documentation of the command
...@@ -125,7 +129,8 @@ namespace dynamicgraph { namespace sot { ...@@ -125,7 +129,8 @@ namespace dynamicgraph { namespace sot {
class AddBody : public Command class AddBody : public Command
{ {
public: public:
virtual ~AddBody() {} virtual ~AddBody() { sotDEBUGIN(15);
sotDEBUGOUT(15);}
/// Create command and store it in Entity /// Create command and store it in Entity
/// \param entity instance of Entity owning this command /// \param entity instance of Entity owning this command
/// \param docstring documentation of the command /// \param docstring documentation of the command
...@@ -150,7 +155,8 @@ namespace dynamicgraph { namespace sot { ...@@ -150,7 +155,8 @@ namespace dynamicgraph { namespace sot {
class SetMass : public Command class SetMass : public Command
{ {
public: public:
virtual ~SetMass() {} virtual ~SetMass() { sotDEBUGIN(15);
sotDEBUGOUT(15);}
/// Create command and store it in Entity /// Create command and store it in Entity
/// \param entity instance of Entity owning this command /// \param entity instance of Entity owning this command
/// \param docstring documentation of the command /// \param docstring documentation of the command
...@@ -174,7 +180,8 @@ namespace dynamicgraph { namespace sot { ...@@ -174,7 +180,8 @@ namespace dynamicgraph { namespace sot {
class SetLocalCenterOfMass : public Command class SetLocalCenterOfMass : public Command
{ {
public: public:
virtual ~SetLocalCenterOfMass() {} virtual ~SetLocalCenterOfMass() { sotDEBUGIN(15);
sotDEBUGOUT(15);}
/// Create command and store it in Entity /// Create command and store it in Entity
/// \param entity instance of Entity owning this command /// \param entity instance of Entity owning this command
/// \param docstring documentation of the command /// \param docstring documentation of the command
...@@ -198,7 +205,8 @@ namespace dynamicgraph { namespace sot { ...@@ -198,7 +205,8 @@ namespace dynamicgraph { namespace sot {
class SetInertiaMatrix : public Command class SetInertiaMatrix : public Command
{ {
public: public:
virtual ~SetInertiaMatrix() {} virtual ~SetInertiaMatrix() { sotDEBUGIN(15);
sotDEBUGOUT(15);}
/// Create command and store it in Entity /// Create command and store it in Entity
/// \param entity instance of Entity owning this command /// \param entity instance of Entity owning this command
/// \param docstring documentation of the command /// \param docstring documentation of the command
...@@ -222,7 +230,8 @@ namespace dynamicgraph { namespace sot { ...@@ -222,7 +230,8 @@ namespace dynamicgraph { namespace sot {
class SetInertiaProperties : public Command class SetInertiaProperties : public Command
{ {
public: public:
virtual ~SetInertiaProperties() {} virtual ~SetInertiaProperties() { sotDEBUGIN(15);
sotDEBUGOUT(15);}
/// Create command and store it in Entity /// Create command and store it in Entity
/// \param entity instance of Entity owning this command /// \param entity instance of Entity owning this command
/// \param docstring documentation of the command /// \param docstring documentation of the command
...@@ -245,12 +254,39 @@ namespace dynamicgraph { namespace sot { ...@@ -245,12 +254,39 @@ namespace dynamicgraph { namespace sot {
} }
}; // class SetInertiaMatrix }; // class SetInertiaMatrix
// Command SetDofBounds
class SetDofBounds : public Command
{
public:
virtual ~SetDofBounds() { sotDEBUGIN(15);
sotDEBUGOUT(15);}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
/// \param docstring documentation of the command
SetDofBounds(Dynamic& entity, const std::string& docstring) :
Command(entity, boost::assign::list_of(Value::STRING)(Value::UNSIGNED)
(Value::DOUBLE)(Value::DOUBLE), docstring)
{
}
virtual Value doExecute()
{
Dynamic& robot = static_cast<Dynamic&>(owner());
std::vector<Value> values = getParameterValues();
std::string jointName = values[0].value();
unsigned int dofId = values[1].value();
double minValue = values[2].value();
double maxValue = values[3].value();
robot.setDofBounds(jointName, dofId, minValue, maxValue);
return Value();
}
}; // class SetDofBounds
// Command SetLowerPositionLimit // Command SetLowerPositionLimit
class SetLowerPositionLimit : public Command class SetLowerPositionLimit : public Command
{ {
public: public:
virtual ~SetLowerPositionLimit() {} virtual ~SetLowerPositionLimit() { sotDEBUGIN(15);
sotDEBUGOUT(15);}
/// Create command and store it in Entity /// Create command and store it in Entity
/// \param entity instance of Entity owning this command /// \param entity instance of Entity owning this command
/// \param docstring documentation of the command /// \param docstring documentation of the command
...@@ -270,11 +306,14 @@ namespace dynamicgraph { namespace sot { ...@@ -270,11 +306,14 @@ namespace dynamicgraph { namespace sot {
} }
}; // class SetLowerPositionLimit }; // class SetLowerPositionLimit
// Command SetUpperPositionLimit // Command SetUpperPositionLimit
class SetUpperPositionLimit : public Command class SetUpperPositionLimit : public Command
{ {
public: public:
virtual ~SetUpperPositionLimit() {} virtual ~SetUpperPositionLimit() { sotDEBUGIN(15);
sotDEBUGOUT(15);}
/// Create command and store it in Entity /// Create command and store it in Entity
/// \param entity instance of Entity owning this command /// \param entity instance of Entity owning this command
/// \param docstring documentation of the command /// \param docstring documentation of the command
...@@ -298,7 +337,8 @@ namespace dynamicgraph { namespace sot { ...@@ -298,7 +337,8 @@ namespace dynamicgraph { namespace sot {
class SetMaxVelocityLimit : public Command class SetMaxVelocityLimit : public Command
{ {
public: public:
virtual ~SetMaxVelocityLimit() {} virtual ~SetMaxVelocityLimit() { sotDEBUGIN(15);
sotDEBUGOUT(15);}
/// Create command and store it in Entity /// Create command and store it in Entity
/// \param entity instance of Entity owning this command /// \param entity instance of Entity owning this command
/// \param docstring documentation of the command /// \param docstring documentation of the command
...@@ -323,7 +363,8 @@ namespace dynamicgraph { namespace sot { ...@@ -323,7 +363,8 @@ namespace dynamicgraph { namespace sot {
class SetMaxEffortLimit : public Command class SetMaxEffortLimit : public Command
{ {
public: public:
virtual ~SetMaxEffortLimit() {} virtual ~SetMaxEffortLimit() { sotDEBUGIN(15);
sotDEBUGOUT(15);}
/// Create command and store it in Entity /// Create command and store it in Entity
/// \param entity instance of Entity owning this command /// \param entity instance of Entity owning this command
/// \param docstring documentation of the command /// \param docstring documentation of the command
...@@ -344,6 +385,54 @@ namespace dynamicgraph { namespace sot { ...@@ -344,6 +385,54 @@ namespace dynamicgraph { namespace sot {
}; // class SetMaxEffortLimit }; // class SetMaxEffortLimit
// Command GetDimension
class GetDimension : public Command
{
public:
virtual ~GetDimension() { sotDEBUGIN(15);
sotDEBUGOUT(15);}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
/// \param docstring documentation of the command
GetDimension(Dynamic& entity, const std::string& docstring) :
Command(entity, std::vector<Value::Type>(),
docstring)
{
}
virtual Value doExecute()
{
Dynamic& robot = static_cast<Dynamic&>(owner());
unsigned int dimension = robot.m_model.nv;
return Value(dimension);
}
}; // class GetDimension
// Command Write
class Write : public Command
{
public:
virtual ~Write() { sotDEBUGIN(15);
sotDEBUGOUT(15);}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
/// \param docstring documentation of the command
Write(Dynamic& entity, const std::string& docstring) :
Command(entity, boost::assign::list_of(Value::STRING),
docstring)
{
}
virtual Value doExecute()
{
Dynamic& robot = static_cast<Dynamic&>(owner());
std::vector<Value> values = getParameterValues();
std::string filename = values[0].value();
std::ofstream file(filename.c_str(), std::ios_base::out);
file << (robot.m_model);
file.close();
return Value();
}
}; // class Write
} // namespace command } // namespace command
} /* namespace sot */} /* namespace dynamicgraph */ } /* namespace sot */} /* namespace dynamicgraph */
......
...@@ -21,7 +21,17 @@ ...@@ -21,7 +21,17 @@
#include <sot-dynamic/dynamic.h> #include <sot-dynamic/dynamic.h>
#include <dynamic-graph/factory.h> #include <dynamic-graph/factory.h>
using namespace dynamicgraph::sot;
using namespace dynamicgraph; using namespace dynamicgraph;
using namespace dynamicgraph::sot;
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Dynamic,"Dynamic"); extern "C" {
::dynamicgraph::Entity*
EntityMaker_Dynamic(const std::string& objname)
{
return new Dynamic (objname);
}
::dynamicgraph::EntityRegisterer
reg_Dynamic ("Dynamic",
&EntityMaker_Dynamic);
}
//DYNAMICGRAPH_FACTORY_DYNAMIC_PLUGIN(Dynamic,"Dynamic");
This diff is collapsed.
...@@ -16,7 +16,8 @@ ...@@ -16,7 +16,8 @@
ADD_DEFINITIONS(-DDEBUG=2) ADD_DEFINITIONS(-DDEBUG=2)
SET(tests SET(tests
dev_test_all_compute test_constructor
test_config
# dummy # dummy
# test_djj # test_djj
# test_dyn # test_dyn
......
Markdown is supported
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