Skip to content
GitLab
Commit 313146af authored by Rohan Budhiraja's avatar Rohan Budhiraja
Browse files

[cpp] change joint-limits to model class + temporary fix for specificities

parent f434b528
Expand all Hide whitespace changes
Inline Side-by-side
include/sot-dynamic/dynamic.h
View file @ 313146af
......@@ -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;
};
......
src/dynamic-command.h
View file @ 313146af
......@@ -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
{
......
src/dynamic_graph/sot/dynamics/humanoid_robot.py
View file @ 313146af
......@@ -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()
src/sot-dynamic.cpp
View file @ 313146af
This diff is collapsed.
unitTesting/test_config.cpp
View file @ 313146af
......@@ -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);
......
unitTesting/two_link.urdf
View file @ 313146af
......@@ -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>
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment