Skip to content

Commit 30f08e1

Browse files
rickertmrickertm
committed
Replace rl::kin with rl::mdl in demos and tests
1 parent d0b2712 commit 30f08e1

18 files changed

Lines changed: 439 additions & 91 deletions

‎demos/rlJacobianDemo/CMakeLists.txt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -14,5 +14,5 @@ target_include_directories(
1414

1515
target_link_libraries(
1616
rlJacobianDemo
17-
kin
17+
mdl
1818
)

‎demos/rlJacobianDemo/rlJacobianDemo.cpp‎

Lines changed: 28 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -28,8 +28,10 @@
2828
#include <memory>
2929
#include <stdexcept>
3030
#include <boost/lexical_cast.hpp>
31-
#include <rl/kin/Kinematics.h>
3231
#include <rl/math/Unit.h>
32+
#include <rl/mdl/Kinematic.h>
33+
#include <rl/mdl/UrdfFactory.h>
34+
#include <rl/mdl/XmlFactory.h>
3335

3436
int
3537
main(int argc, char** argv)
@@ -42,46 +44,56 @@ main(int argc, char** argv)
4244

4345
try
4446
{
45-
std::shared_ptr<rl::kin::Kinematics> kinematics(rl::kin::Kinematics::create(argv[1]));
47+
std::string filename(argv[1]);
48+
std::shared_ptr<rl::mdl::Kinematic> kinematic;
4649

47-
rl::math::Vector q(kinematics->getDof());
50+
if ("urdf" == filename.substr(filename.length() - 4, 4))
51+
{
52+
rl::mdl::UrdfFactory factory;
53+
kinematic = std::dynamic_pointer_cast<rl::mdl::Kinematic>(factory.create(filename));
54+
}
55+
else
56+
{
57+
rl::mdl::XmlFactory factory;
58+
kinematic = std::dynamic_pointer_cast<rl::mdl::Kinematic>(factory.create(filename));
59+
}
60+
61+
rl::math::Vector q(kinematic->getDofPosition());
4862

4963
for (std::ptrdiff_t i = 0; i < q.size(); ++i)
5064
{
5165
q(i) = boost::lexical_cast<rl::math::Real>(argv[i + 2]);
5266
}
5367

54-
rl::math::Vector qdot(kinematics->getDof());
68+
rl::math::Vector qdot(kinematic->getDof());
5569

5670
for (std::ptrdiff_t i = 0; i < qdot.size(); ++i)
5771
{
5872
qdot(i) = boost::lexical_cast<rl::math::Real>(argv[q.size() + i + 2]);
5973
}
6074

61-
kinematics->setPosition(q);
62-
kinematics->updateFrames();
63-
kinematics->updateJacobian();
64-
65-
std::cout << "J=" << std::endl << kinematics->getJacobian() << std::endl;
75+
kinematic->setPosition(q);
76+
kinematic->forwardPosition();
77+
kinematic->calculateJacobian();
6678

67-
rl::math::Vector xdot(kinematics->getOperationalDof() * 6);
79+
std::cout << "J=" << std::endl << kinematic->getJacobian() << std::endl;
6880

69-
kinematics->forwardVelocity(qdot, xdot);
81+
rl::math::Vector xdot = kinematic->getJacobian() * qdot;
7082

7183
std::cout << "xdot=" << std::endl;
7284

73-
for (std::size_t i = 0; i < kinematics->getOperationalDof(); ++i)
85+
for (std::size_t i = 0; i < kinematic->getOperationalDof(); ++i)
7486
{
7587
std::cout << i << " " << xdot.transpose() << std::endl;
7688
}
7789

78-
kinematics->updateJacobianInverse(static_cast<rl::math::Real>(1.0e-9));
90+
kinematic->calculateJacobianInverse();
7991

80-
std::cout << "invJ=" << std::endl << kinematics->getJacobianInverse() << std::endl;
92+
std::cout << "invJ=" << std::endl << kinematic->getJacobianInverse() << std::endl;
8193

82-
kinematics->inverseVelocity(xdot, qdot);
94+
rl::math::Vector qdot2 = kinematic->getJacobianInverse() * xdot;
8395

84-
std::cout << "qdot=" << std::endl << qdot.transpose() << std::endl;
96+
std::cout << "qdot=" << std::endl << qdot2.transpose() << std::endl;
8597
}
8698
catch (const std::exception& e)
8799
{

‎demos/rlPlanDemo/CMakeLists.txt‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -128,7 +128,7 @@ if(QT_FOUND AND SoQt_FOUND AND (Bullet_FOUND OR (CCD_FOUND AND FCL_FOUND) OR ODE
128128
\\\"$SMPROGRAMS\\\\$STARTMENU_FOLDER\\\\rlPlanDemo.lnk\\\" \\\\
129129
\\\"$INSTDIR\\\\${CMAKE_INSTALL_BINDIR}\\\\rlPlanDemo.exe\\\" \\\\
130130
\\\"\\\\
131-
\\\$\\\\\\\"$INSTDIR\\\\${CMAKE_INSTALL_DATADIR}\\\\rl-${VERSION}\\\\examples\\\\rlplan\\\\unimation-puma560_boxes_rrtConCon.xml\\\$\\\\\\\"\\\\
131+
\\\$\\\\\\\"$INSTDIR\\\\${CMAKE_INSTALL_DATADIR}\\\\rl-${VERSION}\\\\examples\\\\rlplan\\\\unimation-puma560_boxes_rrtConCon.mdl.xml\\\$\\\\\\\"\\\\
132132
\\\" \\\\
133133
\\\"\\\" \\\"\\\" \\\"\\\" \\\"\\\" \\\\
134134
\\\"Path planning demo with Unimation Puma 560 and RRT algorithm\\\""
@@ -140,7 +140,7 @@ if(QT_FOUND AND SoQt_FOUND AND (Bullet_FOUND OR (CCD_FOUND AND FCL_FOUND) OR ODE
140140
WIX_SHORTCUTS
141141
${WIX_SHORTCUTS}
142142
"<Shortcut
143-
Arguments=\"&quot;[INSTALL_ROOT]${CMAKE_INSTALL_DATADIR}\\rl-${VERSION}\\examples\\rlplan\\unimation-puma560_boxes_rrtConCon.xml&quot;\"
143+
Arguments=\"&quot;[INSTALL_ROOT]${CMAKE_INSTALL_DATADIR}\\rl-${VERSION}\\examples\\rlplan\\unimation-puma560_boxes_rrtConCon.mdl.xml&quot;\"
144144
Description=\"Path planning demo with Unimation Puma 560 and RRT algorithm\"
145145
Id=\"CM_SP_bin.rlPlanDemo.exe\"
146146
Name=\"rlPlanDemo\"

‎demos/rlPlanDemo/rlPlanDemo.desktop.in‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
[Desktop Entry]
22
Categories=Science;Robotics;
33
Comment=Path planning demo with Unimation Puma 560 and RRT algorithm
4-
Exec=sh -c "rlPlanDemo $(dirname %k)/../rl-@VERSION@/examples/rlplan/unimation-puma560_boxes_rrtConCon.xml"
4+
Exec=sh -c "rlPlanDemo $(dirname %k)/../rl-@VERSION@/examples/rlplan/unimation-puma560_boxes_rrtConCon.mdl.xml"
55
Icon=robotics-library
66
Name=Robotics Library (rlPlanDemo)
77
Type=Application

‎demos/rlPrmDemo/CMakeLists.txt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -19,8 +19,8 @@ if(Bullet_FOUND OR ODE_FOUND OR SOLID3_FOUND)
1919

2020
target_link_libraries(
2121
rlPrmDemo
22+
mdl
2223
plan
23-
kin
2424
sg
2525
)
2626
endif()

‎demos/rlPrmDemo/rlPrmDemo.cpp‎

Lines changed: 35 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -28,13 +28,16 @@
2828
#include <memory>
2929
#include <stdexcept>
3030
#include <boost/lexical_cast.hpp>
31-
#include <rl/kin/Kinematics.h>
3231
#include <rl/math/Unit.h>
32+
#include <rl/mdl/Kinematic.h>
33+
#include <rl/mdl/UrdfFactory.h>
34+
#include <rl/mdl/XmlFactory.h>
3335
#include <rl/plan/KdtreeNearestNeighbors.h>
3436
#include <rl/plan/Prm.h>
3537
#include <rl/plan/RecursiveVerifier.h>
3638
#include <rl/plan/SimpleModel.h>
3739
#include <rl/plan/UniformSampler.h>
40+
#include <rl/sg/UrdfFactory.h>
3841
#include <rl/sg/XmlFactory.h>
3942

4043
#if defined(RL_SG_SOLID)
@@ -59,20 +62,42 @@ main(int argc, char** argv)
5962

6063
try
6164
{
62-
rl::sg::XmlFactory factory;
65+
std::string scenefile(argv[1]);
6366
#if defined(RL_SG_SOLID)
6467
rl::sg::solid::Scene scene;
6568
#elif defined(RL_SG_BULLET)
6669
rl::sg::bullet::Scene scene;
6770
#elif defined(RL_SG_ODE)
6871
rl::sg::ode::Scene scene;
6972
#endif
70-
factory.load(argv[1], &scene);
7173

72-
std::shared_ptr<rl::kin::Kinematics> kinematics(rl::kin::Kinematics::create(argv[2]));
74+
if ("urdf" == scenefile.substr(scenefile.length() - 4, 4))
75+
{
76+
rl::sg::UrdfFactory factory;
77+
factory.load(scenefile, &scene);
78+
}
79+
else
80+
{
81+
rl::sg::XmlFactory factory;
82+
factory.load(scenefile, &scene);
83+
}
84+
85+
std::string kinematicsfile(argv[2]);
86+
std::shared_ptr<rl::mdl::Kinematic> kinematic;
87+
88+
if ("urdf" == kinematicsfile.substr(kinematicsfile.length() - 4, 4))
89+
{
90+
rl::mdl::UrdfFactory factory;
91+
kinematic = std::dynamic_pointer_cast<rl::mdl::Kinematic>(factory.create(kinematicsfile));
92+
}
93+
else
94+
{
95+
rl::mdl::XmlFactory factory;
96+
kinematic = std::dynamic_pointer_cast<rl::mdl::Kinematic>(factory.create(kinematicsfile));
97+
}
7398

7499
rl::plan::SimpleModel model;
75-
model.kin = kinematics.get();
100+
model.mdl = kinematic.get();
76101
model.model = scene.getModel(0);
77102
model.scene = &scene;
78103

@@ -91,20 +116,20 @@ main(int argc, char** argv)
91116
verifier.delta = 1 * rl::math::DEG2RAD;
92117
verifier.model = &model;
93118

94-
rl::math::Vector start(kinematics->getDof());
119+
rl::math::Vector start(kinematic->getDofPosition());
95120

96-
for (std::size_t i = 0; i < kinematics->getDof(); ++i)
121+
for (std::ptrdiff_t i = 0; i < start.size(); ++i)
97122
{
98123
start(i) = boost::lexical_cast<rl::math::Real>(argv[i + 3]) * rl::math::DEG2RAD;
99124
}
100125

101126
planner.start = &start;
102127

103-
rl::math::Vector goal(kinematics->getDof());
128+
rl::math::Vector goal(kinematic->getDofPosition());
104129

105-
for (std::size_t i = 0; i < kinematics->getDof(); ++i)
130+
for (std::ptrdiff_t i = 0; i < goal.size(); ++i)
106131
{
107-
goal(i) = boost::lexical_cast<rl::math::Real>(argv[kinematics->getDof() + i + 3]) * rl::math::DEG2RAD;
132+
goal(i) = boost::lexical_cast<rl::math::Real>(argv[start.size() + i + 3]) * rl::math::DEG2RAD;
108133
}
109134

110135
planner.goal = &goal;

‎demos/rlRrtDemo/CMakeLists.txt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -19,8 +19,8 @@ if(Bullet_FOUND OR ODE_FOUND OR SOLID3_FOUND)
1919

2020
target_link_libraries(
2121
rlRrtDemo
22+
mdl
2223
plan
23-
kin
2424
sg
2525
)
2626
endif()

‎demos/rlRrtDemo/rlRrtDemo.cpp‎

Lines changed: 35 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -28,12 +28,15 @@
2828
#include <memory>
2929
#include <stdexcept>
3030
#include <boost/lexical_cast.hpp>
31-
#include <rl/kin/Kinematics.h>
3231
#include <rl/math/Unit.h>
32+
#include <rl/mdl/Kinematic.h>
33+
#include <rl/mdl/UrdfFactory.h>
34+
#include <rl/mdl/XmlFactory.h>
3335
#include <rl/plan/KdtreeNearestNeighbors.h>
3436
#include <rl/plan/RrtConCon.h>
3537
#include <rl/plan/SimpleModel.h>
3638
#include <rl/plan/UniformSampler.h>
39+
#include <rl/sg/UrdfFactory.h>
3740
#include <rl/sg/XmlFactory.h>
3841

3942
#if defined(RL_SG_SOLID)
@@ -58,20 +61,42 @@ main(int argc, char** argv)
5861

5962
try
6063
{
61-
rl::sg::XmlFactory factory;
64+
std::string scenefile(argv[1]);
6265
#if defined(RL_SG_SOLID)
6366
rl::sg::solid::Scene scene;
6467
#elif defined(RL_SG_BULLET)
6568
rl::sg::bullet::Scene scene;
6669
#elif defined(RL_SG_ODE)
6770
rl::sg::ode::Scene scene;
6871
#endif
69-
factory.load(argv[1], &scene);
7072

71-
std::shared_ptr<rl::kin::Kinematics> kinematics(rl::kin::Kinematics::create(argv[2]));
73+
if ("urdf" == scenefile.substr(scenefile.length() - 4, 4))
74+
{
75+
rl::sg::UrdfFactory factory;
76+
factory.load(scenefile, &scene);
77+
}
78+
else
79+
{
80+
rl::sg::XmlFactory factory;
81+
factory.load(scenefile, &scene);
82+
}
83+
84+
std::string kinematicsfile(argv[2]);
85+
std::shared_ptr<rl::mdl::Kinematic> kinematic;
86+
87+
if ("urdf" == kinematicsfile.substr(kinematicsfile.length() - 4, 4))
88+
{
89+
rl::mdl::UrdfFactory factory;
90+
kinematic = std::dynamic_pointer_cast<rl::mdl::Kinematic>(factory.create(kinematicsfile));
91+
}
92+
else
93+
{
94+
rl::mdl::XmlFactory factory;
95+
kinematic = std::dynamic_pointer_cast<rl::mdl::Kinematic>(factory.create(kinematicsfile));
96+
}
7297

7398
rl::plan::SimpleModel model;
74-
model.kin = kinematics.get();
99+
model.mdl = kinematic.get();
75100
model.model = scene.getModel(0);
76101
model.scene = &scene;
77102

@@ -89,20 +114,20 @@ main(int argc, char** argv)
89114

90115
planner.delta = 1 * rl::math::DEG2RAD;
91116

92-
rl::math::Vector start(kinematics->getDof());
117+
rl::math::Vector start(kinematic->getDofPosition());
93118

94-
for (std::size_t i = 0; i < kinematics->getDof(); ++i)
119+
for (std::ptrdiff_t i = 0; i < start.size(); ++i)
95120
{
96121
start(i) = boost::lexical_cast<rl::math::Real>(argv[i + 3]) * rl::math::DEG2RAD;
97122
}
98123

99124
planner.start = &start;
100125

101-
rl::math::Vector goal(kinematics->getDof());
126+
rl::math::Vector goal(kinematic->getDofPosition());
102127

103-
for (std::size_t i = 0; i < kinematics->getDof(); ++i)
128+
for (std::ptrdiff_t i = 0; i < goal.size(); ++i)
104129
{
105-
goal(i) = boost::lexical_cast<rl::math::Real>(argv[kinematics->getDof() + i + 3]) * rl::math::DEG2RAD;
130+
goal(i) = boost::lexical_cast<rl::math::Real>(argv[start.size() + i + 3]) * rl::math::DEG2RAD;
106131
}
107132

108133
planner.goal = &goal;

‎examples/rlmdl/box-6d-300505.sixDof.xml‎

Lines changed: 54 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,54 @@
1+
<?xml version="1.0" encoding="UTF-8"?>
2+
<rlmdl xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="rlmdl.xsd">
3+
<model>
4+
<manufacturer></manufacturer>
5+
<name>6D Box (3.0m x 0.5m x 0.5m)</name>
6+
<world id="world">
7+
<rotation>
8+
<x>0</x>
9+
<y>0</y>
10+
<z>0</z>
11+
</rotation>
12+
<translation>
13+
<x>0</x>
14+
<y>0</y>
15+
<z>0</z>
16+
</translation>
17+
<g>
18+
<x>0</x>
19+
<y>0</y>
20+
<z>9.86055</z>
21+
</g>
22+
</world>
23+
<sixDof id="joint0">
24+
<frame>
25+
<a idref="world"/>
26+
<b idref="body1"/>
27+
</frame>
28+
<max>30</max>
29+
<max>30</max>
30+
<max>2</max>
31+
<min>0</min>
32+
<min>0</min>
33+
<min>0</min>
34+
</sixDof>
35+
<body id="body1"/>
36+
<fixed id="tool">
37+
<frame>
38+
<a idref="body1"/>
39+
<b idref="tcp"/>
40+
</frame>
41+
<rotation>
42+
<x>0</x>
43+
<y>0</y>
44+
<z>0</z>
45+
</rotation>
46+
<translation>
47+
<x>1.5</x>
48+
<y>0</y>
49+
<z>0</z>
50+
</translation>
51+
</fixed>
52+
<frame id="tcp"/>
53+
</model>
54+
</rlmdl>

0 commit comments

Comments
 (0)