GCC Code Coverage Report


Directory: src/
File: src/robot.impl.cc
Date: 2024-12-13 15:43:02
Exec Total Coverage
Lines: 0 256 0.0%
Branches: 0 774 0.0%

Line Branch Exec Source
1 // Copyright (c) 2012 CNRS
2 // Author: Florent Lamiraux, Joseph Mirabel
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 "robot.impl.hh"
30
31 #include <hpp/corbaserver/manipulation/server.hh>
32 #include <hpp/manipulation/device.hh>
33 #include <hpp/manipulation/handle.hh>
34 #include <hpp/manipulation/srdf/util.hh>
35 #include <hpp/pinocchio/collision-object.hh>
36 #include <hpp/pinocchio/gripper.hh>
37 #include <hpp/pinocchio/humanoid-robot.hh>
38 #include <hpp/pinocchio/joint.hh>
39 #include <hpp/pinocchio/urdf/util.hh>
40 #include <hpp/util/debug.hh>
41 #include <hpp/util/exception-factory.hh>
42 #include <pinocchio/multibody/model.hpp>
43
44 #include "tools.hh"
45
46 namespace hpp {
47 namespace manipulation {
48 namespace impl {
49 namespace {
50 using core::Container;
51 using pinocchio::Gripper;
52
53 DevicePtr_t getOrCreateRobot(ProblemSolverPtr_t p,
54 const std::string& name = "Robot") {
55 DevicePtr_t r = p->robot();
56 if (r) return r;
57 pinocchio::DevicePtr_t robot(p->createRobot(name));
58 assert(HPP_DYNAMIC_PTR_CAST(Device, robot));
59 p->robot(robot);
60 return HPP_STATIC_PTR_CAST(Device, robot);
61 }
62
63 JointPtr_t getJointByBodyNameOrThrow(ProblemSolverPtr_t p,
64 const std::string& n) {
65 DevicePtr_t r = getRobotOrThrow(p);
66 JointPtr_t j = r->getJointByBodyName(n);
67 return j;
68 }
69
70 template <typename GripperOrHandle>
71 GripperOrHandle copy(const GripperOrHandle& in, const DevicePtr_t& device,
72 const std::string& p);
73
74 template <>
75 GripperPtr_t copy(const GripperPtr_t& in, const DevicePtr_t& device,
76 const std::string& p) {
77 Transform3s position = (in->joint() ? in->joint()->currentTransformation() *
78 in->objectPositionInJoint()
79 : in->objectPositionInJoint());
80
81 pinocchio::Model& model = device->model();
82 const std::string name = p + in->name();
83 if (model.existFrame(name))
84 throw std::invalid_argument("Could not add the gripper because a frame \'" +
85 name + "\" already exists.");
86 model.addFrame(::pinocchio::Frame(name, model.getJointId("universe"),
87 model.getFrameId("universe"), position,
88 ::pinocchio::OP_FRAME));
89
90 GripperPtr_t out = Gripper::create(name, device);
91 out->clearance(in->clearance());
92 return out;
93 }
94
95 template <>
96 HandlePtr_t copy(const HandlePtr_t& in, const DevicePtr_t& device,
97 const std::string& p) {
98 Transform3s position =
99 (in->joint() ? in->joint()->currentTransformation() * in->localPosition()
100 : in->localPosition());
101
102 HandlePtr_t out =
103 Handle::create(p + in->name(), position, device, JointPtr_t());
104 out->clearance(in->clearance());
105 return out;
106 }
107
108 template <typename Object>
109 void copy(const Container<Object>& from, Container<Object>& to,
110 const DevicePtr_t& d, const std::string& prefix) {
111 typedef Container<Object> Container_t;
112 for (typename Container_t::const_iterator it = from.map.begin();
113 it != from.map.end(); it++) {
114 Object obj = copy<Object>(it->second, d, prefix);
115 to.add(obj->name(), obj);
116 }
117 }
118 } // namespace
119
120 Robot::Robot() : server_(0x0) {}
121
122 ProblemSolverPtr_t Robot::problemSolver() { return server_->problemSolver(); }
123
124 void Robot::insertRobotModel(const char* robotName, const char* rootJointType,
125 const char* urdfName, const char* srdfName) {
126 insertRobotModelOnFrame(robotName, "universe", rootJointType, urdfName,
127 srdfName);
128 }
129
130 void Robot::insertRobotModelOnFrame(const char* robotName,
131 const char* frameName,
132 const char* rootJointType,
133 const char* urdfName,
134 const char* srdfName) {
135 try {
136 DevicePtr_t robot = getOrCreateRobot(problemSolver());
137 if (robot->robotFrames(robotName).size() > 0)
138 HPP_THROW(std::invalid_argument,
139 "A robot named " << robotName << " already exists");
140 if (!robot->model().existFrame(frameName))
141 HPP_THROW(std::invalid_argument, "No frame named " << frameName << ".");
142 pinocchio::FrameIndex frame = robot->model().getFrameId(frameName);
143 pinocchio::urdf::loadModel(robot, frame, robotName, rootJointType, urdfName,
144 srdfName);
145 if (!std::string(srdfName).empty()) {
146 srdf::loadModelFromFile(robot, robotName, srdfName);
147 }
148 problemSolver()->resetProblem();
149 } catch (const std::exception& exc) {
150 throw Error(exc.what());
151 }
152 }
153
154 void Robot::insertRobotModelFromString(const char* robotName,
155 const char* rootJointType,
156 const char* urdfString,
157 const char* srdfString) {
158 insertRobotModelOnFrameFromString(robotName, "universe", rootJointType,
159 urdfString, srdfString);
160 }
161
162 void Robot::insertRobotModelOnFrameFromString(const char* robotName,
163 const char* frameName,
164 const char* rootJointType,
165 const char* urdfString,
166 const char* srdfString) {
167 try {
168 DevicePtr_t robot = getOrCreateRobot(problemSolver());
169 if (robot->robotFrames(robotName).size() > 0)
170 HPP_THROW(std::invalid_argument,
171 "A robot named " << robotName << " already exists");
172 if (!robot->model().existFrame(frameName))
173 HPP_THROW(std::invalid_argument, "No frame named " << frameName << ".");
174 pinocchio::FrameIndex frame = robot->model().getFrameId(frameName);
175
176 pinocchio::urdf::loadModelFromString(robot, frame, robotName, rootJointType,
177 urdfString, srdfString);
178 srdf::loadModelFromXML(robot, robotName, srdfString);
179 problemSolver()->resetProblem();
180 } catch (const std::exception& exc) {
181 throw Error(exc.what());
182 }
183 }
184
185 void Robot::insertRobotSRDFModel(const char* robotName, const char* srdfPath) {
186 try {
187 DevicePtr_t robot = getOrCreateRobot(problemSolver());
188 srdf::loadModelFromFile(robot, robotName, srdfPath);
189 hpp::pinocchio::urdf::loadSRDFModel(robot, robotName, srdfPath);
190 problemSolver()->resetProblem();
191 } catch (const std::exception& exc) {
192 throw Error(exc.what());
193 }
194 }
195
196 void Robot::insertRobotSRDFModelFromString(const char* robotName,
197 const char* srdfString) {
198 try {
199 DevicePtr_t robot = getOrCreateRobot(problemSolver());
200 srdf::loadModelFromXML(robot, robotName, srdfString);
201 hpp::pinocchio::urdf::loadSRDFModelFromString(robot, robotName, srdfString);
202 problemSolver()->resetProblem();
203 } catch (const std::exception& exc) {
204 throw Error(exc.what());
205 }
206 }
207
208 void Robot::insertHumanoidModel(const char* robotName,
209 const char* rootJointType, const char* urdfName,
210 const char* srdfName) {
211 try {
212 DevicePtr_t robot = getOrCreateRobot(problemSolver());
213 if (robot->robotFrames(robotName).size() > 0)
214 HPP_THROW(std::invalid_argument,
215 "A robot named " << robotName << " already exists");
216 pinocchio::urdf::loadModel(robot, 0, robotName, rootJointType, urdfName,
217 srdfName);
218 pinocchio::urdf::setupHumanoidRobot(robot, robotName);
219 srdf::loadModelFromFile(robot, robotName, srdfName);
220 problemSolver()->resetProblem();
221 } catch (const std::exception& exc) {
222 throw Error(exc.what());
223 }
224 }
225
226 void Robot::insertHumanoidModelFromString(const char* robotName,
227 const char* rootJointType,
228 const char* urdfString,
229 const char* srdfString) {
230 try {
231 DevicePtr_t robot = getOrCreateRobot(problemSolver());
232 if (robot->robotFrames(robotName).size() > 0)
233 HPP_THROW(std::invalid_argument,
234 "A robot named " << robotName << " already exists");
235 pinocchio::urdf::loadModelFromString(robot, 0, robotName, rootJointType,
236 urdfString, srdfString);
237 pinocchio::urdf::setupHumanoidRobot(robot, robotName);
238 srdf::loadModelFromXML(robot, robotName, srdfString);
239 problemSolver()->resetProblem();
240 } catch (const std::exception& exc) {
241 throw Error(exc.what());
242 }
243 }
244
245 void Robot::loadEnvironmentModel(const char* urdfName, const char* srdfName,
246 const char* prefix) {
247 try {
248 DevicePtr_t robot = getRobotOrThrow(problemSolver());
249
250 std::string p(prefix);
251 DevicePtr_t object = Device::create(p);
252 pinocchio::urdf::loadModel(object, 0, "", "anchor", urdfName, "");
253 srdf::loadModelFromFile(object, "", srdfName);
254 object->computeForwardKinematics(pinocchio::JOINT_POSITION);
255 object->updateGeometryPlacements();
256
257 // Detach objects from joints
258 problemSolver()->addObstacle(object, true, true);
259
260 // Add contact shapes.
261 typedef core::Container<JointAndShapes_t>::Map_t ShapeMap;
262 const ShapeMap& m = object->jointAndShapes.map;
263 for (ShapeMap::const_iterator it = m.begin(); it != m.end(); it++) {
264 JointAndShapes_t shapes;
265 for (JointAndShapes_t::const_iterator itT = it->second.begin();
266 itT != it->second.end(); ++itT) {
267 Transform3s M(Transform3s::Identity());
268 if (itT->first) M = itT->first->currentTransformation();
269 Shape_t newShape(itT->second.size());
270 for (std::size_t i = 0; i < newShape.size(); ++i)
271 newShape[i] = M.act(itT->second[i]);
272 shapes.push_back(JointAndShape_t(JointPtr_t(), newShape));
273 }
274 problemSolver()->jointAndShapes.add(p + it->first, shapes);
275 }
276
277 copy(object->handles, robot->handles, robot, p);
278 copy(object->grippers, robot->grippers, robot, p);
279 problemSolver()->resetProblem();
280 } catch (const std::exception& exc) {
281 throw hpp::Error(exc.what());
282 }
283 }
284
285 void Robot::loadEnvironmentModelFromString(const char* urdfString,
286 const char* srdfString,
287 const char* prefix) {
288 try {
289 DevicePtr_t robot = getRobotOrThrow(problemSolver());
290
291 std::string p(prefix);
292 DevicePtr_t object = Device::create(p);
293 // TODO replace "" by p and remove `p +` in what follows
294 pinocchio::urdf::loadModelFromString(object, 0, "", "anchor", urdfString,
295 srdfString);
296 srdf::loadModelFromXML(object, "", srdfString);
297 object->computeForwardKinematics(hpp::pinocchio::JOINT_POSITION);
298 object->updateGeometryPlacements();
299
300 // Detach objects from joints
301 problemSolver()->addObstacle(object, true, true);
302
303 // Add contact shapes.
304 typedef core::Container<JointAndShapes_t>::Map_t ShapeMap;
305 const ShapeMap& m = object->jointAndShapes.map;
306 for (ShapeMap::const_iterator it = m.begin(); it != m.end(); it++) {
307 JointAndShapes_t shapes;
308 for (JointAndShapes_t::const_iterator itT = it->second.begin();
309 itT != it->second.end(); ++itT) {
310 Transform3s M(Transform3s::Identity());
311 if (itT->first) M = itT->first->currentTransformation();
312 Shape_t newShape(itT->second.size());
313 for (std::size_t i = 0; i < newShape.size(); ++i)
314 newShape[i] = M.act(itT->second[i]);
315 shapes.push_back(JointAndShape_t(JointPtr_t(), newShape));
316 }
317 problemSolver()->jointAndShapes.add(p + it->first, shapes);
318 }
319
320 copy(object->handles, robot->handles, robot, p);
321 copy(object->grippers, robot->grippers, robot, p);
322 problemSolver()->resetProblem();
323 } catch (const std::exception& exc) {
324 throw hpp::Error(exc.what());
325 }
326 }
327
328 Transform__slice* Robot::getRootJointPosition(const char* robotName) {
329 try {
330 DevicePtr_t robot = getRobotOrThrow(problemSolver());
331 std::string n(robotName);
332 FrameIndices_t frameIdx = robot->robotFrames(robotName);
333 if (frameIdx.size() == 0)
334 throw hpp::Error("Root of subtree with the provided prefix not found");
335 const pinocchio::Model& model = robot->model();
336 const ::pinocchio::Frame& rf = model.frames[frameIdx[0]];
337 double* res = new Transform_;
338 if (rf.type == ::pinocchio::JOINT)
339 Transform3sTohppTransform(model.jointPlacements[rf.parent], res);
340 else
341 Transform3sTohppTransform(rf.placement, res);
342 return res;
343 } catch (const std::exception& exc) {
344 throw Error(exc.what());
345 }
346 }
347
348 void Robot::setRootJointPosition(const char* robotName,
349 const ::hpp::Transform_ position) {
350 try {
351 DevicePtr_t robot = getRobotOrThrow(problemSolver());
352 std::string n(robotName);
353 Transform3s T;
354 hppTransformToTransform3s(position, T);
355 robot->setRobotRootPosition(n, T);
356 robot->computeForwardKinematics();
357 } catch (const std::exception& exc) {
358 throw Error(exc.what());
359 }
360 }
361
362 void Robot::addHandle(const char* linkName, const char* handleName,
363 const ::hpp::Transform_ localPosition, double clearance,
364 const ::hpp::boolSeq& mask) {
365 try {
366 DevicePtr_t robot = getRobotOrThrow(problemSolver());
367 JointPtr_t joint = getJointByBodyNameOrThrow(problemSolver(), linkName);
368 Transform3s T;
369
370 const ::pinocchio::Frame& linkFrame =
371 robot->model().frames[robot->model().getFrameId(std::string(linkName))];
372 assert(linkFrame.type == ::pinocchio::BODY);
373
374 pinocchio::JointIndex index(0);
375 std::string jointName("universe");
376 if (joint) {
377 index = joint->index();
378 jointName = joint->name();
379 }
380 hppTransformToTransform3s(localPosition, T);
381 HandlePtr_t handle =
382 Handle::create(handleName, linkFrame.placement * T, robot, joint);
383 handle->clearance(clearance);
384 handle->mask(corbaServer::boolSeqToVector(mask, 6));
385 robot->handles.add(handleName, handle);
386 assert(robot->model().existFrame(jointName));
387 FrameIndex previousFrame(robot->model().getFrameId(jointName));
388 robot->model().addFrame(::pinocchio::Frame(handleName, index, previousFrame,
389 linkFrame.placement * T,
390 ::pinocchio::OP_FRAME));
391 // Recreate pinocchio data after modifying model
392 robot->createData();
393 } catch (const std::exception& exc) {
394 throw Error(exc.what());
395 }
396 }
397
398 void Robot::addGripper(const char* linkName, const char* gripperName,
399 const ::hpp::Transform_ p, double clearance) {
400 try {
401 DevicePtr_t robot = getRobotOrThrow(problemSolver());
402 JointPtr_t joint = getJointByBodyNameOrThrow(problemSolver(), linkName);
403 Transform3s T;
404
405 const ::pinocchio::Frame& linkFrame =
406 robot->model().frames[robot->model().getFrameId(std::string(linkName))];
407 assert(linkFrame.type == ::pinocchio::BODY);
408
409 pinocchio::JointIndex index(0);
410 std::string jointName("universe");
411 if (joint) {
412 index = joint->index();
413 jointName = joint->name();
414 }
415 hppTransformToTransform3s(p, T);
416 assert(robot->model().existFrame(jointName));
417 FrameIndex previousFrame(robot->model().getFrameId(jointName));
418 robot->model().addFrame(
419 ::pinocchio::Frame(gripperName, index, previousFrame,
420 linkFrame.placement * T, ::pinocchio::OP_FRAME));
421 // Recreate pinocchio data after modifying model
422 robot->createData();
423 GripperPtr_t gripper = Gripper::create(gripperName, robot);
424 gripper->clearance(clearance);
425 robot->grippers.add(gripperName, gripper);
426 // hppDout (info, "add Gripper: " << *gripper);
427 } catch (const std::exception& exc) {
428 throw Error(exc.what());
429 }
430 }
431
432 char* Robot::getGripperPositionInJoint(const char* gripperName,
433 ::hpp::Transform__out position) {
434 try {
435 DevicePtr_t robot = getRobotOrThrow(problemSolver());
436 GripperPtr_t gripper = robot->grippers.get(gripperName);
437 if (!gripper) throw Error("This gripper does not exists.");
438 const Transform3s& t = gripper->objectPositionInJoint();
439 Transform3sTohppTransform(t, position);
440 if (gripper->joint())
441 return c_str(gripper->joint()->name());
442 else
443 return c_str("universe");
444 } catch (const std::exception& exc) {
445 throw Error(exc.what());
446 }
447 }
448
449 char* Robot::getHandlePositionInJoint(const char* handleName,
450 ::hpp::Transform__out position) {
451 try {
452 DevicePtr_t robot = getRobotOrThrow(problemSolver());
453 HandlePtr_t handle = robot->handles.get(handleName);
454 if (!handle) throw Error("This handle does not exists.");
455 const Transform3s& t = handle->localPosition();
456 Transform3sTohppTransform(t, position);
457 if (handle->joint())
458 return c_str(handle->joint()->name());
459 else
460 return c_str("universe");
461 } catch (const std::exception& exc) {
462 throw Error(exc.what());
463 }
464 }
465
466 void Robot::setHandlePositionInJoint(const char* handleName,
467 const ::hpp::Transform_ position) {
468 try {
469 DevicePtr_t robot = getRobotOrThrow(problemSolver());
470 HandlePtr_t handle = robot->handles.get(handleName);
471 std::string name_str(handleName);
472 if (!handle)
473 throw std::invalid_argument("Robot does not have any handle named " +
474 name_str);
475 Transform3s t;
476 hppTransformToTransform3s(position, t);
477 handle->localPosition(t);
478 } catch (const std::exception& exc) {
479 throw Error(exc.what());
480 }
481 }
482
483 } // namespace impl
484 } // namespace manipulation
485 } // namespace hpp
486