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 @@
/* STD */
#include <string>
#include <map>
/* Matrix */
#include <dynamic-graph/linear-algebra.h>
/*Python API*/
#include <Python.h>
/* SOT */
#include <sot/core/flags.hh>
......@@ -97,12 +95,11 @@ class SOTDYNAMIC_EXPORT Dynamic
DYNAMIC_GRAPH_ENTITY_DECL();
/* --- MODEL ATRIBUTES --- */
se3::Model* m_model;
se3::Model m_model;
se3::Data* m_data;
std::string m_urdfPath;
/* --- MODEL ATRIBUTES --- */
PyObject* getPinocchioModel(PyObject*, PyObject* args);
public:
/* --- SIGNAL ACTIVATION --- */
......@@ -182,13 +179,18 @@ class SOTDYNAMIC_EXPORT Dynamic
/// \note creates a pinocchio model. needs urdfdom libraries to parse.
void setUrdfFile( const std::string& path );
//TODO:
// void parseAndAddFrames(se3::Model& pinocchio_model,
// const std::string& filename);
/// \name Construction of a robot by commands
///@{
///
/// \brief Create an empty device
void createRobot();
void displayModel(){ std::cout<<*m_model<<std::endl; };
void displayModel(){ std::cout<<m_model<<std::endl; };
/// \brief create a joint
/// \param inJointName name of the joint,
......@@ -259,34 +261,54 @@ class SOTDYNAMIC_EXPORT Dynamic
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
///
/// \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,
const dg::Vector& lowPos);
const dg::Vector& lowPos);
void setLowerPositionLimit(const std::string& inJointName,
const double lowPos);
/// \brief Set upper bound of joint position
///
/// \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,
const dg::Vector& lowPos);
const double upPos);
/// \brief Set upper bound of joint velocities
///
/// \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,
const dg::Vector& maxVel);
void setMaxVelocityLimit(const std::string& inJointName,
const double maxVel);
/// \brief Set upper bound of joint effort
///
/// \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,
const dg::Vector& maxEffort);
const dg::Vector& maxEffort);
void setMaxEffortLimit(const std::string& inJointName,
const double maxEffort);
/* --- GETTERS --- */
......@@ -353,6 +375,9 @@ class SOTDYNAMIC_EXPORT Dynamic
std::map<std::string, JointDetails> jointMap_;
std::vector<std::string> jointTypes;
std::map<std::string,std::string> specificitiesMap;
};
// std::ostream& operator<<(std::ostream& os, const CjrlJoint& r);
......
......@@ -29,13 +29,6 @@
#include <sot/core/matrix-geometry.hh>
namespace dynamicsJRLJapan
{
class HumanoidDynamicMultiBody;
}
class CjrlHumanoidDynamicRobot;
class CjrlJoint;
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
......
......@@ -35,7 +35,10 @@ namespace dynamicgraph { namespace sot {
class FromSensor : public Command
{
public:
virtual ~FromSensor() {}
virtual ~FromSensor() {
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
......
......@@ -36,7 +36,8 @@ namespace dynamicgraph { namespace sot {
class SetFile : public Command
{
public:
virtual ~SetFile() {}
virtual ~SetFile() { 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
......@@ -58,7 +59,8 @@ namespace dynamicgraph { namespace sot {
class CreateRobot : public Command
{
public:
virtual ~CreateRobot() {}
virtual ~CreateRobot() { 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
......@@ -79,7 +81,8 @@ namespace dynamicgraph { namespace sot {
class DisplayModel : public Command
{
public:
virtual ~DisplayModel() {}
virtual ~DisplayModel() { 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
......@@ -100,7 +103,8 @@ namespace dynamicgraph { namespace sot {
class CreateJoint : public Command
{
public:
virtual ~CreateJoint() {}
virtual ~CreateJoint() { 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
......@@ -125,7 +129,8 @@ namespace dynamicgraph { namespace sot {
class AddBody : public Command
{
public:
virtual ~AddBody() {}
virtual ~AddBody() { 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
......@@ -150,7 +155,8 @@ namespace dynamicgraph { namespace sot {
class SetMass : public Command
{
public:
virtual ~SetMass() {}
virtual ~SetMass() { 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
......@@ -174,7 +180,8 @@ namespace dynamicgraph { namespace sot {
class SetLocalCenterOfMass : public Command
{
public:
virtual ~SetLocalCenterOfMass() {}
virtual ~SetLocalCenterOfMass() { 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
......@@ -198,7 +205,8 @@ namespace dynamicgraph { namespace sot {
class SetInertiaMatrix : public Command
{
public:
virtual ~SetInertiaMatrix() {}
virtual ~SetInertiaMatrix() { 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
......@@ -222,7 +230,8 @@ namespace dynamicgraph { namespace sot {
class SetInertiaProperties : public Command
{
public:
virtual ~SetInertiaProperties() {}
virtual ~SetInertiaProperties() { 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
......@@ -245,12 +254,39 @@ namespace dynamicgraph { namespace sot {
}
}; // 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
class SetLowerPositionLimit : public Command
{
public:
virtual ~SetLowerPositionLimit() {}
virtual ~SetLowerPositionLimit() { 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
......@@ -270,11 +306,14 @@ namespace dynamicgraph { namespace sot {
}
}; // class SetLowerPositionLimit
// Command SetUpperPositionLimit
class SetUpperPositionLimit : public Command
{
public:
virtual ~SetUpperPositionLimit() {}
virtual ~SetUpperPositionLimit() { 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
......@@ -298,7 +337,8 @@ namespace dynamicgraph { namespace sot {
class SetMaxVelocityLimit : public Command
{
public:
virtual ~SetMaxVelocityLimit() {}
virtual ~SetMaxVelocityLimit() { 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
......@@ -323,7 +363,8 @@ namespace dynamicgraph { namespace sot {
class SetMaxEffortLimit : public Command
{
public:
virtual ~SetMaxEffortLimit() {}
virtual ~SetMaxEffortLimit() { 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
......@@ -344,6 +385,54 @@ namespace dynamicgraph { namespace sot {
}; // 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 sot */} /* namespace dynamicgraph */
......
......@@ -21,7 +21,17 @@
#include <sot-dynamic/dynamic.h>
#include <dynamic-graph/factory.h>
using namespace dynamicgraph::sot;
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 @@
ADD_DEFINITIONS(-DDEBUG=2)
SET(tests
dev_test_all_compute
test_constructor
test_config
# dummy
# test_djj
# 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