Skip to content
GitLab
Commit 703372f9 authored by Rohan Budhiraja's avatar Rohan Budhiraja
Browse files

Use pinocchio model object in python to access Model instance 

* Use pointer to pinocchio model object instead of holding the resource in Dynamic entity
* Remove API for setting model parameters. Use Pinocchio API directly
parent 83fdbb2b
Expand all Hide whitespace changes
Inline Side-by-side
.gitignore
View file @ 703372f9
*.pyc
*~
build
.gitignore
CMakeLists.txt
View file @ 703372f9
......@@ -40,6 +40,7 @@ SET(PKG_CONFIG_ADDITIONAL_VARIABLES
SETUP_PROJECT()
# Search for dependencies.
ADD_REQUIRED_DEPENDENCY("pinocchio")
ADD_REQUIRED_DEPENDENCY("eigenpy")
ADD_REQUIRED_DEPENDENCY("dynamic-graph >= 3.0.0")
ADD_REQUIRED_DEPENDENCY("sot-core >= 3.0.0")
ADD_REQUIRED_DEPENDENCY("sot-tools >= 2.0.0")
......@@ -68,7 +69,7 @@ PKG_CONFIG_APPEND_LIBS(${LIBRARY_NAME})
# Search for dependencies.
# Boost
SET(BOOST_COMPONENTS filesystem system unit_test_framework)
SET(BOOST_COMPONENTS filesystem system unit_test_framework python)
SEARCH_FOR_BOOST()
SEARCH_FOR_EIGEN()
......@@ -77,7 +78,7 @@ ADD_SUBDIRECTORY(include)
ADD_SUBDIRECTORY(src)
ADD_SUBDIRECTORY(doc)
ADD_SUBDIRECTORY(python)
ADD_SUBDIRECTORY(unitTesting)
#ADD_SUBDIRECTORY(unitTesting)
SETUP_PROJECT_FINALIZE()
SETUP_PROJECT_CPACK()
custom_cmake/python.cmake 0 → 100644
View file @ 703372f9
# Copyright (C) 2008-2016 LAAS-CNRS, JRL AIST-CNRS.
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <http://www.gnu.org/licenses/>.
#
# SOT_DYNAMIC_PYTHON_MODULE SUBMODULENAME LIBRARYNAME TARGETNAME
# ---------------------------
#
# Add a python submodule to dynamic_graph
#
# SUBMODULENAME : the name of the submodule (can be foo/bar),
#
# LIBRARYNAME : library to link the submodule with.
#
# TARGETNAME : name of the target: should be different for several
# calls to the macro.
#
# NOTICE : Before calling this macro, set variable NEW_ENTITY_CLASS as
# the list of new Entity types that you want to be bound.
# Entity class name should match the name referencing the type
# in the factory.
#
MACRO(SOT_DYNAMIC_PYTHON_MODULE SUBMODULENAME LIBRARYNAME TARGETNAME)
FINDPYTHON()
SET(PYTHON_MODULE ${TARGETNAME})
ADD_LIBRARY(${PYTHON_MODULE}
MODULE
${PROJECT_SOURCE_DIR}/src/python-module-py.cpp)
#${PROJECT_SOURCE_DIR}/src/sot-dynamic-py.cpp)
FILE(MAKE_DIRECTORY ${PROJECT_BINARY_DIR}/src/dynamic_graph/${SUBMODULENAME})
SET_TARGET_PROPERTIES(${PYTHON_MODULE}
PROPERTIES PREFIX ""
OUTPUT_NAME dynamic_graph/${SUBMODULENAME}/wrap
)
TARGET_LINK_LIBRARIES(${PYTHON_MODULE} "-Wl,--no-as-needed")
TARGET_LINK_LIBRARIES(${PYTHON_MODULE} ${LIBRARYNAME} ${PYTHON_LIBRARY})
TARGET_LINK_LIBRARIES(${PYTHON_MODULE} ${Boost_LIBRARIES})
TARGET_LINK_LIBRARIES(${PYTHON_MODULE} pinocchio)
TARGET_LINK_LIBRARIES(${PYTHON_MODULE} eigenpy)
PKG_CONFIG_USE_DEPENDENCY(${PYTHON_MODULE} dynamic-graph)
PKG_CONFIG_USE_DEPENDENCY(${PYTHON_MODULE} pinocchio)
PKG_CONFIG_USE_DEPENDENCY(${PYTHON_MODULE} eigenpy)
INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_PATH})
#
# Installation
#
SET(PYTHON_INSTALL_DIR ${PYTHON_SITELIB}/dynamic_graph/${SUBMODULENAME})
INSTALL(TARGETS ${PYTHON_MODULE}
DESTINATION
${PYTHON_INSTALL_DIR})
SET(ENTITY_CLASS_LIST "")
FOREACH (ENTITY ${NEW_ENTITY_CLASS})
SET(ENTITY_CLASS_LIST "${ENTITY_CLASS_LIST}${ENTITY}('')\n")
ENDFOREACH(ENTITY ${NEW_ENTITY_CLASS})
CONFIGURE_FILE(
${PROJECT_SOURCE_DIR}/cmake/dynamic_graph/submodule/__init__.py.cmake
${PROJECT_BINARY_DIR}/src/dynamic_graph/${SUBMODULENAME}/__init__.py
)
INSTALL(
FILES ${PROJECT_BINARY_DIR}/src/dynamic_graph/${SUBMODULENAME}/__init__.py
DESTINATION ${PYTHON_INSTALL_DIR}
)
ENDMACRO(DYNAMIC_GRAPH_PYTHON_MODULE SUBMODULENAME)
\ No newline at end of file
include/sot-dynamic/dynamic.h
View file @ 703372f9
......@@ -27,6 +27,7 @@
/* STD */
#include <Python.h>
#include <string>
#include <map>
/* Matrix */
......@@ -46,10 +47,10 @@
/* PINOCCHIO */
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/multibody/joint/joint-variant.hpp>
#include <pinocchio/multibody/parser/urdf.hpp>
#include <pinocchio/parsers/urdf.hpp>
#include <pinocchio/algorithm/rnea.hpp>
#include <pinocchio/algorithm/jacobian.hpp>
#include <pinocchio/algorithm/operational-frames.hpp>
#include <pinocchio/algorithm/frames.hpp>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
......@@ -66,7 +67,8 @@
#endif
namespace dynamicgraph { namespace sot {
namespace dynamicgraph {
namespace sot {
namespace dg = dynamicgraph;
namespace command {
......@@ -96,9 +98,8 @@ 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 --- */
......@@ -123,7 +124,6 @@ class SOTDYNAMIC_EXPORT Dynamic
void destroyAccelerationSignal( const std::string& signame );
/*! @} */
bool init;
std::list< dg::SignalBase<int>* > genericSignalRefs;
......@@ -174,141 +174,10 @@ class SOTDYNAMIC_EXPORT Dynamic
/* --- MODEL CREATION --- */
/// \brief sets a urdf filepath to create robot model. Call parseUrdfFile to parse
/// \param path file location.
///
void setUrdfFile( const std::string& path );
/// \brief parses a urdf filepath to create robot model. Call setUrdfFile to give path
/// \param none.
///
/// \note creates a pinocchio model. needs urdfdom libraries to parse.
void parseUrdfFile(void);
/// \brief Create an empty device
void createRobot();
void displayModel() const
{ std::cout<<m_model<<std::endl; };
/// \brief create a joint
/// \param inJointName name of the joint,
/// \param inJointType type of joint in ["freeflyer","rotation","translation","anchor"],
/// \param inPosition position of the joint (4x4 matrix).
///
/// \note joints are stored in a map with names as keys for retrieval by other
void createJoint(const std::string& inJointName,
const std::string& inJointType,
const dg::Matrix& inPosition);
/// \brief Add a child body.
/// \param inParentName name of the body to which a child is added.
/// \param inJointName name of the joint added.
/// \param inChildName name of the body added.
/// \param mass mass of the child body. default 0.
/// \param lever com position of the body. default zero vector.
/// \param inertia3 matrix of the body. default zero matrix.
void addBody(const std::string& inParentName,
const std::string& inJointName,
const std::string& inChildName,
const double mass,
const dg::Vector& lever,
const dg::Matrix& inertia3);
void displayModel() const
{ assert(m_model); std::cout<<(*m_model)<<std::endl; };
/// \brief Add a child body.
/// \param inParentName name of the body to which a child is added.
/// \param inJointName name of the joint added.
/// \param inChildName name of the body added.
/// \note default mass=0, default inertia=Zero Matrix, default com=Zero Vector
void addBody(const std::string& inParentName,
const std::string& inJointName,
const std::string& inChildName);
/// \brief Set mass of a body
///
/// \param inBodyName name of the body whose properties are being set,
/// \param mass mass of the body. default 0.
void setMass(const std::string& inBodyName,
const double mass = 0);
/// \brief Set COM of the body in local frame
///
/// \param inBodyName name of the body whose properties are being set,
/// \param local COM vector
void setLocalCenterOfMass(const std::string& inBodyName,
const dg::Vector& lever);
/// \brief Set Inertia Matrix of the body
///
/// \param inBodyName name of the body whose properties are being set,
/// \param Inertia matrix
void setInertiaMatrix(const std::string& inBodyName,
const dg::Matrix& inertia3);
/// \brief Set Inertia properties of a body
///
/// \param inBodyName name of the body whose properties are being set,
/// \param mass mass of the body. default 0.
/// \param lever com position of the body. default zero vector,
/// \param inertia3 inertia matrix of the body. default zero matrix.
void setInertiaProperties(const std::string& inBodyName,
const double mass,
const dg::Vector& lever,
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 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);
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, 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 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, 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, or a double containing limits for a revolute joint.
void setMaxEffortLimit(const std::string& inJointName,
const dg::Vector& maxEffort);
void setMaxEffortLimit(const std::string& inJointName,
const double maxEffort);
/* --- GETTERS --- */
......@@ -379,13 +248,6 @@ class SOTDYNAMIC_EXPORT Dynamic
dg::Vector getPinocchioVel(int);
dg::Vector getPinocchioAcc(int);
typedef std::pair<std::string,Eigen::Matrix4d> JointDetails;
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);
......
src/CMakeLists.txt
View file @ 703372f9
......@@ -14,6 +14,7 @@
# received a copy of the GNU Lesser General Public License along with
# sot-dynamic. If not, see <http://www.gnu.org/licenses/>.
INCLUDE(../custom_cmake/python.cmake)
INCLUDE(../cmake/python.cmake)
LINK_DIRECTORIES(${Boost_LIBRARY_DIRS})
......@@ -44,6 +45,7 @@ FOREACH(lib ${plugins})
TARGET_LINK_LIBRARIES(${lib} ${Boost_LIBRARIES})
PKG_CONFIG_USE_DEPENDENCY(${lib} pinocchio)
PKG_CONFIG_USE_DEPENDENCY(${lib} eigenpy)
PKG_CONFIG_USE_DEPENDENCY(${lib} sot-core)
PKG_CONFIG_USE_DEPENDENCY(${lib} dynamic-graph)
......@@ -52,7 +54,7 @@ FOREACH(lib ${plugins})
# build python submodule
STRING(REPLACE - _ PYTHON_LIBRARY_NAME ${lib})
DYNAMIC_GRAPH_PYTHON_MODULE("sot/dynamics/${PYTHON_LIBRARY_NAME}"
SOT_DYNAMIC_PYTHON_MODULE("sot/dynamics/${PYTHON_LIBRARY_NAME}"
${lib}
sot-dynamics-${PYTHON_LIBRARY_NAME}-wrap
)
......@@ -78,5 +80,4 @@ INSTALL(FILES
${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dynamics/tools.py
${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dynamics/parser.py
DESTINATION ${PYTHON_SITELIB}/dynamic_graph/sot/dynamics
)
)
\ No newline at end of file
src/dynamic-command.h
View file @ 703372f9
......@@ -32,74 +32,6 @@ namespace dynamicgraph { namespace sot {
using ::dynamicgraph::command::Command;
using ::dynamicgraph::command::Value;
// Command SetFiles
class SetFile : public Command
{
public:
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
SetFile(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 urdfFile = values[0].value();
robot.setUrdfFile(urdfFile);
return Value();
}
}; // class SetFiles
// Command Parse
class Parse : public Command
{
public:
virtual ~Parse() {}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
/// \param docstring documentation of the command
Parse(Dynamic& entity, const std::string& docstring) :
Command(entity, std::vector<Value::Type>(), docstring)
{
}
virtual Value doExecute()
{
Dynamic& robot = static_cast<Dynamic&>(owner());
if(! robot.init ) robot.parseUrdfFile();
else std::cout << " !! Already parsed." << std::endl;
// return void
return Value();
}
}; // class Parse
// Command CreateRobot
class CreateRobot : public Command
{
public:
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
CreateRobot(Dynamic& entity, const std::string& docstring) :
Command(entity, std::vector<Value::Type>(),
docstring)
{
}
virtual Value doExecute()
{
Dynamic& robot = static_cast<Dynamic&>(owner());
robot.createRobot();
return Value();
}
}; // class CreateRobot
// Command DisplayModel
class DisplayModel : public Command
{
......@@ -122,291 +54,6 @@ namespace dynamicgraph { namespace sot {
}
}; // class DisplayModel
// Command CreateJoint
class CreateJoint : public Command
{
public:
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
CreateJoint(Dynamic& entity, const std::string& docstring) :
Command(entity, boost::assign::list_of(Value::STRING)(Value::STRING)
(Value::MATRIX), docstring)
{
}
virtual Value doExecute()
{
Dynamic& robot = static_cast<Dynamic&>(owner());
std::vector<Value> values = getParameterValues();
std::string jointName = values[0].value();
std::string jointType = values[1].value();
dynamicgraph::Matrix position = values[2].value();
robot.createJoint(jointName, jointType, position);
return Value();
}
}; // class CreateJoint
// Command AddBody
class AddBody : public Command
{
public:
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
AddBody(Dynamic& entity, const std::string& docstring) :
Command(entity, boost::assign::list_of(Value::STRING)(Value::STRING)(Value::STRING),
docstring)
{
}
virtual Value doExecute()
{
Dynamic& robot = static_cast<Dynamic&>(owner());
std::vector<Value> values = getParameterValues();
std::string parentName = values[0].value();
std::string jointName = values[1].value();
std::string childName = values[2].value();
robot.addBody(parentName,jointName,childName);
return Value();
}
}; // class AddBody
// Command SetMass
class SetMass : public Command
{
public:
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
SetMass(Dynamic& entity, const std::string& docstring) :
Command(entity, boost::assign::list_of(Value::STRING)(Value::DOUBLE),
docstring)
{
}
virtual Value doExecute()
{
Dynamic& robot = static_cast<Dynamic&>(owner());
std::vector<Value> values = getParameterValues();
std::string jointName = values[0].value();
double mass = values[1].value();
robot.setMass(jointName, mass);
return Value();
}
}; // class SetMass
// Command SetLocalCenterOfMass
class SetLocalCenterOfMass : public Command
{
public:
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
SetLocalCenterOfMass(Dynamic& entity, const std::string& docstring) :
Command(entity, boost::assign::list_of(Value::STRING)(Value::VECTOR),
docstring)
{
}
virtual Value doExecute()
{
Dynamic& robot = static_cast<Dynamic&>(owner());
std::vector<Value> values = getParameterValues();
std::string jointName = values[0].value();
dynamicgraph::Vector com = values[1].value();
robot.setLocalCenterOfMass(jointName, com);
return Value();
}
}; // class SetLocalCenterOfMass
// Command SetInertiaMatrix
class SetInertiaMatrix : public Command
{
public:
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
SetInertiaMatrix(Dynamic& entity, const std::string& docstring) :
Command(entity, boost::assign::list_of(Value::STRING)(Value::MATRIX),
docstring)
{
}
virtual Value doExecute()
{
Dynamic& robot = static_cast<Dynamic&>(owner());
std::vector<Value> values = getParameterValues();
std::string jointName = values[0].value();
dynamicgraph::Matrix inertiaMatrix = values[1].value();
robot.setInertiaMatrix(jointName, inertiaMatrix);
return Value();
}
}; // class SetInertiaMatrix
// Command SetInertiaProperties
class SetInertiaProperties : public Command
{
public:
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
SetInertiaProperties(Dynamic& entity, const std::string& docstring) :
Command(entity,
boost::assign::list_of(Value::STRING)
(Value::DOUBLE)(Value::VECTOR)(Value::MATRIX),docstring)
{
}
virtual Value doExecute()
{
Dynamic& robot = static_cast<Dynamic&>(owner());
std::vector<Value> values = getParameterValues();
std::string jointName = values[0].value();
double mass = values[1].value();
dynamicgraph::Vector com = values[2].value();
dynamicgraph::Matrix inertiaMatrix = values[3].value();
robot.setInertiaProperties(jointName, mass,com,inertiaMatrix);
return Value();
}
}; // 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: