From 703372f929ca900d8bdd7d62e2f4f4ecdab0e89b Mon Sep 17 00:00:00 2001
From: Rohan Budhiraja <budhiraja@laas.fr>
Date: Mon, 25 Jul 2016 14:16:49 +0200
Subject: [PATCH] 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
---
.gitignore | 1 +
CMakeLists.txt | 5 +-
custom_cmake/python.cmake | 87 +++
include/sot-dynamic/dynamic.h | 154 +----
src/CMakeLists.txt | 7 +-
src/dynamic-command.h | 382 +----------
src/dynamic_graph/sot/dynamics/__init__.py | 11 +-
src/python-module-py.cpp | 182 ++++++
src/sot-dynamic.cpp | 705 +++------------------
unitTesting/kineromeo.py | 25 +-
unitTesting/test_config.cpp | 2 +-
unitTesting/test_constructor.cpp | 2 +-
12 files changed, 402 insertions(+), 1161 deletions(-)
create mode 100644 custom_cmake/python.cmake
create mode 100644 src/python-module-py.cpp
diff --git a/.gitignore b/.gitignore
index 89d5637..6799242 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,3 +1,4 @@
*.pyc
+*~
build
.gitignore
diff --git a/CMakeLists.txt b/CMakeLists.txt
index c7dd8b5..294735b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -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()
diff --git a/custom_cmake/python.cmake b/custom_cmake/python.cmake
new file mode 100644
index 0000000..2c6ed4f
--- /dev/null
+++ b/custom_cmake/python.cmake
@@ -0,0 +1,87 @@
+# 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
diff --git a/include/sot-dynamic/dynamic.h b/include/sot-dynamic/dynamic.h
index eeb247f..20406b1 100644
--- a/include/sot-dynamic/dynamic.h
+++ b/include/sot-dynamic/dynamic.h
@@ -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);
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index d6bb62d..0184e4e 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -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
diff --git a/src/dynamic-command.h b/src/dynamic-command.h
index c8668dd..ae5c3f2 100644
--- a/src/dynamic-command.h
+++ b/src/dynamic-command.h
@@ -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 */
diff --git a/src/dynamic_graph/sot/dynamics/__init__.py b/src/dynamic_graph/sot/dynamics/__init__.py
index 03019a0..e6c8443 100755
--- a/src/dynamic_graph/sot/dynamics/__init__.py
+++ b/src/dynamic_graph/sot/dynamics/__init__.py
@@ -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
diff --git a/src/python-module-py.cpp b/src/python-module-py.cpp
new file mode 100644
index 0000000..4473a0d
--- /dev/null
+++ b/src/python-module-py.cpp
@@ -0,0 +1,182 @@
+// 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;
+}
diff --git a/src/sot-dynamic.cpp b/src/sot-dynamic.cpp
index 8fa47d5..13871e6 100644
--- a/src/sot-dynamic.cpp
+++ b/src/sot-dynamic.cpp
@@ -43,11 +43,8 @@ const std::string dg::sot::Dynamic::CLASS_NAME = "Dynamic";
Dynamic::
Dynamic( const std::string & name)
:Entity(name)
- ,m_model()
+ ,m_model(NULL)
,m_data(NULL)
- ,m_urdfPath("")
-
- ,init(false)
,jointPositionSIN(NULL,"sotDynamic("+name+")::input(vector)::position")
,freeFlyerPositionSIN(NULL,"sotDynamic("+name+")::input(vector)::ffposition")
@@ -112,32 +109,6 @@ Dynamic( const std::string & name)
sotDEBUGIN(5);
- jointTypes.push_back("JointModelRX");
- jointTypes.push_back("JointModelRY");
- jointTypes.push_back("JointModelRZ");
- jointTypes.push_back("JointModelRevoluteUnaligned");
- jointTypes.push_back("JointModelSpherical");
- jointTypes.push_back("JointModelSphericalZYX");
- jointTypes.push_back("JointModelPX");
- jointTypes.push_back("JointModelPY");
- jointTypes.push_back("JointModelPZ");
- jointTypes.push_back("JointModelFreeFlyer");
- jointTypes.push_back("JointModelPlanar");
- jointTypes.push_back("JointModelTranslation");
-
-
-
- //TODO: gaze_joint is fixed joint: not one of pinocchio joint types. Confirm specificities joint.
- /*
- specificitiesMap["right-wrist"] = "RWristPitch";
- specificitiesMap["left-wrist"] = "LWristPitch";
- specificitiesMap["right-ankle"] = "LAnkleRoll";
- specificitiesMap["left-ankle"] = "RAnkleRoll";
- specificitiesMap["gaze"] = "HeadRoll";
- specificitiesMap["waist"] = "waist";
- specificitiesMap["chest"] = "TrunkYaw";
- */
-
//TODO-------------------------------------------
//if( build ) buildModel();
@@ -172,25 +143,6 @@ Dynamic( const std::string & name)
//
std::string docstring;
// setFiles
- docstring =
- "\n"
- " Define files to parse in order to build the robot.\n"
- "\n"
- " Input:\n"
- " - a string: urdf file \n"
- "\n";
- addCommand("setFile",
- new command::SetFile(*this, docstring));
-
- docstring =
- "\n"
- " Parse files in order to build the robot.\n"
- "\n"
- " Input:\n"
- " - none \n"
- "\n";
- addCommand("parse",
- new command::Parse(*this, docstring));
docstring =
"\n"
@@ -201,6 +153,15 @@ Dynamic( const std::string & name)
"\n";
addCommand("displayModel",
new command::DisplayModel(*this, docstring));
+ docstring = " \n"
+ " Get the dimension of the robot configuration.\n"
+ " \n"
+ " Return:\n"
+ " an unsigned int: the dimension.\n"
+ " \n";
+ addCommand("getDimension",
+ new command::GetDimension(*this, docstring));
+
{
using namespace ::dg::command;
// CreateOpPoint
@@ -235,513 +196,39 @@ Dynamic( const std::string & name)
makeCommandVoid2(*this,&Dynamic::cmd_createPositionSignal,docstring));
}
- docstring = " \n"
- " Create an empty robot\n"
- " \n";
- addCommand("createRobot", new command::CreateRobot(*this, docstring));
-
- docstring = " \n"
- " Create a joint\n"
- " \n"
- " Input:\n"
- " - a string: name of the joint,\n"
- " - a string: type of the joint in ['JointModelRX', 'JointModelRY', 'JointModelRZ', 'JointModelRevoluteUnaligned', 'JointModelSpherical', 'JointModelSphericalZYX', 'JointModelPX', 'JointModelPY', 'JointModelPZ', 'JointModelFreeFlyer', 'JointModelPlanar', 'JointModelTranslation'],\n"
- " - a matrix: affine position of the joint.\n"
- " \n";
- addCommand("createJoint", new command::CreateJoint(*this, docstring));
-
- docstring = " \n"
- " Add a child Body\n"
- " \n"
- " Input:\n"
- " - a string: name of the parent body,\n"
- " - a string: name of the joint. Joint must be newly created with CreateJoint,\n "
- " - a string: name of the child body,\n"
- " - a double: mass of the child body. Default = 0,\n"
- " - a vector: com position of the child body. Default = Zero Vector,\n"
- " - a matrix: 3x3 inertia matrix of the body. Default = Zero Matrix,\n"
- " \n";
- addCommand("addBody", new command::AddBody(*this, docstring));
-
- docstring = " \n"
- " Set the mass of the body \n"
- " \n"
- " Input:\n"
- " - a string: name of the body whose properties are being set"
- " - a double: mass of the body."
- " \n";
- addCommand("setMass", new command::SetMass(*this, docstring));
-
- docstring = " \n"
- " Get the dimension of the robot configuration.\n"
- " \n"
- " Return:\n"
- " an unsigned int: the dimension.\n"
- " \n";
- addCommand("getDimension",
- new command::GetDimension(*this, docstring));
-
- docstring = " \n"
- " Write the robot kinematic chain in a file.\n"
- " \n"
- " Input:\n"
- " a string: a filename.\n"
- " \n";
- addCommand("write",
- new command::Write(*this, docstring));
- //TODO: add specifities commands
-
- /*
- docstring = " \n"
- " Get ankle position in left foot frame.\n"
- " \n";
- addCommand("getAnklePositionInFootFrame",
- new dynamicgraph::command::Getter<Dynamic, dynamicgraph::Vector>
- (*this, &Dynamic::getAnklePositionInFootFrame, docstring));
-
- */
- docstring = " \n"
- " Set the position of the center of mass of a body\n"
- " \n"
- " Input:\n"
- " - a string: name of the body whose properties are being set"
- " - a vector: local com position of the body."
- " \n";
- addCommand("setLocalCenterOfMass", new command::SetLocalCenterOfMass(*this, docstring));
-
- docstring = " \n"
- " Set inertia matrix of a body attached to a joint\n"
- " \n"
- " Input:\n"
- " - a string: name of the joint,\n"
- " - a matrix: inertia matrix.\n"
- " \n";
- addCommand("setInertiaMatrix",
- new command::SetInertiaMatrix(*this, docstring));
-
- docstring = " \n"
- " Set Inertia properties of a body\n"
- " \n"
- " Input:\n"
- " - a string: name of the body whose properties are being set"
- " - a double: mass of the body. default 0."
- " - a vector: com position of the body. default zero vector,"
- " - a matrix: inertia matrix of the body. default zero matrix."
- " \n";
- addCommand("setInertiaProperties", new command::SetInertiaProperties(*this, docstring));
-
-
- docstring = " \n"
- " (Deprecated. Please use setLowerPositionLimit and setUpperPositionLimit)\n"
- " Set the bounds of a joint degree of freedom\n"
- " \n"
- " Input:\n"
- " - a string: the name of the joint,\n"
- " - a non-negative integer: the dof id in the joint(0,1,2..)\n"
- " - a floating point number: the minimal value,\n"
- " - a floating point number: the maximal value.\n"
- " \n";
- addCommand("setDofBounds", new command::SetDofBounds(*this, docstring));
-
- docstring = " \n"
- " Set the lower bounds of the joint position\n"
- " \n"
- " Input:\n"
- " - a string: the name of the joint,\n"
- " - a vector: lower limit bounds for all dofs of joint,\n"
- " \n";
- addCommand("setLowerPositionLimit", new command::SetLowerPositionLimit(*this, docstring));
-
- docstring = " \n"
- " Set the upper bounds of the joint position\n"
- " \n"
- " Input:\n"
- " - a string: the name of the joint,\n"
- " - a vector: upper limit bounds for all dofs of joint,\n"
- " \n";
- addCommand("setUpperPositionLimit", new command::SetUpperPositionLimit(*this, docstring));
-
- docstring = " \n"
- " Set the upper bounds of the joint velocity\n"
- " \n"
- " Input:\n"
- " - a string: the name of the joint,\n"
- " - a vector: upper limit bounds for joint velocities,\n"
- " \n";
- addCommand("setMaxVelocityLimit", new command::SetMaxVelocityLimit(*this, docstring));
-
- docstring = " \n"
- " Set the upper bounds of the joint effort\n"
- " \n"
- " Input:\n"
- " - a string: the name of the joint,\n"
- " - a vector: upper limit bounds for joint effort,\n"
- " \n";
- addCommand("setMaxEffortLimit", new command::SetMaxEffortLimit(*this, docstring));
sotDEBUG(10)<< "Dynamic class_name address"<<&CLASS_NAME<<std::endl;
-
sotDEBUGOUT(5);
}
-
Dynamic::~Dynamic( void ) {
sotDEBUGIN(15);
// if (0!=m_model){ delete m_model; m_model=NULL;}
- if (0!=m_data){ delete m_data; m_data=NULL;}
+ if (0!=m_data)
+ { delete m_data; m_data=NULL; }
+ if (0!=m_model)
+ { delete m_model; m_model=NULL; }
+
for( std::list< SignalBase<int>* >::iterator iter = genericSignalRefs.begin();
iter != genericSignalRefs.end();
++iter) {
SignalBase<int>* sigPtr = *iter;
delete sigPtr;
}
-
- sotDEBUGOUT(15);
-}
-
-/* ---------------- CONFIG -------------------------------------------- */
-//Import from urdf file
-void Dynamic::setUrdfFile(const std::string& filename) {
- sotDEBUGIN(15);
- m_urdfPath = filename;
- sotDEBUG(15)<<"Urdf file path: "<<m_urdfPath<<std::endl;
- sotDEBUGOUT(15);
-}
-
-void Dynamic::parseUrdfFile() {
- sotDEBUGIN(15);
- if (m_urdfPath.empty()) std::cerr<<"Set input file first"<<std::endl;
- m_model = se3::urdf::buildModel(this->m_urdfPath, false);
- if (m_data) delete m_data;
- m_data = new se3::Data(m_model);
- init=true;
- std::vector<se3::Frame>::const_iterator it = m_model.operational_frames.begin();
- for(;it!=m_model.operational_frames.end();it++)
- sotDEBUG(15)<<"Operational Frames Added: "<<it->name<<std::endl;
- sotDEBUG(15)<<m_model<<std::endl;
- displayModel();
- sotDEBUGOUT(15);
-}
-
-
-//Create an empty robot
-void Dynamic::createRobot()
-{
- sotDEBUGIN(15);
- delete m_data;
- m_model= se3::Model();
- m_data = new se3::Data(m_model);
- init=true;
- sotDEBUGOUT(15);
-}
-
-
-void Dynamic::createJoint(const std::string& inJointName,
- const std::string& inJointType,
- const dg::Matrix& inPosition) {
- sotDEBUGIN(15);
- if (jointMap_.count(inJointName) >= 1)
- SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
- "a joint with name " + inJointName +
- " has already been created.");
- if (m_model.existJointName(inJointName))
- SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
- "a joint with name " + inJointName +
- " already exists in the model.");
-
- if(std::find(jointTypes.begin(),jointTypes.end(),inJointType) != jointTypes.end()) {
- Eigen::Matrix4d inposition_ = inPosition;
- JointDetails jointDetails(inJointType,inposition_);
- jointMap_[inJointName] = jointDetails;
- }
- else {
- std::vector<std::string>::iterator it; std::stringstream ss;
- for (it = jointTypes.begin(); it != jointTypes.end(); ++it) ss << *it;
- SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
- inJointType + " is not a valid type. Valid types are:" +ss.str());
- sotDEBUGOUT(15);
- }
-}
-
-
-void Dynamic::addBody(const std::string& inParentName,
- const std::string& inJointName,
- const std::string& inChildName) {
- sotDEBUGIN(15);
- addBody(inParentName,inJointName,inChildName
- ,0,Eigen::Vector3d::Zero(),Eigen::Matrix3d::Zero());
- sotDEBUGOUT(15);
-}
-
-void Dynamic::addBody(const std::string& inParentName,
- const std::string& inJointName,
- const std::string& inChildName,
- const double mass,
- const dg::Vector& lever,
- const dg::Matrix& inertia3) {
- sotDEBUGIN(15);
- if (jointMap_.count(inJointName) != 1)
- SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
- "No joint with name " + inJointName +
- " has been created.");
- if (m_model.existJointName(inJointName))
- SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
- "A joint with name " + inJointName +
- " already exists in the model.");
- if (!m_model.existBodyName(inParentName))
- SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
- "No parent body with name " + inParentName +
- " has been created.");
- se3::Model::Index parent = m_model.getBodyId(inParentName);
- const JointDetails jointDetails_ = jointMap_[inJointName];
- const std::string type = jointDetails_.first;
- const se3::Inertia inertia(mass,lever,inertia3);
- if(type == "JointModelRX")
- m_model.addBody(parent,se3::JointModelRX (),
- jointDetails_.second,inertia,inJointName,inChildName);
- else if(type == "JointModelRY" )
- m_model.addBody(parent,se3::JointModelRY (),
- jointDetails_.second,inertia,inJointName,inChildName);
- else if(type =="JointModelRZ" ) {
- m_model.addBody(parent,se3::JointModelRZ (),
- jointDetails_.second,inertia,inJointName,inChildName);
- }
- else if(type =="JointModelRevoluteUnaligned" )
- m_model.addBody(parent,se3::JointModelRevoluteUnaligned (),
- jointDetails_.second,inertia,inJointName,inChildName);
- else if(type =="JointModelSpherical" )
- m_model.addBody(parent,se3::JointModelSpherical (),
- jointDetails_.second,inertia,inJointName,inChildName);
- else if(type =="JointModelSphericalZYX" )
- m_model.addBody(parent,se3::JointModelSphericalZYX (),
- jointDetails_.second,inertia,inJointName,inChildName);
- else if(type =="JointModelPX" )
- m_model.addBody(parent,se3::JointModelPX (),
- jointDetails_.second,inertia,inJointName,inChildName);
- else if(type =="JointModelPY" )
- m_model.addBody(parent,se3::JointModelPY (),
- jointDetails_.second,inertia,inJointName,inChildName);
- else if(type =="JointModelPZ" )
- m_model.addBody(parent,se3::JointModelPZ (),
- jointDetails_.second,inertia,inJointName,inChildName);
- else if(type =="JointModelFreeFlyer" )
- m_model.addBody(parent,se3::JointModelFreeFlyer (),
- jointDetails_.second,inertia,inJointName,inChildName);
- else if(type =="JointModelPlanar" )
- m_model.addBody(parent,se3::JointModelPlanar (),
- jointDetails_.second,inertia,inJointName,inChildName);
- else if(type =="JointModelTranslation" )
- m_model.addBody(parent,se3::JointModelTranslation (),
- jointDetails_.second,inertia,inJointName,inChildName);
- else
- SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
- "Joint with type " + type +
- "should not have been created");
-
- if(m_data) delete m_data;
- m_data = new se3::Data(m_model);
- sotDEBUGOUT(15);
-}
-
- /*--------------------------------SETTERS-------------------------------------------*/
-
-void Dynamic::setMass(const std::string& inBodyName,
- const double mass) {
- sotDEBUGIN(15);
- if (!m_model.existBodyName(inBodyName))
- SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
- "No body with name " + inBodyName +
- " has been added.");
- se3::Model::Index index = m_model.getBodyId(inBodyName);
- m_model.inertias[index].mass() = mass;
- sotDEBUGOUT(15);
-}
-
-void Dynamic::setLocalCenterOfMass(const std::string& inBodyName,
- const dg::Vector& lever) {
- sotDEBUGIN(15);
- if (!m_model.existBodyName(inBodyName))
- SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
- "No body with name " + inBodyName +
- " has been added.");
- se3::Model::Index index = m_model.getBodyId(inBodyName);
- m_model.inertias[index].lever() = lever;
- sotDEBUGOUT(15);
-}
-
-void Dynamic::setInertiaMatrix(const std::string& inBodyName,
- const dg::Matrix& inertia3) {
- sotDEBUGIN(15);
- if (!m_model.existBodyName(inBodyName))
- SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
- "No body with name " + inBodyName +
- " has been added.");
- se3::Model::Index index = m_model.getBodyId(inBodyName);
- Eigen::Matrix3d inertia_3d(inertia3);
- se3::Symmetric3 symmetricMatrix(inertia_3d);
- m_model.inertias[index].inertia() = symmetricMatrix;
- sotDEBUGOUT(15);
-}
-
-void Dynamic::setInertiaProperties(const std::string& inBodyName,
- const double mass,
- const dg::Vector& lever,
- const dg::Matrix& inertia3) {
- sotDEBUGIN(15);
- if (!m_model.existBodyName(inBodyName))
- SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
- "No body with name " + inBodyName +
- " has been added.");
- se3::Inertia inertia_(mass,lever,inertia3);
- se3::Model::Index index = m_model.getBodyId(inBodyName);
- m_model.inertias[index] = inertia_;
sotDEBUGOUT(15);
}
-
-void Dynamic::setDofBounds(const std::string& inJointName,
- const unsigned int inDofId,
- const double inMinValue, double inMaxValue) {
- sotDEBUGIN(15);
- if (!m_model.existJointName(inJointName))
- SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
- "No joint with name " + inJointName +
- " has been created.");
- se3::Model::Index joint_index = m_model.getJointId(inJointName);
- int prev_cumulative_nq = se3::idx_q(m_model.joints[joint_index]);
-
- assert(se3::nq(m_model.joints[joint_index]) > inDofId);
-
- m_model.lowerPositionLimit(prev_cumulative_nq+inDofId) = inMinValue;
- m_model.upperPositionLimit(prev_cumulative_nq+inDofId) = inMaxValue;
- sotDEBUGOUT(15);
- return;
-}
-
-void Dynamic::setLowerPositionLimit(const std::string& inJointName,
- const double lowPos) {
- if (!m_model.existJointName(inJointName))
- SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
- "No joint with name " + inJointName +
- " has been created.");
- se3::Model::Index joint_index = m_model.getJointId(inJointName);
-
- int prev_cumulative_nq = se3::idx_q(m_model.joints[joint_index]);
- assert (se3::nq(m_model.joints[joint_index])==1 && "Joint is not revolute");
- m_model.lowerPositionLimit(prev_cumulative_nq) = lowPos;
- return;
-}
-
-void Dynamic::setLowerPositionLimit(const std::string& inJointName,
- const dg::Vector& lowPos) {
- if (!m_model.existJointName(inJointName))
- SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
- "No joint with name " + inJointName +
- " has been created.");
- se3::Model::Index joint_index = m_model.getJointId(inJointName);
- int prev_cumulative_nq = se3::idx_q(m_model.joints[joint_index]);
- int current_nq = se3::nq(m_model.joints[joint_index]);
- assert (lowPos.size()==current_nq);
- m_model.lowerPositionLimit.segment(prev_cumulative_nq,current_nq) = lowPos;
- return;
-}
-
-
-void Dynamic::setUpperPositionLimit(const std::string& inJointName,
- const double upPos) {
- if (!m_model.existJointName(inJointName))
- SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
- "No joint with name " + inJointName +
- " has been created.");
- se3::Model::Index joint_index = m_model.getJointId(inJointName);
- int prev_cumulative_nq = se3::idx_q(m_model.joints[joint_index]);
- assert (se3::nq(m_model.joints[joint_index])==1 && "Joint is not revolute");
- m_model.upperPositionLimit(prev_cumulative_nq) = upPos;
- return;
-}
-
-void Dynamic::setUpperPositionLimit(const std::string& inJointName,
- const dg::Vector& upPos) {
- if (!m_model.existJointName(inJointName))
- SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
- "No joint with name " + inJointName +
- " has been created.");
- se3::Model::Index joint_index = m_model.getJointId(inJointName);
- int prev_cumulative_nq = se3::idx_q(m_model.joints[joint_index]);
- int current_nq = se3::nq(m_model.joints[joint_index]);
-
- assert (upPos.size()==current_nq);
- m_model.upperPositionLimit.segment(prev_cumulative_nq,current_nq) = upPos;
- return;
-}
-
-void Dynamic::setMaxVelocityLimit(const std::string& inJointName,
- const double maxVel) {
- if (!m_model.existJointName(inJointName))
- SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
- "No joint with name " + inJointName +
- " has been created.");
- se3::Model::Index joint_index = m_model.getJointId(inJointName);
- int prev_cumulative_nv = se3::idx_v(m_model.joints[joint_index]);
-
- assert (se3::nv(m_model.joints[joint_index])==1 && "Joint is not revolute");
- m_model.velocityLimit(prev_cumulative_nv) = maxVel;
- return;
-}
-
-void Dynamic::setMaxVelocityLimit(const std::string& inJointName,
- const dg::Vector& maxVel) {
- if (!m_model.existJointName(inJointName))
- SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
- "No joint with name " + inJointName +
- " has been created.");
- se3::Model::Index joint_index = m_model.getJointId(inJointName);
- int prev_cumulative_nv = se3::idx_v(m_model.joints[joint_index]);
- int current_nv = se3::nv(m_model.joints[joint_index]);
- assert (maxVel.size()==current_nv);
- m_model.velocityLimit.segment(prev_cumulative_nv,current_nv) = maxVel;
- return;
-}
-
-
-void Dynamic::setMaxEffortLimit(const std::string& inJointName,
- const double maxEffort) {
-
- if (!m_model.existJointName(inJointName))
- SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
- "No joint with name " + inJointName +
- " has been created.");
- se3::Model::Index joint_index = m_model.getJointId(inJointName);
- int prev_cumulative_nv = se3::idx_v(m_model.joints[joint_index]);
- assert (se3::nv(m_model.joints[joint_index])==1 && "Joint is not revolute");
- m_model.effortLimit(prev_cumulative_nv) = maxEffort;
- return;
-}
-
-void Dynamic::setMaxEffortLimit(const std::string& inJointName,
- const dg::Vector& maxEffort) {
-
- if (!m_model.existJointName(inJointName))
- SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
- "No joint with name " + inJointName +
- " has been created.");
- se3::Model::Index joint_index = m_model.getJointId(inJointName);
- int prev_cumulative_nv = se3::idx_v(m_model.joints[joint_index]);
- int current_nv = se3::nv(m_model.joints[joint_index]);
- assert (maxEffort.size()==current_nv);
- m_model.effortLimit.segment(prev_cumulative_nv,current_nv) = maxEffort;
- return;
-}
-
/*--------------------------------GETTERS-------------------------------------------*/
-
dg::Vector& Dynamic::
getLowerPositionLimits(dg::Vector& res, const int&)
{
sotDEBUGIN(15);
- res.resize(m_model.nq);
- res = m_model.lowerPositionLimit;
+ assert(m_model);
+
+ res.resize(m_model->nq);
+ res = m_model->lowerPositionLimit;
+
sotDEBUG(15) << "lowerLimit (" << res << ")=" << std::endl;
sotDEBUGOUT(15);
return res;
@@ -751,8 +238,11 @@ dg::Vector& Dynamic::
getUpperPositionLimits(dg::Vector& res, const int&)
{
sotDEBUGIN(15);
- res.resize(m_model.nq);
- res = m_model.upperPositionLimit;
+ assert(m_model);
+
+ res.resize(m_model->nq);
+ res = m_model->upperPositionLimit;
+
sotDEBUG(15) << "upperLimit (" << res << ")=" <<std::endl;
sotDEBUGOUT(15);
return res;
@@ -762,8 +252,11 @@ dg::Vector& Dynamic::
getUpperVelocityLimits(dg::Vector& res, const int&)
{
sotDEBUGIN(15);
- res.resize(m_model.nv);
- res = m_model.velocityLimit;
+ assert(m_model);
+
+ res.resize(m_model->nv);
+ res = m_model->velocityLimit;
+
sotDEBUG(15) << "upperVelocityLimit (" << res << ")=" <<std::endl;
sotDEBUGOUT(15);
return res;
@@ -773,30 +266,15 @@ dg::Vector& Dynamic::
getMaxEffortLimits(dg::Vector& res, const int&)
{
sotDEBUGIN(15);
- res.resize(m_model.nv);
- res = m_model.effortLimit;
- sotDEBUGOUT(15);
- return res;
-}
+ assert(m_model);
+ res.resize(m_model->nv);
+ res = m_model->effortLimit;
-//TODO: To be set via srdf file
-/*dynamicgraph::Vector Dynamic::getAnklePositionInFootFrame() const
-{
- if (!m_data) {
- SOT_THROW ExceptionDynamic(ExceptionDynamic::GENERIC,
- "you must create a robot first.");
- }
-
- dg::Vector anklePosition(3);
- foot->getAnklePositionInLocalFrame(anklePosition);
- dynamicgraph::Vector res(3);
- res(0) = anklePosition[0];
- res(1) = anklePosition[1];
- res(2) = anklePosition[2];
+ sotDEBUGOUT(15);
return res;
}
-*/
+
/* ---------------- INTERNAL ------------------------------------------------ */
dg::Vector Dynamic::getPinocchioPos(int time)
@@ -804,7 +282,8 @@ dg::Vector Dynamic::getPinocchioPos(int time)
sotDEBUGIN(15);
dg::Vector qJoints=jointPositionSIN.access(time);
dg::Vector q;
-
+ assert(m_model);
+
if( freeFlyerPositionSIN) {
dg::Vector qFF=freeFlyerPositionSIN.access(time);
q.resize(qJoints.size() + 7);
@@ -814,7 +293,7 @@ dg::Vector Dynamic::getPinocchioPos(int time)
rot.getQuaternion(x,y,z,w);
q << qFF(0),qFF(1),qFF(2),x,y,z,w,qJoints;
}
- else if (se3::nv(m_model.joints[1]) == 6){
+ else if (se3::nv(m_model->joints[1]) == 6){
dg::Vector qFF = qJoints.head<6>();
urdf::Rotation rot;
rot.setFromRPY(qFF(3),qFF(4),qFF(5));
@@ -822,12 +301,13 @@ dg::Vector Dynamic::getPinocchioPos(int time)
rot.getQuaternion(x,y,z,w);
q.resize(qJoints.size()+1);
q << qFF(0),qFF(1),qFF(2),x,y,z,w,qJoints.segment(6,qJoints.size()-6);
- assert(q.size() == m_model.nq);
+ assert(q.size() == m_model->nq);
}
else {
q.resize(qJoints.size());
q=qJoints;
}
+
sotDEBUGOUT(15) <<"Position out"<<q<<std::endl;
return q;
}
@@ -841,7 +321,8 @@ Eigen::VectorXd Dynamic::getPinocchioVel(int time)
v << vFF,vJoints;
return v;
}
- else return vJoints;
+ else
+ return vJoints;
}
Eigen::VectorXd Dynamic::getPinocchioAcc(int time)
@@ -861,16 +342,17 @@ dg::SignalTimeDependent< dg::Matrix,int > & Dynamic::
createJacobianSignal( const std::string& signame, const std::string& jointName )
{
sotDEBUGIN(15);
+ assert(m_model);
dg::SignalTimeDependent< dg::Matrix,int > * sig;
- if(m_model.existFrame(jointName)) {
- int frameId = m_model.getFrameId(jointName);
+ if(m_model->existFrame(jointName)) {
+ int frameId = m_model->getFrameId(jointName);
sig = new dg::SignalTimeDependent< dg::Matrix,int >
( boost::bind(&Dynamic::computeGenericJacobian,this,true,frameId,_1,_2),
newtonEulerSINTERN,
"sotDynamic("+name+")::output(matrix)::"+signame );
}
- else if(m_model.existJointName(jointName)) {
- int jointId = m_model.getJointId(jointName);
+ else if(m_model->existJointName(jointName)) {
+ int jointId = m_model->getJointId(jointName);
sig = new dg::SignalTimeDependent< dg::Matrix,int >
( boost::bind(&Dynamic::computeGenericJacobian,this,false,jointId,_1,_2),
newtonEulerSINTERN,
@@ -889,16 +371,17 @@ dg::SignalTimeDependent< dg::Matrix,int > & Dynamic::
createEndeffJacobianSignal( const std::string& signame, const std::string& jointName )
{
sotDEBUGIN(15);
+ assert(m_model);
dg::SignalTimeDependent< dg::Matrix,int > * sig;
- if(m_model.existFrame(jointName)) {
- int frameId = m_model.getFrameId(jointName);
+ if(m_model->existFrame(jointName)) {
+ int frameId = m_model->getFrameId(jointName);
sig = new dg::SignalTimeDependent< dg::Matrix,int >
( boost::bind(&Dynamic::computeGenericEndeffJacobian,this,true,frameId,_1,_2),
newtonEulerSINTERN,
"sotDynamic("+name+")::output(matrix)::"+signame );
}
- else if(m_model.existJointName(jointName)) {
- int jointId = m_model.getJointId(jointName);
+ else if(m_model->existJointName(jointName)) {
+ int jointId = m_model->getJointId(jointName);
sig = new dg::SignalTimeDependent< dg::Matrix,int >
( boost::bind(&Dynamic::computeGenericEndeffJacobian,this,false,jointId,_1,_2),
newtonEulerSINTERN,
@@ -916,16 +399,17 @@ dg::SignalTimeDependent< MatrixHomogeneous,int >& Dynamic::
createPositionSignal( const std::string& signame, const std::string& jointName)
{
sotDEBUGIN(15);
+ assert(m_model);
dg::SignalTimeDependent< MatrixHomogeneous,int > * sig;
- if(m_model.existFrame(jointName)) {
- int frameId = m_model.getFrameId(jointName);
+ if(m_model->existFrame(jointName)) {
+ int frameId = m_model->getFrameId(jointName);
sig = new dg::SignalTimeDependent< MatrixHomogeneous,int >
( boost::bind(&Dynamic::computeGenericPosition,this,true,frameId,_1,_2),
newtonEulerSINTERN,
"sotDynamic("+name+")::output(matrixHomo)::"+signame );
}
- else if(m_model.existJointName(jointName)) {
- int jointId = m_model.getJointId(jointName);
+ else if(m_model->existJointName(jointName)) {
+ int jointId = m_model->getJointId(jointName);
sig = new dg::SignalTimeDependent< MatrixHomogeneous,int >
( boost::bind(&Dynamic::computeGenericPosition,this,false,jointId,_1,_2),
newtonEulerSINTERN,
@@ -944,7 +428,8 @@ SignalTimeDependent< dg::Vector,int >& Dynamic::
createVelocitySignal( const std::string& signame,const std::string& jointName )
{
sotDEBUGIN(15);
- int jointId = m_model.getJointId(jointName);
+ assert(m_model);
+ int jointId = m_model->getJointId(jointName);
SignalTimeDependent< dg::Vector,int > * sig
= new SignalTimeDependent< dg::Vector,int >
@@ -962,7 +447,8 @@ dg::SignalTimeDependent< dg::Vector,int >& Dynamic::
createAccelerationSignal( const std::string& signame, const std::string& jointName)
{
sotDEBUGIN(15);
- int jointId = m_model.getJointId(jointName);
+ assert(m_model);
+ int jointId = m_model->getJointId(jointName);
dg::SignalTimeDependent< dg::Vector,int > * sig
= new dg::SignalTimeDependent< dg::Vector,int >
( boost::bind(&Dynamic::computeGenericAcceleration,this,jointId,_1,_2),
@@ -983,6 +469,7 @@ void Dynamic::
destroyJacobianSignal( const std::string& signame )
{
sotDEBUGIN(15);
+
bool deletable = false;
dg::SignalTimeDependent< dg::Matrix,int > * sig = & jacobiansSOUT( signame );
for(std::list< SignalBase<int>* >::iterator iter = genericSignalRefs.begin();
@@ -1090,9 +577,11 @@ dg::Vector& Dynamic::computeZmp( dg::Vector& res,int time )
{
//TODO: To be verified
sotDEBUGIN(25);
+ assert(m_data);
if (res.size()!=3)
res.resize(3);
newtonEulerSINTERN(time);
+
se3::Force ftau = m_data->oMi[1].act(m_data->f[1]);
se3::Force::Vector3 tau = ftau.angular();
se3::Force::Vector3 f = ftau.linear();
@@ -1110,17 +599,19 @@ dg::Matrix& Dynamic::
computeGenericJacobian(bool isFrame, int jointId, dg::Matrix& res,int time )
{
sotDEBUGIN(25);
+ assert(m_model);
+ assert(m_data);
newtonEulerSINTERN(time);
- res.resize(6,m_model.nv);
- se3::computeJacobians(m_model,*m_data,this->getPinocchioPos(time));
+ res.resize(6,m_model->nv);
+ se3::computeJacobians(*m_model,*m_data,this->getPinocchioPos(time));
- se3::Data::Matrix6x m_output = Eigen::MatrixXd::Zero(6,m_model.nv);
+ se3::Data::Matrix6x m_output = Eigen::MatrixXd::Zero(6,m_model->nv);
//Computes Jacobian in world coordinates.
if(isFrame){
- se3::framesForwardKinematics(m_model,*m_data);
- se3::getFrameJacobian<false>(m_model,*m_data,(se3::Model::Index)jointId,m_output);
+ se3::framesForwardKinematics(*m_model,*m_data);
+ se3::getFrameJacobian<false>(*m_model,*m_data,(se3::Model::Index)jointId,m_output);
}
- else se3::getJacobian<false>(m_model,*m_data,(se3::Model::Index)jointId,m_output);
+ else se3::getJacobian<false>(*m_model,*m_data,(se3::Model::Index)jointId,m_output);
res = m_output;
sotDEBUGOUT(25);
return res;
@@ -1130,17 +621,19 @@ dg::Matrix& Dynamic::
computeGenericEndeffJacobian(bool isFrame, int jointId,dg::Matrix& res,int time )
{
sotDEBUGIN(25);
+ assert(m_model);
+ assert(m_data);
newtonEulerSINTERN(time);
- res.resize(6,m_model.nv);
+ res.resize(6,m_model->nv);
//In local coordinates.
- se3::computeJacobians(m_model,*m_data,this->getPinocchioPos(time));
- se3::Data::Matrix6x m_output = Eigen::MatrixXd::Zero(6,m_model.nv);
+ se3::computeJacobians(*m_model,*m_data,this->getPinocchioPos(time));
+ se3::Data::Matrix6x m_output = Eigen::MatrixXd::Zero(6,m_model->nv);
if(isFrame){
- se3::framesForwardKinematics(m_model,*m_data);
- se3::getFrameJacobian<true>(m_model,*m_data,(se3::Model::Index)jointId,m_output);
+ se3::framesForwardKinematics(*m_model,*m_data);
+ se3::getFrameJacobian<true>(*m_model,*m_data,(se3::Model::Index)jointId,m_output);
}
- else se3::getJacobian<true>(m_model,*m_data,(se3::Model::Index)jointId,m_output);
+ else se3::getJacobian<true>(*m_model,*m_data,(se3::Model::Index)jointId,m_output);
res = m_output;
sotDEBUGOUT(25);
@@ -1152,12 +645,12 @@ MatrixHomogeneous& Dynamic::
computeGenericPosition(bool isFrame, int jointId, MatrixHomogeneous& res, int time)
{
sotDEBUGIN(25);
+ assert(m_data);
newtonEulerSINTERN(time);
-
if(isFrame){
//TODO: Confirm if we need this. Already being called when calculating jacobian
//se3::framesForwardKinematics(m_model,*m_data);
- res.matrix()= m_data->oMof[jointId].toHomogeneousMatrix();
+ res.matrix()= m_data->oMi[jointId].toHomogeneousMatrix();
}
else res.matrix()= m_data->oMi[jointId].toHomogeneousMatrix();
@@ -1169,6 +662,7 @@ dg::Vector& Dynamic::
computeGenericVelocity( int jointId, dg::Vector& res,int time )
{
sotDEBUGIN(25);
+ assert(m_data);
newtonEulerSINTERN(time);
res.resize(6);
se3::Motion aRV = m_data->v[jointId];
@@ -1181,6 +675,7 @@ dg::Vector& Dynamic::
computeGenericAcceleration( int jointId ,dg::Vector& res,int time )
{
sotDEBUGIN(25);
+ assert(m_data);
newtonEulerSINTERN(time);
res.resize(6);
se3::Motion aRA = m_data->a[jointId];
@@ -1193,11 +688,13 @@ int& Dynamic::
computeNewtonEuler( int& dummy,int time )
{
sotDEBUGIN(15);
-
+ assert(m_model);
+ assert(m_data);
+
const Eigen::VectorXd q=getPinocchioPos(time);
const Eigen::VectorXd v=getPinocchioVel(time);
const Eigen::VectorXd a=getPinocchioAcc(time);
- se3::rnea(m_model,*m_data,q,v,a);
+ se3::rnea(*m_model,*m_data,q,v,a);
sotDEBUG(1)<< "pos = " <<q <<std::endl;
sotDEBUG(1)<< "vel = " <<v <<std::endl;
@@ -1215,7 +712,7 @@ computeJcom( dg::Matrix& Jcom,int time )
newtonEulerSINTERN(time);
const Eigen::VectorXd q=getPinocchioPos(time);
- Jcom = se3::jacobianCenterOfMass(m_model, *m_data,
+ Jcom = se3::jacobianCenterOfMass(*m_model, *m_data,
q, false);
sotDEBUGOUT(25);
return Jcom;
@@ -1228,7 +725,7 @@ computeCom( dg::Vector& com,int time )
sotDEBUGIN(25);
newtonEulerSINTERN(time);
const Eigen::VectorXd q=getPinocchioPos(time);
- com = se3::centerOfMass(m_model,*m_data,q,false,true);
+ com = se3::centerOfMass(*m_model,*m_data,q,false,true);
sotDEBUGOUT(25);
return com;
}
@@ -1239,8 +736,8 @@ computeInertia( dg::Matrix& res,int time )
sotDEBUGIN(25);
newtonEulerSINTERN(time);
//TODO: USE CCRBA
- dg::Matrix upperInertiaMatrix = se3::crba(m_model,
- *m_data,this->getPinocchioPos(time));
+ dg::Matrix upperInertiaMatrix = se3::crba(*m_model,
+ *m_data,this->getPinocchioPos(time));
res = upperInertiaMatrix;
res.triangularView<Eigen::StrictlyLower>() = upperInertiaMatrix.transpose().triangularView<Eigen::StrictlyLower>();
sotDEBUGOUT(25);
@@ -1271,11 +768,11 @@ computeFootHeight (double &res , int time)
//TODO: Confirm that it is in the foot frame
sotDEBUGIN(25);
newtonEulerSINTERN(time);
- if(!m_model.existJointName("r_sole_joint")) {
+ if(!m_model->existJointName("r_sole_joint")) {
SOT_THROW ExceptionDynamic(ExceptionDynamic::GENERIC,
"Robot has no joint corresponding to rigthFoot");
}
- int jointId = m_model.getJointId("r_sole_joint");
+ int jointId = m_model->getJointId("r_sole_joint");
Eigen::Vector3d anklePosInLocalRefFrame= m_data->liMi[jointId].translation();
// TODO: positive or negative? Current output:negative
res = anklePosInLocalRefFrame(2);
@@ -1399,20 +896,9 @@ commandLine( const std::string& cmdLine,
{
sotDEBUG(25) << "# In { Cmd " << cmdLine <<std::endl;
std::string filename;
- if( cmdLine == "setFile" ) {
- cmdArgs>>filename; setUrdfFile(filename);
- }
if( cmdLine == "displayModel" ) {
displayModel();
}
- else if( cmdLine == "parse" ) {
- if(!init) parseUrdfFile();
- else std::cout << " !! Already parsed." <<std::endl;
- }
- else if( cmdLine == "displayFile" ) {
- cmdArgs >> std::ws;
- os << m_urdfPath << std::endl;
- }
else if( cmdLine == "createJacobian" ) {
std::string signame; cmdArgs >> signame;
std::string jointName; cmdArgs >> jointName;
@@ -1472,10 +958,7 @@ commandLine( const std::string& cmdLine,
}
else if( cmdLine == "help" ) {
os << "Dynamics:"<<std::endl
- << " - setFile <%1>\t:set files in the order cited above" <<std::endl
<< " - displayModel\t:display the current model configuration" <<std::endl
- << " - displayFile\t\t\t:display the urdf config file" <<std::endl
- << " - parse\t\t\t:parse the files set unsing the set{Xml|Vrml} commands." <<std::endl
<< " - createJacobian <name> <point>:create a signal named <name> " << std::endl
<< " - destroyJacobian <name>\t:delete the jacobian signal <name>" << std::endl
<< " - createEndeffJacobian <name> <point>:create a signal named <name> "
diff --git a/unitTesting/kineromeo.py b/unitTesting/kineromeo.py
index d4345b0..9e6304f 100644
--- a/unitTesting/kineromeo.py
+++ b/unitTesting/kineromeo.py
@@ -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
diff --git a/unitTesting/test_config.cpp b/unitTesting/test_config.cpp
index 09c212a..6aea603 100644
--- a/unitTesting/test_config.cpp
+++ b/unitTesting/test_config.cpp
@@ -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;
diff --git a/unitTesting/test_constructor.cpp b/unitTesting/test_constructor.cpp
index 90130bb..cf32b5d 100644
--- a/unitTesting/test_constructor.cpp
+++ b/unitTesting/test_constructor.cpp
@@ -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;
--
GitLab