Skip to content

Commit 6e4283b

Browse files
aclodicFrancois Keith
aclodic
authored and
Francois Keith
committed
Update to be able to load urdf model
 * Add buildModelUrdf function dedicated to the load * Add setUrdfDirectory and setUrdfMainfile to be able to set urdf file name * Add python bindings
1 parent f0ceca8 commit 6e4283b

File tree

4 files changed

+174
-8
lines changed

4 files changed

+174
-8
lines changed

‎include/sot-pattern-generator/pg.h

Lines changed: 23 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -131,7 +131,17 @@ namespace dynamicgraph {
131131

132132
/*! \brief Name of the XML file which specificies the rank of the Joints in the state vector. */
133133
std::string m_xmlRankFile;
134-
/*! @} */
134+
135+
/*! \brief Directory where the VRML file of the robot's model is located. */
136+
std::string m_urdfDirectory;
137+
138+
/*! \brief Name of the VRML file which containes the robot's model. */
139+
std::string m_urdfMainFile;
140+
141+
/* \brief Special joints map for the parser */
142+
std::map<std::string, std::string> specialJoints_;
143+
144+
/*! @} */
135145

136146
/*! \brief Boolean variable to initialize the object by loading an object. */
137147
bool m_init;
@@ -175,6 +185,9 @@ namespace dynamicgraph {
175185
/*! \brief Build the pattern generator interface. */
176186
bool buildModel( void );
177187

188+
/*! \brief Build the pattern generator interface from a Urdf file. */
189+
bool buildModelUrdf( void );
190+
178191
/*! \brief Initialize the state of the robot. */
179192
bool InitState( void );
180193

@@ -199,6 +212,15 @@ namespace dynamicgraph {
199212
of the preview control. */
200213
void setParamPreviewFile(const std::string &filename);
201214

215+
/*! \brief Set the directory which contains the urdf files robot's model. */
216+
void setUrdfDirectory( const std::string& filename );
217+
218+
/*! \brief Set the name of the urdf file. */
219+
void setUrdfMainFile( const std::string& filename );
220+
221+
/*! \brief Set mapping between a link and actual robot name */
222+
void addJointMapping(const std::string& link, const std::string& repName);
223+
202224
/*! \brief Give access directly to the pattern generator...
203225
You really have to know what your are doing. */
204226
pg::PatternGeneratorInterface * GetPatternGeneratorInterface()

‎python/walk_romeo.py

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,7 @@
1010
from dynamic_graph.sot.dynamics import *
1111
from dynamic_graph.sot.romeo.robot import *
1212
robot = Robot('ROMEO', device=RobotSimu('ROMEO'))
13+
from dynamic_graph import plug
1314
plug(robot.device.state, robot.dynamic.position)
1415

1516

@@ -21,8 +22,8 @@
2122
from dynamic_graph.sot.application.velocity.precomputed_tasks import initialize
2223
solver = initialize ( robot )
2324

24-
from dynamic_graph.sot.pattern_generator.walking import CreateEverythingForPG , walkFewSteps, walkAndrei
25-
CreateEverythingForPG ( robot , solver )
25+
from dynamic_graph.sot.pattern_generator.walking import CreateEverythingForPGwithUrdf , walkFewSteps, walkAndrei
26+
CreateEverythingForPGwithUrdf ( robot , solver )
2627
# walkFewSteps ( robot )
2728
walkAndrei( robot )
2829

@@ -55,4 +56,3 @@ def inc():
5556

5657
go()
5758

58-

‎src/dynamic_graph/sot/pattern_generator/walking.py

Lines changed: 46 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,7 @@ def addPgToRobot(robot):
2626
robotName=robot.modelName
2727
specificitiesPath=robot.specificitiesPath
2828
jointRankPath=robot.jointRankPath
29+
2930
robot.pg = PatternGenerator('pg')
3031
robot.pg.setVrmlDir(modelDir+'/')
3132
robot.pg.setVrml(robotName)
@@ -60,6 +61,41 @@ def addPgToRobot(robot):
6061

6162
robot.pg.initState()
6263

64+
def addPgToUrdfRobot(robot):
65+
robot.pg = PatternGenerator('pg')
66+
robot.pg.setUrdfDir('package://romeo_description/urdf/')
67+
robot.pg.setUrdf('romeo_small.urdf')
68+
robot.pg.addJointMapping('BODY','body')
69+
70+
print "At this stage"
71+
robot.pg.buildModelUrdf()
72+
73+
# Standard initialization
74+
robot.pg.parseCmd(":samplingperiod 0.005")
75+
robot.pg.parseCmd(":previewcontroltime 1.6")
76+
robot.pg.parseCmd(":walkmode 0")
77+
robot.pg.parseCmd(":omega 0.0")
78+
robot.pg.parseCmd(":stepheight 0.05")
79+
robot.pg.parseCmd(":singlesupporttime 0.780")
80+
robot.pg.parseCmd(":doublesupporttime 0.020")
81+
robot.pg.parseCmd(":armparameters 0.5")
82+
robot.pg.parseCmd(":LimitsFeasibility 0.0")
83+
robot.pg.parseCmd(":ZMPShiftParameters 0.015 0.015 0.015 0.015")
84+
robot.pg.parseCmd(":TimeDistributeParameters 2.0 3.5 1.0 3.0")
85+
robot.pg.parseCmd(":UpperBodyMotionParameters 0.0 -0.5 0.0")
86+
robot.pg.parseCmd(":comheight 0.814")
87+
robot.pg.parseCmd(":SetAlgoForZmpTrajectory Morisawa")
88+
89+
plug(robot.dynamic.position,robot.pg.position)
90+
plug(robot.com, robot.pg.com)
91+
plug(robot.dynamic.signal('left-ankle'), robot.pg.leftfootcurrentpos)
92+
plug(robot.dynamic.signal('right-ankle'), robot.pg.rightfootcurrentpos)
93+
robotDim = len(robot.dynamic.velocity.value)
94+
robot.pg.motorcontrol.value = robotDim*(0,)
95+
robot.pg.zmppreviouscontroller.value = (0,0,0)
96+
97+
robot.pg.initState()
98+
6399

64100
def addPgTaskToRobot(robot,solver):
65101
# --- ROBOT.PG INIT FRAMES ---
@@ -188,8 +224,8 @@ def initFeetTask(robot):
188224
robot.pg.rightfootref])
189225

190226
plug(robot.pg.inprocess,robot.selecFeet.selec)
191-
robot.tasks['right-ankle'].controlGain.value = 200
192-
robot.tasks['left-ankle'].controlGain.value = 200
227+
robot.tasks['right-ankle'].controlGain.value = 180
228+
robot.tasks['left-ankle'].controlGain.value = 180
193229

194230
print "After Task for Right and Left Feet"
195231

@@ -295,6 +331,12 @@ def CreateEverythingForPG(robot,solver):
295331
addPgTaskToRobot(robot,solver)
296332
createGraph(robot,solver)
297333

334+
def CreateEverythingForPGwithUrdf(robot,solver):
335+
robot.initializeTracer()
336+
addPgToUrdfRobot(robot)
337+
addPgTaskToRobot(robot,solver)
338+
createGraph(robot,solver)
339+
298340
def walkFewSteps(robot):
299341
robot.pg.parseCmd(":stepseq 0.0 0.1025 0.0 0.17 -0.205 0.0 0.17 0.205 0.0 0.17 -0.205 0.0 0.17 0.205 0.0 0.0 -0.205 0.0")
300342

@@ -305,9 +347,10 @@ def walkAndrei(robot):
305347
robot.startTracer()
306348
robot.pg.parseCmd(":SetAlgoForZmpTrajectory Herdt")
307349
robot.pg.parseCmd(":doublesupporttime 0.1")
308-
robot.pg.parseCmd(":singlesupporttime 0.7")
350+
robot.pg.parseCmd(":singlesupporttime 0.8")
309351
robot.pg.velocitydes.value=(0.01,0.0,0.0)
310352
robot.pg.parseCmd(":numberstepsbeforestop 4")
353+
robot.pg.parseCmd(":setfeetconstraint XY 0.02 0.02")
311354
robot.pg.parseCmd(":setVelReference 0.01 0.0 0.0")
312355
robot.pg.parseCmd(":HerdtOnline 0.01 0.0 0.0")
313356
if robot.device.name == 'HRP2LAAS' or \

‎src/pg.cpp

Lines changed: 102 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -41,6 +41,7 @@
4141
#include <sot/core/matrix-homogeneous.hh>
4242

4343
#include <sot-pattern-generator/pg.h>
44+
#include <jrl/dynamics/urdf/parser.hh>
4445

4546
using namespace std;
4647
namespace dynamicgraph {
@@ -549,6 +550,58 @@ namespace dynamicgraph {
549550
return false;
550551
}
551552

553+
bool PatternGenerator::buildModelUrdf( void )
554+
{
555+
jrl::dynamics::urdf::Parser parser;
556+
557+
// Creating the humanoid robot.
558+
dynamicsJRLJapan::ObjectFactory aRobotDynamicsObjectConstructor;
559+
CjrlHumanoidDynamicRobot * aHDR = 0;
560+
561+
// Parsing the file.
562+
string RobotFileName = m_urdfDirectory + m_urdfMainFile;
563+
564+
std::map<std::string, std::string>::const_iterator it = specialJoints_.begin();
565+
for (;it!=specialJoints_.end();++it) {
566+
parser.specifyREPName(it->first, it->second);
567+
}
568+
aHDR = parser.parse(RobotFileName);
569+
bool ok=true;
570+
571+
if (aHDR!=0)
572+
{
573+
CjrlFoot * rightFoot = aHDR->rightFoot();
574+
if (rightFoot!=0)
575+
{
576+
vector3d AnkleInFoot;
577+
rightFoot->getAnklePositionInLocalFrame(AnkleInFoot);
578+
m_AnkleSoilDistance = fabs(AnkleInFoot[2]);
579+
}
580+
else ok=false;
581+
}
582+
else ok=false;
583+
if (!ok)
584+
{
585+
SOT_THROW ExceptionPatternGenerator( ExceptionPatternGenerator::PATTERN_GENERATOR_JRL,
586+
"Error while creating humanoid robot dynamical model.",
587+
"(PG creation process for object %s).",
588+
getName().c_str());
589+
}
590+
try
591+
{
592+
m_PGI = PatternGeneratorJRL::patternGeneratorInterfaceFactory(aHDR);
593+
}
594+
catch (...)
595+
{
596+
SOT_THROW ExceptionPatternGenerator( ExceptionPatternGenerator::PATTERN_GENERATOR_JRL,
597+
"Error while allocating the Pattern Generator.",
598+
"(PG creation process for object %s).",
599+
getName().c_str());
600+
}
601+
m_init = true;
602+
return false;
603+
}
604+
552605
PatternGenerator::
553606
~PatternGenerator( void )
554607
{
@@ -596,6 +649,21 @@ namespace dynamicgraph {
596649
m_PreviewControlParametersFile = filename;
597650
}
598651

652+
void PatternGenerator::
653+
setUrdfDirectory( const std::string& filename )
654+
{
655+
m_urdfDirectory = filename;
656+
}
657+
void PatternGenerator::
658+
setUrdfMainFile( const std::string& filename )
659+
{
660+
m_urdfMainFile = filename;
661+
}
662+
void PatternGenerator::
663+
addJointMapping(const std::string &link, const std::string &repName)
664+
{
665+
specialJoints_[link] = repName;
666+
}
599667

600668
/* --- COMPUTE -------------------------------------------------------------- */
601669
/* --- COMPUTE -------------------------------------------------------------- */
@@ -1235,6 +1303,19 @@ namespace dynamicgraph {
12351303
makeCommandVoid1(*this,&PatternGenerator::setVrmlMainFile,
12361304
docCommandVoid1("Set VRML main file.",
12371305
"string (file name)")));
1306+
addCommand("setUrdfDir",
1307+
makeCommandVoid1(*this,&PatternGenerator::setUrdfDirectory,
1308+
docCommandVoid1("Set Urdf directory.",
1309+
"string (path name)")));
1310+
addCommand("setUrdf",
1311+
makeCommandVoid1(*this,&PatternGenerator::setUrdfMainFile,
1312+
docCommandVoid1("Set Urdf main file.",
1313+
"string (file name)")));
1314+
addCommand("addJointMapping",
1315+
makeCommandVoid2(*this,&PatternGenerator::addJointMapping,
1316+
docCommandVoid1("Map link names.",
1317+
"string (link name)"
1318+
"string (rep name)")));
12381319
addCommand("setXmlSpec",
12391320
makeCommandVoid1(*this,&PatternGenerator::setXmlSpecificitiesFile,
12401321
docCommandVoid1("Set Xml file for specicifities.",
@@ -1253,6 +1334,10 @@ namespace dynamicgraph {
12531334
makeCommandVoid0(*this,
12541335
(void (PatternGenerator::*) (void))&PatternGenerator::buildModel,
12551336
docCommandVoid0("From the files, parse and build.")));
1337+
addCommand("buildModelUrdf",
1338+
makeCommandVoid0(*this,
1339+
(void (PatternGenerator::*) (void))&PatternGenerator::buildModelUrdf,
1340+
docCommandVoid0("From the files, parse and build.")));
12561341
addCommand("initState",
12571342
makeCommandVoid0(*this,
12581343
(void (PatternGenerator::*) (void))&PatternGenerator::InitState,
@@ -1359,6 +1444,16 @@ namespace dynamicgraph {
13591444
{ cmdArgs>>filename; setVrmlDirectory( filename ); }
13601445
else if( cmdLine == "setVrml" )
13611446
{ cmdArgs>>filename; setVrmlMainFile( filename ); }
1447+
else if( cmdLine == "setUrdfDir" )
1448+
{ cmdArgs>>filename; setUrdfDirectory( filename ); }
1449+
else if( cmdLine == "setUrdf" )
1450+
{ cmdArgs>>filename; setUrdfMainFile( filename ); }
1451+
else if( cmdLine == "addJointMapping" )
1452+
{
1453+
std::string link, repName;
1454+
cmdArgs>> link >> repName;
1455+
addJointMapping( link, repName); }
1456+
13621457
else if( cmdLine == "setXmlSpec" )
13631458
{ cmdArgs>>filename; setXmlSpecificitiesFile( filename ); }
13641459
else if( cmdLine == "setXmlRank" )
@@ -1370,6 +1465,8 @@ namespace dynamicgraph {
13701465
cmdArgs>>filename; setParamPreviewFile( filename );
13711466
cmdArgs>>filename; setVrmlDirectory( filename );
13721467
cmdArgs>>filename; setVrmlMainFile( filename );
1468+
cmdArgs>>filename; setUrdfDirectory( filename );
1469+
cmdArgs>>filename; setUrdfMainFile( filename );
13731470
cmdArgs>>filename; setXmlSpecificitiesFile( filename );
13741471
cmdArgs>>filename; setXmlRankFile( filename );
13751472
}
@@ -1385,14 +1482,18 @@ namespace dynamicgraph {
13851482
else if( "xmlspecificity" == filetype ) { os << m_xmlSpecificitiesFile << std::endl; }
13861483
else if( "xmlrank" == filetype ) { os << m_xmlRankFile << std::endl; }
13871484
else if( "vrmlmain" == filetype ) { os << m_vrmlMainFile << std::endl; }
1485+
else if( "urdfmain" == filetype ) { os << m_urdfMainFile << std::endl; }
1486+
else if( "urdfdir" == filetype ) { os << m_urdfDirectory << std::endl; }
13881487
else filespecified = false;
13891488
}
13901489
if( ! filespecified )
13911490
{
13921491
os << " - VRML Directory:\t\t\t" << m_vrmlDirectory <<endl
13931492
<< " - XML Specificities File:\t\t" << m_xmlSpecificitiesFile <<endl
13941493
<< " - XML Rank File:\t\t\t" << m_xmlRankFile <<endl
1395-
<< " - VRML Main File:\t\t\t" << m_vrmlMainFile <<endl;
1494+
<< " - VRML Main File:\t\t\t" << m_vrmlMainFile <<endl
1495+
<< " - Urdf Directory:\t\t\t" << m_urdfDirectory <<endl
1496+
<< " - Urdf Main File:\t\t\t" << m_urdfMainFile <<endl;
13961497
}
13971498
}
13981499
else if( cmdLine == "buildModel" )

0 commit comments

Comments
 (0)