GCC Code Coverage Report


Directory: src/
File: src/tutorial_1.cc
Date: 2024-12-13 15:46:19
Exec Total Coverage
Lines: 0 50 0.0%
Branches: 0 194 0.0%

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