GCC Code Coverage Report

Directory: ./
File: src/simple-device.cc
Date: 2025-05-04 12:09:19
Exec Total Coverage
Lines: 74 81 91.4%
Branches: 174 351 49.6%
Line Branch Exec Source
1 // Copyright (c) 2017, Joseph Mirabel
2 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
3 //
4
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // 1. Redistributions of source code must retain the above copyright
10 // notice, this list of conditions and the following disclaimer.
11 //
12 // 2. Redistributions in binary form must reproduce the above copyright
13 // notice, this list of conditions and the following disclaimer in the
14 // documentation and/or other materials provided with the distribution.
15 //
16 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27 // DAMAGE.
28
29 #include <hpp/pinocchio/device.hh>
30 #include <hpp/pinocchio/humanoid-robot.hh>
31 #include <hpp/pinocchio/joint-collection.hh>
32 #include <hpp/pinocchio/simple-device.hh>
33 #include <hpp/pinocchio/urdf/util.hh>
34 #include <pinocchio/parsers/sample-models.hpp>
35
36 namespace hpp {
37 namespace pinocchio {
38 template <typename Scalar, int Options,
39 template <typename, int> class JointCollectionTpl>
40 10 void humanoidRandom(
41 ::pinocchio::ModelTpl<Scalar, Options, JointCollectionTpl>& model) {
42 using ::pinocchio::buildModels::details::addJointAndBody;
43
4/8
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 6 times.
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
 10 static const SE3 Id = SE3::Identity();
44 typedef JointCollectionTpl<Scalar, Options> JC;
45
46 // root
47
4/8
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 10 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 10 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 10 times.
✗ Branch 13 not taken.
 10 addJointAndBody(model, typename JC::JointModelFreeFlyer(), "universe", "root",
48 Id);
49
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 10 model.lowerPositionLimit.template segment<4>(3).fill(-1.);
50
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 10 model.upperPositionLimit.template segment<4>(3).fill(1.);
51
52 // lleg
53
4/8
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
 10 addJointAndBody(model, typename JC::JointModelRX(), "root_joint", "lleg1");
54
4/8
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
 10 addJointAndBody(model, typename JC::JointModelRY(), "lleg1_joint", "lleg2");
55
4/8
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
 10 addJointAndBody(model, typename JC::JointModelRZ(), "lleg2_joint", "lleg3");
56
4/8
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
 10 addJointAndBody(model, typename JC::JointModelRY(), "lleg3_joint", "lleg4");
57
4/8
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
 10 addJointAndBody(model, typename JC::JointModelRY(), "lleg4_joint", "lleg5");
58
4/8
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
 10 addJointAndBody(model, typename JC::JointModelRX(), "lleg5_joint", "lleg6");
59
60 // rleg
61
4/8
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
 10 addJointAndBody(model, typename JC::JointModelRX(), "root_joint", "rleg1");
62
4/8
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
 10 addJointAndBody(model, typename JC::JointModelRY(), "rleg1_joint", "rleg2");
63
4/8
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
 10 addJointAndBody(model, typename JC::JointModelRZ(), "rleg2_joint", "rleg3");
64
4/8
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
 10 addJointAndBody(model, typename JC::JointModelRY(), "rleg3_joint", "rleg4");
65
4/8
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
 10 addJointAndBody(model, typename JC::JointModelRY(), "rleg4_joint", "rleg5");
66
4/8
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
 10 addJointAndBody(model, typename JC::JointModelRX(), "rleg5_joint", "rleg6");
67
68 // trunc
69
4/8
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
 10 addJointAndBody(model, typename JC::JointModelRY(), "root_joint", "torso1");
70
4/8
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
 10 addJointAndBody(model, typename JC::JointModelRZ(), "torso1_joint", "chest");
71
72 // rarm
73
4/8
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
 10 addJointAndBody(model, typename JC::JointModelRX(), "chest_joint", "rarm1");
74
4/8
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
 10 addJointAndBody(model, typename JC::JointModelRY(), "rarm1_joint", "rarm2");
75
4/8
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
 10 addJointAndBody(model, typename JC::JointModelRZ(), "rarm2_joint", "rarm3");
76
4/8
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
 10 addJointAndBody(model, typename JC::JointModelRY(), "rarm3_joint", "rarm4");
77
4/8
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
 10 addJointAndBody(model, typename JC::JointModelRY(), "rarm4_joint", "rarm5");
78
4/8
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
 10 addJointAndBody(model, typename JC::JointModelRX(), "rarm5_joint", "rarm6");
79
80 // larm
81
4/8
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
 10 addJointAndBody(model, typename JC::JointModelRX(), "chest_joint", "larm1");
82
4/8
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
 10 addJointAndBody(model, typename JC::JointModelRY(), "larm1_joint", "larm2");
83
4/8
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
 10 addJointAndBody(model, typename JC::JointModelRZ(), "larm2_joint", "larm3");
84
4/8
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
 10 addJointAndBody(model, typename JC::JointModelRY(), "larm3_joint", "larm4");
85
4/8
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
 10 addJointAndBody(model, typename JC::JointModelRY(), "larm4_joint", "larm5");
86
4/8
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
 10 addJointAndBody(model, typename JC::JointModelRX(), "larm5_joint", "larm6");
87 10 }
88
89 ✗ DevicePtr_t humanoidSimple(const std::string& name, bool usingFF,
90 Computation_t compFlags) {
91 ✗ if (!usingFF)
92 ✗ throw std::invalid_argument(
93 ✗ "Humanoid simple without freefloating base is not supported anymore.");
94 ✗ return humanoidSimple(name, compFlags);
95 }
96
97 10 DevicePtr_t humanoidSimple(const std::string& name, Computation_t compFlags) {
98
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 DevicePtr_t robot = Device::create(name);
99
1/2
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
 10 humanoidRandom(robot->model());
100
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 10 robot->createData();
101
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 10 robot->createGeomData();
102
3/6
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 10 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 10 times.
✗ Branch 10 not taken.
 10 robot->currentConfiguration(robot->neutralConfiguration());
103
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 10 robot->computeForwardKinematics(compFlags);
104
105 10 const value_type max = std::numeric_limits<value_type>::max();
106 10 const value_type min = -std::numeric_limits<value_type>::max();
107
2/4
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 10 times.
✗ Branch 7 not taken.
 10 robot->model().lowerPositionLimit.segment<3>(0).setConstant(min);
108
2/4
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 10 times.
✗ Branch 7 not taken.
 10 robot->model().upperPositionLimit.segment<3>(0).setConstant(max);
109
2/4
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 10 times.
✗ Branch 7 not taken.
 10 robot->model().lowerPositionLimit.segment<4>(3).setConstant(-1.01);
110
2/4
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 10 times.
✗ Branch 7 not taken.
 10 robot->model().upperPositionLimit.segment<4>(3).setConstant(1.01);
111 20 return robot;
112 }
113
114 namespace unittest {
115 27 DevicePtr_t makeDevice(TestDeviceType type, const std::string& prefix) {
116 27 DevicePtr_t robot;
117 27 HumanoidRobotPtr_t hrobot;
118 (void)prefix;
119
4/5
✓ Branch 0 taken 9 times.
✓ Branch 1 taken 6 times.
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
 27 switch (type) {
120 9 case CarLike:
121
2/4
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
 9 robot = Device::create("carlike");
122
7/14
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 9 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 9 times.
✗ Branch 14 not taken.
✓ Branch 17 taken 9 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 9 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 9 times.
✗ Branch 25 not taken.
 9 urdf::loadRobotModel(robot, 0, prefix, "planar", "hpp_environments",
123 "buggy", "", "");
124
2/4
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 9 times.
✗ Branch 7 not taken.
 9 robot->model().lowerPositionLimit.head<2>().setConstant(-1);
125
2/4
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 9 times.
✗ Branch 7 not taken.
 9 robot->model().upperPositionLimit.head<2>().setOnes();
126 9 return robot;
127 6 case ManipulatorArm2:
128
2/4
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
 6 robot = Device::create("arm");
129
5/10
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 6 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 6 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 6 times.
✗ Branch 17 not taken.
 6 urdf::loadModel(robot, 0, prefix, "anchor",
130 "package://example-robot-data/robots/baxter_description/"
131 "urdf/baxter.urdf",
132 "");
133 6 return robot;
134 2 case HumanoidRomeo:
135
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 2 hrobot = HumanoidRobot::create("romeo");
136
7/14
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✓ Branch 17 taken 2 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 2 times.
✗ Branch 22 not taken.
✓ Branch 25 taken 2 times.
✗ Branch 26 not taken.
 2 urdf::loadRobotModel(hrobot, 0, prefix, "freeflyer", "romeo_description",
137 "romeo", "_small", "_small");
138
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 urdf::setupHumanoidRobot(hrobot, prefix);
139
2/4
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
 2 hrobot->model().lowerPositionLimit.head<3>().setConstant(-1);
140
2/4
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
 2 hrobot->model().upperPositionLimit.head<3>().setOnes();
141 2 return hrobot;
142 10 case HumanoidSimple:
143
2/4
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
 20 robot = humanoidSimple("simple-humanoid",
144 10 (Computation_t)(JOINT_POSITION | JACOBIAN));
145
2/4
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 10 times.
✗ Branch 7 not taken.
 10 robot->model().lowerPositionLimit.head<3>().setConstant(-1);
146
2/4
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 10 times.
✗ Branch 7 not taken.
 10 robot->model().upperPositionLimit.head<3>().setOnes();
147 10 return robot;
148 ✗ default:
149 ✗ throw std::invalid_argument("Unknown robot type.");
150 }
151 27 }
152 } // namespace unittest
153 } // namespace pinocchio
154 } // namespace hpp
155