Line |
Branch |
Exec |
Source |
1 |
|
|
// Copyright (c) 2018, LAAS-CNRS |
2 |
|
|
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr) |
3 |
|
|
// |
4 |
|
|
// This file is part of hpp-core. |
5 |
|
|
// hpp-core is free software: you can redistribute it |
6 |
|
|
// and/or modify it under the terms of the GNU Lesser General Public |
7 |
|
|
// License as published by the Free Software Foundation, either version |
8 |
|
|
// 3 of the License, or (at your option) any later version. |
9 |
|
|
// |
10 |
|
|
// hpp-core is distributed in the hope that it will be |
11 |
|
|
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty |
12 |
|
|
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
13 |
|
|
// General Lesser Public License for more details. You should have |
14 |
|
|
// received a copy of the GNU Lesser General Public License along with |
15 |
|
|
// hpp-core. If not, see <http://www.gnu.org/licenses/>. |
16 |
|
|
|
17 |
|
|
#include <hpp/core/path-vector.hh> |
18 |
|
|
#include <hpp/core/plugin.hh> |
19 |
|
|
#include <hpp/core/problem-solver.hh> |
20 |
|
|
#include <hpp/pinocchio/configuration.hh> |
21 |
|
|
#include <hpp/pinocchio/device.hh> |
22 |
|
|
#include <hpp/pinocchio/joint.hh> |
23 |
|
|
#include <hpp/pinocchio/urdf/util.hh> |
24 |
|
|
|
25 |
|
|
using namespace hpp::pinocchio; |
26 |
|
|
using namespace hpp::core; |
27 |
|
|
|
28 |
|
✗ |
int main() { |
29 |
|
✗ |
ProblemSolverPtr_t ps = ProblemSolver::create(); |
30 |
|
|
|
31 |
|
|
// Create robot |
32 |
|
✗ |
DevicePtr_t device = ps->createRobot("pr2"); |
33 |
|
✗ |
hpp::pinocchio::urdf::loadRobotModel( |
34 |
|
|
device, "planar", "example-robot-data/robots/pr2_description", "pr2", "", |
35 |
|
|
""); |
36 |
|
✗ |
device->controlComputation((Computation_t)(JOINT_POSITION | JACOBIAN)); |
37 |
|
✗ |
ps->robot(device); |
38 |
|
|
|
39 |
|
✗ |
device->rootJoint()->lowerBound(0, -4); |
40 |
|
✗ |
device->rootJoint()->upperBound(0, -3); |
41 |
|
✗ |
device->rootJoint()->lowerBound(1, -5); |
42 |
|
✗ |
device->rootJoint()->upperBound(1, -3); |
43 |
|
|
|
44 |
|
|
// Add obstacle |
45 |
|
✗ |
DevicePtr_t obstacle = Device::create("kitchen"); |
46 |
|
✗ |
hpp::pinocchio::urdf::loadModel( |
47 |
|
✗ |
obstacle, 0, "", "anchor", |
48 |
|
|
"package://hpp_tutorial/urdf/kitchen_area.urdf", ""); |
49 |
|
✗ |
obstacle->controlComputation(JOINT_POSITION); |
50 |
|
✗ |
ps->addObstacle(obstacle, true, true); |
51 |
|
|
|
52 |
|
|
// Create initial and final configuration |
53 |
|
✗ |
Configuration_t q_init(device->currentConfiguration()); |
54 |
|
✗ |
q_init.head<2>() << -3.2, -4; |
55 |
|
✗ |
q_init[device->getJointByName("torso_lift_joint")->rankInConfiguration()] = |
56 |
|
|
0.2; |
57 |
|
✗ |
ps->initConfig(q_init); |
58 |
|
|
|
59 |
|
✗ |
Configuration_t q_goal(q_init); |
60 |
|
✗ |
q_goal.head<2>() << -3.2, -4; |
61 |
|
✗ |
q_goal[device->getJointByName("torso_lift_joint")->rankInConfiguration()] = 0; |
62 |
|
✗ |
q_goal[device->getJointByName("l_shoulder_lift_joint") |
63 |
|
✗ |
->rankInConfiguration()] = 0.5; |
64 |
|
✗ |
q_goal[device->getJointByName("l_elbow_flex_joint")->rankInConfiguration()] = |
65 |
|
|
-0.5; |
66 |
|
✗ |
q_goal[device->getJointByName("r_shoulder_lift_joint") |
67 |
|
✗ |
->rankInConfiguration()] = 0.5; |
68 |
|
✗ |
q_goal[device->getJointByName("r_elbow_flex_joint")->rankInConfiguration()] = |
69 |
|
|
-0.5; |
70 |
|
✗ |
ps->addGoalConfig(q_goal); |
71 |
|
|
|
72 |
|
|
bool loaded; |
73 |
|
|
try { |
74 |
|
|
std::string filename = |
75 |
|
✗ |
plugin::findPluginLibrary("spline-gradient-based.so"); |
76 |
|
✗ |
loaded = plugin::loadPlugin(filename, ps); |
77 |
|
✗ |
} catch (const std::invalid_argument&) { |
78 |
|
✗ |
loaded = false; |
79 |
|
|
} |
80 |
|
✗ |
ps->addPathOptimizer("RandomShortcut"); |
81 |
|
✗ |
if (loaded) |
82 |
|
✗ |
ps->addPathOptimizer("SplineGradientBased_bezier1"); |
83 |
|
|
else { |
84 |
|
✗ |
std::cerr << "Could not load spline-gradient-based.so" << std::endl; |
85 |
|
|
} |
86 |
|
|
|
87 |
|
✗ |
ps->solve(); |
88 |
|
✗ |
std::cout << "# Solution path.\n"; |
89 |
|
|
|
90 |
|
✗ |
std::cout << "path = list ()" << std::endl; |
91 |
|
✗ |
PathPtr_t path(ps->paths().back()); |
92 |
|
✗ |
value_type L(path->length()); |
93 |
|
|
bool success; |
94 |
|
✗ |
Configuration_t q; |
95 |
|
✗ |
for (value_type t = 0; t < L; t += .01) { |
96 |
|
✗ |
q = path->eval(t, success); |
97 |
|
✗ |
assert(success); |
98 |
|
✗ |
std::cout << "path.append (" << displayConfig(q) << ")" << std::endl; |
99 |
|
|
} |
100 |
|
✗ |
q = path->eval(L, success); |
101 |
|
✗ |
std::cout << "path.append (" << displayConfig(q) << ")" << std::endl; |
102 |
|
|
|
103 |
|
✗ |
return 0; |
104 |
|
|
} |
105 |
|
|
|