Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Stack Of Tasks
sot-dynamic-pinocchio
Commits
313146af
Commit
313146af
authored
Apr 08, 2016
by
Rohan Budhiraja
Browse files
[cpp] change joint-limits to model class + temporary fix for specificities
parent
f434b528
Changes
6
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
par
se a urdf file to create robot model
/// \brief se
ts
a urdf file
path
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
::
Matrix
4d
>
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
.
set
P
ropert
ies
(
model
)
mod
el
.
set
Files
(
modelDir
,
modelName
,
specificitiesPath
,
jointRank
Path
)
#TODO:
set
p
ropert
y flags in sot-pinocchio
#s
el
f
.set
Properties(model)
model
.
setFile
(
urdf
Path
)
model
.
parse
()
return
model
def
setProperties
(
self
,
model
):
model
.
setProperty
(
'TimeStep'
,
str
(
self
.
timeStep
))
model
.
setProperty
(
'ComputeAcceleration'
,
'false'
)
model
.
setProperty
(
'ComputeAcceleration
CoM
'
,
'false'
)
model
.
setProperty
(
'Compute
BackwardDynamics
'
,
'false'
)
model
.
setProperty
(
'Compute
CoM
'
,
'false'
)
model
.
setProperty
(
'Compute
Momentum
'
,
'false'
)
model
.
setProperty
(
'Compute
SkewCo
m'
,
'false'
)
model
.
setProperty
(
'Compute
Velocity
'
,
'false'
)
model
.
setProperty
(
'Compute
ZMP
'
,
'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('Compute
AccelerationCoM
', 'false')
#
model.setProperty('Compute
BackwardDynamics
', 'false')
#
model.setProperty('Compute
CoM
', 'false')
#
model.setProperty('Compute
Momentu
m', 'false')
#
model.setProperty('Compute
SkewCom
', 'false')
#
model.setProperty('Compute
Velocity
', '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.
Click to expand it.
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>
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment