Skip to content
Snippets Groups Projects
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
No related branches found
No related tags found
No related merge requests found
Expand all Hide whitespace changes
Inline Side-by-side
Showing
with 402 additions and 1161 deletions
.gitignore
+ 1
− 0
View file @ 703372f9
*.pyc
*~
build
.gitignore
CMakeLists.txt
+ 3
− 2
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
+ 87
− 0
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
+ 8
− 146
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
+ 4
− 3
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
+ 1
− 381
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:
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
SetLowerPositionLimit(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();
dg::Vector in_vector = values[1].value();
robot.setLowerPositionLimit(jointName,in_vector);
return Value();
}
}; // class SetLowerPositionLimit
// Command SetUpperPositionLimit
class SetUpperPositionLimit : public Command
{
public:
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
SetUpperPositionLimit(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();
dg::Vector in_vector = values[1].value();
robot.setUpperPositionLimit(jointName,in_vector);
return Value();
}
}; // class SetUpperPositionLimit
// Command SetMaxVelocityLimit
class SetMaxVelocityLimit : public Command
{
public:
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
SetMaxVelocityLimit(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();
dg::Vector in_vector = values[1].value();
robot.setMaxVelocityLimit(jointName,in_vector);
return Value();
}
}; // class SetMaxVelocityLimit
// Command SetMaxEffortLimit
class SetMaxEffortLimit : public Command
{
public:
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
SetMaxEffortLimit(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();
dg::Vector in_vector = values[1].value();
robot.setMaxEffortLimit(jointName,in_vector);
return Value();
}
}; // class SetMaxEffortLimit
// Command GetDimension
class GetDimension : public Command
......@@ -425,38 +72,11 @@ namespace dynamicgraph { namespace sot {
virtual Value doExecute()
{
Dynamic& robot = static_cast<Dynamic&>(owner());
unsigned int dimension = robot.m_model.nv;
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 */
......
src/dynamic_graph/sot/dynamics/__init__.py
+ 8
− 3
View file @ 703372f9
......@@ -4,7 +4,12 @@ from zmp_from_forces import ZmpFromForces
DynamicOld = Dynamic
#class Dynamic (DynamicOld):
# def __init__(self):
# def getPinocchioModel():
class Dynamic (DynamicOld):
def setData(self, pinocchio_data):
dynamic.wrap.set_pinocchio_data(self.obj,pinocchio_data)
return
def setModel(self, pinocchio_model):
dynamic.wrap.set_pinocchio_model(self.obj,pinocchio_model)
return
src/python-module-py.cpp 0 → 100644
+ 182
− 0
View file @ 703372f9
// Copyright (C) 2008-2016 LAAS-CNRS, JRL AIST-CNRS.
//
// This file is part of sot-dynamic.
// sot-dynamic is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// sot-dynamic 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 Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public License
// along with sot-dynamic. If not, see <http://www.gnu.org/licenses/>.
#include <Python.h>
#include <sot-dynamic/dynamic.h>
#include <boost/python.hpp>
#include <typeinfo>
#include <cstdio>
#include <pinocchio/python/model.hpp>
#include <pinocchio/python/data.hpp>
#include <pinocchio/python/handler.hpp>
namespace dynamicgraph{
namespace sot{
/* PyObject* getPinocchioModel(PyObject* // self
,PyObject* args) {
PyObject* object = NULL;
void* pointer = NULL;
if (!PyArg_ParseTuple(args, "O", &object))
return NULL;
if (!PyCObject_Check(object)) {
PyErr_SetString(PyExc_TypeError,
"function takes a PyCObject as argument");
return NULL;
}
pointer = PyCObject_AsVoidPtr(object);
Dynamic* dyn_entity = (Dynamic*) pointer;
se3::Model* model_ptr = NULL;
try {
model_ptr = dyn_entity->m_model;
se3::python::ModelHandler& _model(& (dyn_entity->m_model));
}
catch (const std::exception& exc) {
PyErr_SetString(dgpyError, exc.what());
return NULL;
}
catch (const char* s) {
PyErr_SetString(dgpyError, s);
return NULL;
}
catch (...) {
PyErr_SetString(dgpyError, "Unknown exception");
return NULL;
}
//CATCH_ALL_EXCEPTIONS();
// Return the pointer to the signal without destructor since the signal
// is not owned by the calling object but by the Entity.
//return boost::python::incref();
return PyCObject_FromVoidPtr((void*)model_ptr, NULL);
}
*/
PyObject* setPinocchioModel(PyObject* /* self */,PyObject* args) {
PyObject* object = NULL;
PyObject* pyPinocchioObject;
void* pointer1 = NULL;
if (!PyArg_ParseTuple(args, "OO", &object, &pyPinocchioObject))
return NULL;
if (!PyCObject_Check(object)) {
PyErr_SetString(PyExc_TypeError,
"function takes a PyCObject as argument");
return NULL;
}
pointer1 = PyCObject_AsVoidPtr(object);
Dynamic* dyn_entity = (Dynamic*) pointer1;
std::string msg("Error in obtaining pinocchio model");
PyObject* dgpyError =
PyErr_NewException(const_cast<char*>(msg.c_str()), NULL, NULL);
try {
se3::python::ModelHandler cppModelHandle =
boost::python::extract<se3::python::ModelHandler>(pyPinocchioObject);
dyn_entity->m_model = cppModelHandle.ptr();
}
catch (const std::exception& exc) {
PyErr_SetString(dgpyError, exc.what());
return NULL;
}
catch (const char* s) {
PyErr_SetString(dgpyError, s);
return NULL;
}
catch (...) {
PyErr_SetString(dgpyError, "Unknown exception");
return NULL;
}
// Return the pointer to the signal without destructor since the signal
// is not owned by the calling object but by the Entity.
//return boost::python::incref();
return Py_BuildValue("");
}
PyObject* setPinocchioData(PyObject* /* self */,PyObject* args) {
PyObject* object = NULL;
PyObject* pyPinocchioObject;
void* pointer1 = NULL;
if (!PyArg_ParseTuple(args, "OO", &object, &pyPinocchioObject))
return NULL;
if (!PyCObject_Check(object)) {
PyErr_SetString(PyExc_TypeError,
"function takes a PyCObject as argument");
return NULL;
}
pointer1 = PyCObject_AsVoidPtr(object);
Dynamic* dyn_entity = (Dynamic*) pointer1;
std::string msg("Error in obtaining pinocchio model");
PyObject* dgpyError =
PyErr_NewException(const_cast<char*>(msg.c_str()), NULL, NULL);
try {
se3::python::DataHandler cppDataHandle =
boost::python::extract<se3::python::DataHandler>(pyPinocchioObject);
dyn_entity->m_data = cppDataHandle.ptr();
}
catch (const std::exception& exc) {
PyErr_SetString(dgpyError, exc.what());
return NULL;
}
catch (const char* s) {
PyErr_SetString(dgpyError, s);
return NULL;
}
catch (...) {
PyErr_SetString(dgpyError, "Unknown exception");
return NULL;
}
// Return the pointer to the signal without destructor since the signal
// is not owned by the calling object but by the Entity.
//return boost::python::incref();
return Py_BuildValue("");
}
}
}
/**
\brief List of python functions
*/
static PyMethodDef functions[] = {
/* {"get_pinocchio_model", dynamicgraph::sot::getPinocchioModel, METH_VARARGS,
"Get the pinocchio model as python object"},*/
{"set_pinocchio_model", dynamicgraph::sot::setPinocchioModel, METH_VARARGS,
"Set the model from pinocchio python object"},
{"set_pinocchio_data", dynamicgraph::sot::setPinocchioData, METH_VARARGS,
"Set the data from pinocchio python object"},
{NULL, NULL, 0, NULL} /* Sentinel */
};
PyMODINIT_FUNC
initwrap(void)
{
PyObject *m;
m = Py_InitModule("wrap", functions);
if (m == NULL)
return;
}
src/sot-dynamic.cpp
+ 94
− 611
View file @ 703372f9
This diff is collapsed.
unitTesting/kineromeo.py
+ 12
− 13
View file @ 703372f9
......@@ -23,24 +23,23 @@ set_printoptions(suppress=True, precision=7)
#Define robotName, urdfpath and initialConfig
#robotName = 'romeo'
#urdfpath = ""
#initialConfig = (0, 0, .840252, 0, 0, 0, # FF
# 0, 0, -0.3490658, 0.6981317, -0.3490658, 0, # LLEG
# 0, 0, -0.3490658, 0.6981317, -0.3490658, 0, # RLEG
# 0, # TRUNK
# 1.5, 0.6, -0.5, -1.05, -0.4, -0.3, -0.2, # LARM
# 0, 0, 0, 0, # HEAD
# 1.5, -0.6, 0.5, 1.05, -0.4, -0.3, -0.2, # RARM
# )
robotName = 'romeo'
urdfpath = "/local/rbudhira/git/proyan/sot-pinocchio/unitTesting/romeoNoToes.urdf"
initialConfig = (0, 0, .840252, 0, 0, 0, # FF
0, 0, -0.3490658, 0.6981317, -0.3490658, 0, # LLEG
0, 0, -0.3490658, 0.6981317, -0.3490658, 0, # RLEG
0, # TRUNK
1.5, 0.6, -0.5, -1.05, -0.4, -0.3, -0.2, # LARM
0, 0, 0, 0, # HEAD
1.5, -0.6, 0.5, 1.05, -0.4, -0.3, -0.2, # RARM
)
#-----------------------------------------------------------------------------
#---- DYN --------------------------------------------------------------------
#-----------------------------------------------------------------------------
dyn = Dynamic("dyn")
dyn.setFile(urdfpath)
dyn.parse()
dyn.displayModel()
inertiaRotor = (0,)*6+(5e-4,)*31
......
unitTesting/test_config.cpp
+ 1
− 1
View file @ 703372f9
......@@ -19,7 +19,7 @@
/*-----------PINOCCHIO-------------*/
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/multibody/parser/urdf.hpp>
#include <pinocchio/parsers/urdf.hpp>
using namespace dynamicgraph::sot;
......
unitTesting/test_constructor.cpp
+ 1
− 1
View file @ 703372f9
......@@ -19,7 +19,7 @@
/*-----------PINOCCHIO-------------*/
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/multibody/parser/urdf.hpp>
#include <pinocchio/parsers/urdf.hpp>
using namespace dynamicgraph::sot;
......
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