From 313146af05760662128138390dc90af0c1ff7156 Mon Sep 17 00:00:00 2001
From: Rohan Budhiraja <budhiraja@laas.fr>
Date: Fri, 8 Apr 2016 20:34:34 +0200
Subject: [PATCH] [cpp] change joint-limits to model class + temporary fix for
specificities
---
include/sot-dynamic/dynamic.h | 40 ++-
src/dynamic-command.h | 23 ++
.../sot/dynamics/humanoid_robot.py | 118 ++++---
src/sot-dynamic.cpp | 322 +++++++++++-------
unitTesting/test_config.cpp | 2 +
unitTesting/two_link.urdf | 47 +--
6 files changed, 314 insertions(+), 238 deletions(-)
diff --git a/include/sot-dynamic/dynamic.h b/include/sot-dynamic/dynamic.h
index 85e9b53..eeb247f 100644
--- a/include/sot-dynamic/dynamic.h
+++ b/include/sot-dynamic/dynamic.h
@@ -49,6 +49,7 @@
#include <pinocchio/multibody/parser/urdf.hpp>
#include <pinocchio/algorithm/rnea.hpp>
#include <pinocchio/algorithm/jacobian.hpp>
+#include <pinocchio/algorithm/operational-frames.hpp>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
@@ -173,24 +174,23 @@ class SOTDYNAMIC_EXPORT Dynamic
/* --- MODEL CREATION --- */
- /// \brief parse a urdf file to create robot model
+ /// \brief sets a urdf filepath to create robot model. Call parseUrdfFile to parse
/// \param path file location.
///
- /// \note creates a pinocchio model. needs urdfdom libraries to parse.
void setUrdfFile( const std::string& path );
- //TODO:
- // void parseAndAddFrames(se3::Model& pinocchio_model,
- // const std::string& filename);
-
- /// \name Construction of a robot by commands
- ///@{
+ /// \brief 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(){ std::cout<<m_model<<std::endl; };
+ void displayModel() const
+ { std::cout<<m_model<<std::endl; };
/// \brief create a joint
/// \param inJointName name of the joint,
@@ -336,11 +336,19 @@ class SOTDYNAMIC_EXPORT Dynamic
/// \return result vector
dg::Vector& getMaxEffortLimits(dg::Vector& res,const int&);
-
- protected:
- dg::Matrix& computeGenericJacobian(int jointId,dg::Matrix& res,int time );
- dg::Matrix& computeGenericEndeffJacobian(int jointId,dg::Matrix& res,int time );
- MatrixHomogeneous& computeGenericPosition(int jointId,MatrixHomogeneous& res,int time );
+
+ // dg::Vector& getAnklePositionInFootFrame() const;
+
+ protected:
+ dg::Matrix& computeGenericJacobian(bool isFrame,
+ int jointId,
+ dg::Matrix& res,int time );
+ dg::Matrix& computeGenericEndeffJacobian(bool isFrame,
+ int jointId,
+ dg::Matrix& res,int time );
+ MatrixHomogeneous& computeGenericPosition(bool isFrame,
+ int jointId,
+ MatrixHomogeneous& res,int time );
dg::Vector& computeGenericVelocity(int jointId,dg::Vector& res,int time );
dg::Vector& computeGenericAcceleration(int jointId,dg::Vector& res,int time );
@@ -371,12 +379,12 @@ class SOTDYNAMIC_EXPORT Dynamic
dg::Vector getPinocchioVel(int);
dg::Vector getPinocchioAcc(int);
- typedef std::pair<std::string,dg::Matrix> JointDetails;
+ 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::map<std::string,std::string> specificitiesMap;
};
diff --git a/src/dynamic-command.h b/src/dynamic-command.h
index bd4b4fe..c8668dd 100644
--- a/src/dynamic-command.h
+++ b/src/dynamic-command.h
@@ -55,6 +55,29 @@ namespace dynamicgraph { namespace sot {
}
}; // 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
{
diff --git a/src/dynamic_graph/sot/dynamics/humanoid_robot.py b/src/dynamic_graph/sot/dynamics/humanoid_robot.py
index 7a56201..a565072 100755
--- a/src/dynamic_graph/sot/dynamics/humanoid_robot.py
+++ b/src/dynamic_graph/sot/dynamics/humanoid_robot.py
@@ -47,6 +47,10 @@ class AbstractHumanoidRobot (object):
- rightAnkle,
- Gaze.
+ Operational points are mapped to the actual joints in the robot model
+ via 'OperationalPointsMap' dictionary.
+ This attribute *must* be defined in the subclasses
+
Other attributes require to be defined:
- halfSitting: half-sitting position is the robot initial pose.
This attribute *must* be defined in subclasses.
@@ -65,6 +69,8 @@ class AbstractHumanoidRobot (object):
OperationalPoints = ['left-wrist', 'right-wrist',
'left-ankle', 'right-ankle',
'gaze']
+
+
"""
Operational points are specific interesting points of the robot
used to control it.
@@ -91,7 +97,6 @@ class AbstractHumanoidRobot (object):
"""
-
frames = dict()
"""
Additional frames defined by using OpPointModifier.
@@ -163,51 +168,48 @@ class AbstractHumanoidRobot (object):
self.setProperties(model)
return model
- def loadModelFromJrlDynamics(self, name, modelDir, modelName,
- specificitiesPath, jointRankPath,
- dynamicType):
+ def loadModelFromUrdf(self, name, urdfPath,
+ dynamicType):
"""
- Load a model using the jrl-dynamics parser. This parser looks
- for VRML files in which kinematics and dynamics information
- have been added by extending the VRML format.
-
- It is mainly used by OpenHRP.
+ Load a model using the pinocchio urdf parser. This parser looks
+ for urdf files in which kinematics and dynamics information
+ have been added.
Additional information are located in two different XML files.
"""
model = dynamicType(name)
- self.setProperties(model)
- model.setFiles(modelDir, modelName,
- specificitiesPath, jointRankPath)
+ #TODO: setproperty flags in sot-pinocchio
+ #self.setProperties(model)
+ model.setFile(urdfPath)
model.parse()
return model
- def setProperties(self, model):
- model.setProperty('TimeStep', str(self.timeStep))
-
- model.setProperty('ComputeAcceleration', 'false')
- model.setProperty('ComputeAccelerationCoM', 'false')
- model.setProperty('ComputeBackwardDynamics', 'false')
- model.setProperty('ComputeCoM', 'false')
- model.setProperty('ComputeMomentum', 'false')
- model.setProperty('ComputeSkewCom', 'false')
- model.setProperty('ComputeVelocity', 'false')
- model.setProperty('ComputeZMP', 'false')
-
- model.setProperty('ComputeAccelerationCoM', 'true')
- model.setProperty('ComputeCoM', 'true')
- model.setProperty('ComputeVelocity', 'true')
- model.setProperty('ComputeZMP', 'true')
-
- if self.enableZmpComputation:
- model.setProperty('ComputeBackwardDynamics', 'true')
- model.setProperty('ComputeAcceleration', 'true')
- model.setProperty('ComputeMomentum', 'true')
+ #TODO: put these flags in sot-pinocchio
+ #def setProperties(self, model):
+ # model.setProperty('TimeStep', str(self.timeStep))
+ #
+ # model.setProperty('ComputeAcceleration', 'false')
+ # model.setProperty('ComputeAccelerationCoM', 'false')
+ # model.setProperty('ComputeBackwardDynamics', 'false')
+ # model.setProperty('ComputeCoM', 'false')
+ # model.setProperty('ComputeMomentum', 'false')
+ # model.setProperty('ComputeSkewCom', 'false')
+ # model.setProperty('ComputeVelocity', 'false')
+ # model.setProperty('ComputeZMP', 'false')
+ # model.setProperty('ComputeAccelerationCoM', 'true')
+ # model.setProperty('ComputeCoM', 'true')
+ # model.setProperty('ComputeVelocity', 'true')
+ # model.setProperty('ComputeZMP', 'true')
+ #
+ # if self.enableZmpComputation:
+ # model.setProperty('ComputeBackwardDynamics', 'true')
+ # model.setProperty('ComputeAcceleration', 'true')
+ # model.setProperty('ComputeMomentum', 'true')
def initializeOpPoints(self, model):
for op in self.OperationalPoints:
- model.createOpPoint(op, op)
+ model.createOpPoint(self.OperationalPointsMap[op], self.OperationalPointsMap[op])
def createFrame(self, frameName, transformation, operationalPoint):
frame = OpPointModifier(frameName)
@@ -263,27 +265,27 @@ class AbstractHumanoidRobot (object):
self.initializeOpPoints(self.dynamic)
- # --- additional frames ---
- self.frames = dict()
- frameName = 'rightHand'
- self.frames [frameName] = self.createFrame (
- "{0}_{1}".format (self.name, frameName),
- self.dynamic.getHandParameter (True), "right-wrist")
+ #TODO: hand parameters through srdf --- additional frames ---
+ #self.frames = dict()
+ #frameName = 'rightHand'
+ #self.frames [frameName] = self.createFrame (
+ # "{0}_{1}".format (self.name, frameName),
+ # self.dynamic.getHandParameter (True), "right-wrist")
# rightGripper is an alias for the rightHand:
- self.frames ['rightGripper'] = self.frames [frameName]
+ #self.frames ['rightGripper'] = self.frames [frameName]
- frameName = 'leftHand'
- self.frames [frameName] = self.createFrame (
- "{0}_{1}".format (self.name, frameName),
- self.dynamic.getHandParameter (False), "left-wrist")
+ #frameName = 'leftHand'
+ #self.frames [frameName] = self.createFrame (
+ # "{0}_{1}".format (self.name, frameName),
+ # self.dynamic.getHandParameter (False), "left-wrist")
# leftGripper is an alias for the leftHand:
- self.frames ["leftGripper"] = self.frames [frameName]
+ #self.frames ["leftGripper"] = self.frames [frameName]
- for (frameName, transformation, signalName) in self.AdditionalFrames:
- self.frames[frameName] = self.createFrame(
- "{0}_{1}".format(self.name, frameName),
- transformation,
- signalName)
+ #for (frameName, transformation, signalName) in self.AdditionalFrames:
+ # self.frames[frameName] = self.createFrame(
+ # "{0}_{1}".format(self.name, frameName),
+ # transformation,
+ # signalName)
def addTrace(self, entityName, signalName):
if self.tracer:
@@ -371,21 +373,25 @@ class AbstractHumanoidRobot (object):
self.dynamic.Jcom.recompute(self.device.state.time+1)
for op in self.OperationalPoints:
- self.dynamic.signal(op).recompute(self.device.state.time+1)
- self.dynamic.signal('J'+op).recompute(self.device.state.time+1)
+ self.dynamic.signal(self.OperationalPointsMap[op]).recompute(self.device.state.time+1)
+ self.dynamic.signal('J'+self.OperationalPointsMap[op]).recompute(self.device.state.time+1)
class HumanoidRobot(AbstractHumanoidRobot):
halfSitting = [] #FIXME
-
+
name = None
filename = None
def __init__(self, name, filename, tracer = None):
AbstractHumanoidRobot.__init__(self, name, tracer)
self.filename = filename
- self.dynamic = \
- self.loadModelFromKxml (self.name + '_dynamics', self.filename)
+ self.OperationalPointsMap ={'left-wrist' : 'left-wrist',
+ 'right-wrist' : 'right-wrist',
+ 'left-ankle' : 'left-ankle',
+ 'right-ankle' : 'right-ankle',
+ 'gaze' : 'gaze'}
+ self.dynamic = self.loadModelFromKxml (self.name + '_dynamics', self.filename)
self.dimension = self.dynamic.getDimension()
self.halfSitting = self.dimension*(0.,)
self.initializeRobot()
diff --git a/src/sot-dynamic.cpp b/src/sot-dynamic.cpp
index e1bfd9a..c6fc097 100644
--- a/src/sot-dynamic.cpp
+++ b/src/sot-dynamic.cpp
@@ -17,7 +17,6 @@
* have received a copy of the GNU Lesser General Public License along
* with sot-dynamic. If not, see <http://www.gnu.org/licenses/>.
*/
-#include <limits>
#include <sot/core/debug.hh>
#include <sot-dynamic/dynamic.h>
@@ -27,7 +26,6 @@
#include <boost/format.hpp>
#include <pinocchio/algorithm/kinematics.hpp>
-#include <pinocchio/algorithm/joint-limits.hpp>
#include <pinocchio/algorithm/center-of-mass.hpp>
#include <pinocchio/spatial/motion.hpp>
#include <pinocchio/algorithm/crba.hpp>
@@ -47,7 +45,7 @@ Dynamic( const std::string & name)
:Entity(name)
,m_model()
,m_data(NULL)
- ,m_urdfPath()
+ ,m_urdfPath("")
,init(false)
@@ -128,18 +126,18 @@ Dynamic( const std::string & name)
jointTypes.push_back("JointModelTranslation");
- //TODO: wrist and ankle have different roll-pitch-yaw frames. Confirm which?
+
+ //TODO: gaze_joint is fixed joint: not one of pinocchio joint types. Confirm specificities joint.
/*
- specificitiesMap["right-wrist"] = "RWristRoll";
- specificitiesMap["left-wrist"] = "RWristRoll";
- specificitiesMap["right-ankle"] = "RWristRoll";
- specificitiesMap["left-ankle"] = "RWristRoll";
- specificitiesMap["gaze"] = "";
- specificitiesMap["waist"] = "";
- specificitiesMap["chest"] = "";
+ 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();
@@ -184,6 +182,16 @@ Dynamic( const std::string & name)
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"
" Display the current robot configuration.\n"
@@ -243,7 +251,7 @@ Dynamic( const std::string & name)
addCommand("createJoint", new command::CreateJoint(*this, docstring));
docstring = " \n"
- " Add a child joint\n"
+ " Add a child Body\n"
" \n"
" Input:\n"
" - a string: name of the parent body,\n"
@@ -273,15 +281,25 @@ Dynamic( const std::string & name)
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"
+ " 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"
@@ -384,26 +402,35 @@ Dynamic::~Dynamic( void ) {
//Import from urdf file
void Dynamic::setUrdfFile(const std::string& filename) {
sotDEBUGIN(15);
- // m_model = new se3::Model();
- m_model = se3::urdf::buildModel(filename);
- this->m_urdfPath = filename;
+ 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);
- se3::jointLimits(m_model, *m_data, Eigen::VectorXd::Zero(m_model.nq));
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);
- // if (m_model) {
- //m_model.~Model();
- delete m_data;
- //}
+ delete m_data;
m_model= se3::Model();
m_data = new se3::Data(m_model);
+ init=true;
sotDEBUGOUT(15);
}
@@ -422,7 +449,8 @@ void Dynamic::createJoint(const std::string& inJointName,
" already exists in the model.");
if(std::find(jointTypes.begin(),jointTypes.end(),inJointType) != jointTypes.end()) {
- JointDetails jointDetails(inJointType,inPosition);
+ Eigen::Matrix4d inposition_ = inPosition;
+ JointDetails jointDetails(inJointType,inposition_);
jointMap_[inJointName] = jointDetails;
}
else {
@@ -444,7 +472,6 @@ void Dynamic::addBody(const std::string& inParentName,
sotDEBUGOUT(15);
}
-
void Dynamic::addBody(const std::string& inParentName,
const std::string& inJointName,
const std::string& inChildName,
@@ -512,7 +539,6 @@ void Dynamic::addBody(const std::string& inParentName,
if(m_data) delete m_data;
m_data = new se3::Data(m_model);
- se3::jointLimits(m_model, *m_data, Eigen::VectorXd::Zero(m_model.nq));
sotDEBUGOUT(15);
}
@@ -580,11 +606,12 @@ void Dynamic::setDofBounds(const std::string& inJointName,
"No joint with name " + inJointName +
" has been created.");
se3::Model::Index joint_index = m_model.getJointId(inJointName);
- //TODO: Only change in data->vector. No change in the joint model class. Potential bug
int prev_cumulative_nq = se3::idx_q(m_model.joints[joint_index]);
+
assert(se3::nq(m_model.joints[joint_index]) > inDofId);
- m_data->lowerPositionLimit(prev_cumulative_nq+inDofId) = inMinValue;
- m_data->upperPositionLimit(prev_cumulative_nq+inDofId) = inMaxValue;
+
+ m_model.lowerPositionLimit(prev_cumulative_nq+inDofId) = inMinValue;
+ m_model.upperPositionLimit(prev_cumulative_nq+inDofId) = inMaxValue;
sotDEBUGOUT(15);
return;
}
@@ -598,9 +625,8 @@ void Dynamic::setLowerPositionLimit(const std::string& inJointName,
se3::Model::Index joint_index = m_model.getJointId(inJointName);
int prev_cumulative_nq = se3::idx_q(m_model.joints[joint_index]);
- //TODO: Only change in data->vector. No change in the joint model class. Potential bug
assert (se3::nq(m_model.joints[joint_index])==1 && "Joint is not revolute");
- m_data->lowerPositionLimit(prev_cumulative_nq) = lowPos;
+ m_model.lowerPositionLimit(prev_cumulative_nq) = lowPos;
return;
}
@@ -613,9 +639,8 @@ void Dynamic::setLowerPositionLimit(const std::string& inJointName,
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]);
- //TODO: Only change in data->vector. No change in the joint model class. Potential bug
assert (lowPos.size()==current_nq);
- m_data->lowerPositionLimit.segment(prev_cumulative_nq,current_nq) = lowPos;
+ m_model.lowerPositionLimit.segment(prev_cumulative_nq,current_nq) = lowPos;
return;
}
@@ -628,9 +653,8 @@ void Dynamic::setUpperPositionLimit(const std::string& 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]);
- //TODO: Only change in data->vector. No change in the joint model class. Potential bug
assert (se3::nq(m_model.joints[joint_index])==1 && "Joint is not revolute");
- m_data->upperPositionLimit(prev_cumulative_nq) = upPos;
+ m_model.upperPositionLimit(prev_cumulative_nq) = upPos;
return;
}
@@ -645,8 +669,7 @@ void Dynamic::setUpperPositionLimit(const std::string& inJointName,
int current_nq = se3::nq(m_model.joints[joint_index]);
assert (upPos.size()==current_nq);
- //TODO: Only change in data->vector. No change in the joint model class. Potential bug
- m_data->upperPositionLimit.segment(prev_cumulative_nq,current_nq) = upPos;
+ m_model.upperPositionLimit.segment(prev_cumulative_nq,current_nq) = upPos;
return;
}
@@ -660,8 +683,7 @@ void Dynamic::setMaxVelocityLimit(const std::string& 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");
- //TODO: Only change in data->vector. No change in the joint model class. Potential bug
- m_data->velocityLimit(prev_cumulative_nv) = maxVel;
+ m_model.velocityLimit(prev_cumulative_nv) = maxVel;
return;
}
@@ -674,9 +696,8 @@ void Dynamic::setMaxVelocityLimit(const std::string& inJointName,
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]);
- //TODO: Only change in data->vector. No change in the joint model class. Potential bug
assert (maxVel.size()==current_nv);
- m_data->velocityLimit.segment(prev_cumulative_nv,current_nv) = maxVel;
+ m_model.velocityLimit.segment(prev_cumulative_nv,current_nv) = maxVel;
return;
}
@@ -690,9 +711,8 @@ void Dynamic::setMaxEffortLimit(const std::string& 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]);
- //TODO: Only change in data->vector. No change in the joint model class. Potential bug
assert (se3::nv(m_model.joints[joint_index])==1 && "Joint is not revolute");
- m_data->effortLimit(prev_cumulative_nv) = maxEffort;
+ m_model.effortLimit(prev_cumulative_nv) = maxEffort;
return;
}
@@ -706,9 +726,8 @@ void Dynamic::setMaxEffortLimit(const std::string& inJointName,
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]);
- //TODO: Only change in data->vector. No change in the joint model class. Potential bug
assert (maxEffort.size()==current_nv);
- m_data->effortLimit.segment(prev_cumulative_nv,current_nv) = maxEffort;
+ m_model.effortLimit.segment(prev_cumulative_nv,current_nv) = maxEffort;
return;
}
@@ -721,7 +740,7 @@ getLowerPositionLimits(dg::Vector& res, const int&)
sotDEBUGIN(15);
res.resize(m_model.nq);
- res = m_data->lowerPositionLimit;
+ res = m_model.lowerPositionLimit;
sotDEBUG(15) << "lowerLimit (" << res << ")=" << std::endl;
sotDEBUGOUT(15);
return res;
@@ -732,7 +751,7 @@ getUpperPositionLimits(dg::Vector& res, const int&)
{
sotDEBUGIN(15);
res.resize(m_model.nq);
- res = m_data->upperPositionLimit;
+ res = m_model.upperPositionLimit;
sotDEBUG(15) << "upperLimit (" << res << ")=" <<std::endl;
sotDEBUGOUT(15);
return res;
@@ -743,7 +762,7 @@ getUpperVelocityLimits(dg::Vector& res, const int&)
{
sotDEBUGIN(15);
res.resize(m_model.nv);
- res = m_data->velocityLimit;
+ res = m_model.velocityLimit;
sotDEBUG(15) << "upperVelocityLimit (" << res << ")=" <<std::endl;
sotDEBUGOUT(15);
return res;
@@ -754,11 +773,30 @@ getMaxEffortLimits(dg::Vector& res, const int&)
{
sotDEBUGIN(15);
res.resize(m_model.nv);
- res = m_data->effortLimit;
+ res = m_model.effortLimit;
sotDEBUGOUT(15);
return res;
}
+
+//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];
+ return res;
+}
+*/
+
/* ---------------- INTERNAL ------------------------------------------------ */
dg::Vector Dynamic::getPinocchioPos(int time)
{
@@ -821,16 +859,28 @@ Eigen::VectorXd Dynamic::getPinocchioAcc(int time)
dg::SignalTimeDependent< dg::Matrix,int > & Dynamic::
createJacobianSignal( const std::string& signame, const std::string& jointName )
{
- int jointId = m_model.getJointId(jointName);
-
- dg::SignalTimeDependent< dg::Matrix,int > * sig
- = new dg::SignalTimeDependent< dg::Matrix,int >
- ( boost::bind(&Dynamic::computeGenericJacobian,this,jointId,_1,_2),
- newtonEulerSINTERN,
- "sotDynamic("+name+")::output(matrix)::"+signame );
+ sotDEBUGIN(15);
+ dg::SignalTimeDependent< dg::Matrix,int > * sig;
+ 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);
+ sig = new dg::SignalTimeDependent< dg::Matrix,int >
+ ( boost::bind(&Dynamic::computeGenericJacobian,this,false,jointId,_1,_2),
+ newtonEulerSINTERN,
+ "sotDynamic("+name+")::output(matrix)::"+signame );
+ }
+ else SOT_THROW ExceptionDynamic(ExceptionDynamic::GENERIC,
+ "Robot has no joint corresponding to " + jointName);
genericSignalRefs.push_back( sig );
signalRegistration( *sig );
+ sotDEBUGOUT(15);
return *sig;
}
@@ -838,16 +888,25 @@ dg::SignalTimeDependent< dg::Matrix,int > & Dynamic::
createEndeffJacobianSignal( const std::string& signame, const std::string& jointName )
{
sotDEBUGIN(15);
- int jointId = m_model.getJointId(jointName);
- dg::SignalTimeDependent< dg::Matrix,int > * sig
- = new dg::SignalTimeDependent< dg::Matrix,int >
- ( boost::bind(&Dynamic::computeGenericEndeffJacobian,this,jointId,_1,_2),
- newtonEulerSINTERN,
- "sotDynamic("+name+")::output(matrix)::"+signame );
-
+ dg::SignalTimeDependent< dg::Matrix,int > * sig;
+ 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);
+ sig = new dg::SignalTimeDependent< dg::Matrix,int >
+ ( boost::bind(&Dynamic::computeGenericEndeffJacobian,this,false,jointId,_1,_2),
+ newtonEulerSINTERN,
+ "sotDynamic("+name+")::output(matrix)::"+signame );
+ }
+ else SOT_THROW ExceptionDynamic(ExceptionDynamic::GENERIC,
+ "Robot has no joint corresponding to " + jointName);
genericSignalRefs.push_back( sig );
signalRegistration( *sig );
-
sotDEBUGOUT(15);
return *sig;
}
@@ -856,15 +915,26 @@ dg::SignalTimeDependent< MatrixHomogeneous,int >& Dynamic::
createPositionSignal( const std::string& signame, const std::string& jointName)
{
sotDEBUGIN(15);
- int jointId = m_model.getJointId(jointName);
- dg::SignalTimeDependent< MatrixHomogeneous,int > * sig
- = new dg::SignalTimeDependent< MatrixHomogeneous,int >
- ( boost::bind(&Dynamic::computeGenericPosition,this,jointId,_1,_2),
- newtonEulerSINTERN,
- "sotDynamic("+name+")::output(matrixHomo)::"+signame );
+ dg::SignalTimeDependent< MatrixHomogeneous,int > * sig;
+ 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);
+ sig = new dg::SignalTimeDependent< MatrixHomogeneous,int >
+ ( boost::bind(&Dynamic::computeGenericPosition,this,false,jointId,_1,_2),
+ newtonEulerSINTERN,
+ "sotDynamic("+name+")::output(matrixHomo)::"+signame );
+ }
+ else SOT_THROW ExceptionDynamic(ExceptionDynamic::GENERIC,
+ "Robot has no joint corresponding to " + jointName);
+
genericSignalRefs.push_back( sig );
signalRegistration( *sig );
-
sotDEBUGOUT(15);
return *sig;
}
@@ -1034,9 +1104,9 @@ dg::Vector& Dynamic::computeZmp( dg::Vector& res,int time )
return res;
}
-//In world frame
+//In world coordinates
dg::Matrix& Dynamic::
-computeGenericJacobian( int jointId,dg::Matrix& res,int time )
+computeGenericJacobian(bool isFrame, int jointId, dg::Matrix& res,int time )
{
sotDEBUGIN(25);
newtonEulerSINTERN(time);
@@ -1044,25 +1114,32 @@ computeGenericJacobian( int jointId,dg::Matrix& res,int time )
se3::computeJacobians(m_model,*m_data,this->getPinocchioPos(time));
se3::Data::Matrix6x m_output = Eigen::MatrixXd::Zero(6,m_model.nv);
- //Computes Jacobian in world frame.
- se3::getJacobian<false>(m_model,*m_data,(se3::Model::Index)jointId,m_output);
-
+ //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);
+ }
+ else se3::getJacobian<false>(m_model,*m_data,(se3::Model::Index)jointId,m_output);
res = m_output;
sotDEBUGOUT(25);
return res;
}
dg::Matrix& Dynamic::
-computeGenericEndeffJacobian( int jointId,dg::Matrix& res,int time )
+computeGenericEndeffJacobian(bool isFrame, int jointId,dg::Matrix& res,int time )
{
sotDEBUGIN(25);
newtonEulerSINTERN(time);
res.resize(6,m_model.nv);
- //In local Frame.
+ //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::getJacobian<true>(m_model,*m_data,(se3::Model::Index)jointId,m_output);
+
+ if(isFrame){
+ 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);
res = m_output;
sotDEBUGOUT(25);
@@ -1071,18 +1148,25 @@ computeGenericEndeffJacobian( int jointId,dg::Matrix& res,int time )
}
MatrixHomogeneous& Dynamic::
-computeGenericPosition( int jointId, MatrixHomogeneous& res, int time)
+computeGenericPosition(bool isFrame, int jointId, MatrixHomogeneous& res, int time)
{
sotDEBUGIN(25);
newtonEulerSINTERN(time);
- res.matrix()= m_data->oMi[jointId].toHomogeneousMatrix();
+
+ 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();
+ }
+ else res.matrix()= m_data->oMi[jointId].toHomogeneousMatrix();
+
+ sotDEBUGOUT(25);
return res;
}
dg::Vector& Dynamic::
computeGenericVelocity( int jointId, dg::Vector& res,int time )
{
-
sotDEBUGIN(25);
newtonEulerSINTERN(time);
res.resize(6);
@@ -1321,7 +1405,7 @@ commandLine( const std::string& cmdLine,
displayModel();
}
else if( cmdLine == "parse" ) {
- if(!init) std::cout << "No file parsed, run command setFile" << std::endl;
+ if(!init) parseUrdfFile();
else std::cout << " !! Already parsed." <<std::endl;
}
else if( cmdLine == "displayFile" ) {
@@ -1398,6 +1482,28 @@ commandLine( const std::string& cmdLine,
<< " - {create|destroy}Position\t:handle position signals." <<std::endl
<< " - {create|destroy}OpPoint\t:handle Operation Point (ie pos+jac) signals." <<std::endl
<< " - {create|destroy}Acceleration\t:handle acceleration signals." <<std::endl
+ /*TODO: Put these flags for computations (copied from humanoid_robot.py):
+ def setProperties(self, model):
+ model.setProperty('TimeStep', str(self.timeStep))
+ model.setProperty('ComputeAcceleration', 'false')
+ model.setProperty('ComputeAccelerationCoM', 'false')
+ model.setProperty('ComputeBackwardDynamics', 'false')
+ model.setProperty('ComputeCoM', 'false')
+ model.setProperty('ComputeMomentum', 'false')
+ model.setProperty('ComputeSkewCom', 'false')
+ model.setProperty('ComputeVelocity', 'false')
+ model.setProperty('ComputeZMP', 'false')
+
+ model.setProperty('ComputeAccelerationCoM', 'true')
+ model.setProperty('ComputeCoM', 'true')
+ model.setProperty('ComputeVelocity', 'true')
+ model.setProperty('ComputeZMP', 'true')
+
+ if self.enableZmpComputation:
+ model.setProperty('ComputeBackwardDynamics', 'true')
+ model.setProperty('ComputeAcceleration', 'true')
+ model.setProperty('ComputeMomentum', 'true')
+ */
// << " - {get|set}Property <name> [<val>]: set/get the property." <<std::endl
// << " - displayProperties: print the prop-val couples list." <<std::endl
<< " - ndof\t\t\t: display the number of DOF of the robot."<< std::endl;
@@ -1411,56 +1517,26 @@ commandLine( const std::string& cmdLine,
}
+//jointName is either a fixed-joint (pinocchio operational frame) or a
+//movable joint (pinocchio joint-variant).
void Dynamic::cmd_createOpPointSignals( const std::string& opPointName,
const std::string& jointName )
{
- if(std::find(m_model.names.begin(),
- m_model.names.end(),
- jointName)!=m_model.names.end()) {
createEndeffJacobianSignal(std::string("J")+opPointName,jointName);
createPositionSignal(opPointName,jointName);
- }
- else
- SOT_THROW ExceptionDynamic(ExceptionDynamic::GENERIC,
- "Robot has no joint corresponding to " + jointName);
-
}
void Dynamic::cmd_createJacobianWorldSignal( const std::string& signalName,
const std::string& jointName )
{
- if(std::find(m_model.names.begin(),
- m_model.names.end(),
- jointName)!=m_model.names.end()) {
createJacobianSignal(signalName, jointName);
- }
- else
- SOT_THROW ExceptionDynamic(ExceptionDynamic::GENERIC,
- "Robot has no joint corresponding to " + jointName);
}
void Dynamic::cmd_createJacobianEndEffectorSignal( const std::string& signalName,
const std::string& jointName )
{
- if(std::find(m_model.names.begin(),
- m_model.names.end(),
- jointName)!=m_model.names.end()) {
createEndeffJacobianSignal(signalName, jointName);
- }
- else
- SOT_THROW ExceptionDynamic(ExceptionDynamic::GENERIC,
- "Robot has no joint corresponding to " + jointName);
}
void Dynamic::cmd_createPositionSignal( const std::string& signalName,
const std::string& jointName )
{
- SOT_THROW ExceptionDynamic(ExceptionDynamic::GENERIC,
- "Robot has no joint corresponding to " + jointName);
- if(std::find(m_model.names.begin(),
- m_model.names.end(),
- jointName)!=m_model.names.end()) {
createPositionSignal(signalName, jointName);
- }
- else
- SOT_THROW ExceptionDynamic(ExceptionDynamic::GENERIC,
- "Robot has no joint corresponding to " + jointName);
-
}
diff --git a/unitTesting/test_config.cpp b/unitTesting/test_config.cpp
index 8010561..09c212a 100644
--- a/unitTesting/test_config.cpp
+++ b/unitTesting/test_config.cpp
@@ -46,6 +46,8 @@ BOOST_AUTO_TEST_CASE (config)
//Parse urdf file
dynamic_.setUrdfFile("urdf/two_link.urdf");
+ dynamic_.parseUrdfFile();
+ dynamic_.displayModel();
BOOST_CHECK_EQUAL(dynamic_.m_model.nbody,3);
BOOST_CHECK_EQUAL(std::strcmp(dynamic_.m_model.names[0].c_str(),"universe"),0);
BOOST_CHECK_EQUAL(std::strcmp(dynamic_.m_model.names[1].c_str(),"JOINT1"),0);
diff --git a/unitTesting/two_link.urdf b/unitTesting/two_link.urdf
index d20a2d2..570717d 100644
--- a/unitTesting/two_link.urdf
+++ b/unitTesting/two_link.urdf
@@ -14,8 +14,6 @@
<origin xyz="1 0 0"/>
<limit effort="12" lower="0" upper="3.14" velocity="10"/>
</joint>
-
-
<link name="CHILD1">
<inertial>
<origin xyz="0.5 0 0" rpy="0 0 0"/>
@@ -37,48 +35,11 @@
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
</inertial>
</link>
-<!--
- <link name="CHILD3">
- <inertial>
- <origin xyz="0.5 0 0" rpy="0 0 0"/>
- <mass value="20"/>
- <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
- </inertial>
- </link>
- <link name="CHILD4">
- <inertial>
- <origin xyz="0.5 0 0" rpy="0 0 0"/>
- <mass value="20"/>
- <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
- </inertial>
- </link>
- <link name="CHILD5">
- <inertial>
- <origin xyz="0.5 0 0" rpy="0 0 0"/>
- <mass value="20"/>
- <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
- </inertial>
- </link>
- <joint name="JOINT3" type="revolute">
- <axis xyz="0 0 1"/>
+ <joint name="FRAMEJOINT1" type="fixed">
<parent link="CHILD2"/>
<child link="CHILD3"/>
- <origin xyz="1 0 0"/>
- <limit effort="100" lower="0" upper="3.14" velocity="10"/>
+ <origin rpy="0 0 0" xyz="0 0 -0.16"/>
</joint>
- <joint name="JOINT4" type="revolute">
- <axis xyz="0 0 1"/>
- <parent link="CHILD3"/>
- <child link="CHILD4"/>
- <origin xyz="1 0 0"/>
- <limit effort="100" lower="0" upper="3.14" velocity="10"/>
- </joint>
- <joint name="JOINT5" type="revolute">
- <axis xyz="0 0 1"/>
- <parent link="CHILD4"/>
- <child link="CHILD5"/>
- <origin xyz="1 0 0"/>
- <limit effort="100" lower="0" upper="3.14" velocity="10"/>
- </joint>
--->
+ <link name="CHILD3"/>
+
</robot>
--
GitLab