GCC Code Coverage Report

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2016 CNRS
3			// Author: NMansard from Florent Lamiraux
4			//
5			//
6
7			// Redistribution and use in source and binary forms, with or without
8			// modification, are permitted provided that the following conditions are
9			// met:
10			//
11			// 1. Redistributions of source code must retain the above copyright
12			// notice, this list of conditions and the following disclaimer.
13			//
14			// 2. Redistributions in binary form must reproduce the above copyright
15			// notice, this list of conditions and the following disclaimer in the
16			// documentation and/or other materials provided with the distribution.
17			//
18			// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
19			// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
20			// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
21			// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
22			// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
23			// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
24			// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
25			// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
26			// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
27			// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
28			// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
29			// DAMAGE.
30
31			#ifndef HPP_PINOCCHIO_DEVICE_HH
32			#define HPP_PINOCCHIO_DEVICE_HH
33
34			#include <boost/thread/condition_variable.hpp>
35			#include <boost/thread/mutex.hpp>
36			#include <hpp/pinocchio/config.hh>
37			#include <hpp/pinocchio/deprecated.hh>
38			#include <hpp/pinocchio/device-data.hh>
39			#include <hpp/pinocchio/device-sync.hh>
40			#include <hpp/pinocchio/extra-config-space.hh>
41			#include <hpp/pinocchio/frame.hh>
42			#include <hpp/pinocchio/fwd.hh>
43			#include <hpp/pinocchio/pool.hh>
44			#include <hpp/util/debug.hh>
45			#include <hpp/util/serialization-fwd.hh>
46			#include <iostream>
47			#include <list>
48			#include <vector>
49
50			namespace hpp {
51			namespace pinocchio {
52
53			/// \brief Robot with geometric and dynamic pinocchio
54
55			/// The creation of the device is done by Device::create(const
56			/// std::string name). This function returns a shared pointer
57			/// to the newly created object. \sa Smart pointers
58			/// documentation:
59			/// http://www.boost.org/libs/smart_ptr/smart_ptr.htm
60			class HPP_PINOCCHIO_DLLAPI Device : public AbstractDevice {
61			friend class Joint;
62			friend class Frame;
63			friend class DeviceSync;
64			friend class CollisionObject;
65			friend class CenterOfMassComputation;
66
67			public:
68			/// Collision pairs between bodies
69			typedef std::pair<JointPtr_t, JointPtr_t> CollisionPair_t;
70			typedef std::list<CollisionPair_t> CollisionPairs_t;
71
72			// -----------------------------------------------------------------------
73			/// \name Construction, copy and destruction
74			/// \{
75			virtual ~Device();
76
77			/// \brief Clone as a CkwsDevice
78			/// The pinocchio model is not copied (only copy the pointer).
79			/// A new Pinocchio "data" is created.
80			/// As the model is not copied, cloning is a non constant operation. \sa
81			/// cloneConst
82		✗	virtual DevicePtr_t clone() const { return createCopy(weakPtr_.lock()); }
83			/// \brief Clone as a CkwsDevice
84			/// Both pinocchio objects model and data are copied.
85			/// TODO: this method is not implemented yet (assert if called)
86			DevicePtr_t cloneConst() const { return createCopyConst(weakPtr_.lock()); }
87
88		17	DevicePtr_t self() const { return weakPtr_.lock(); }
89
90			/// Get name of device
91		48	const std::string& name() const { return name_; }
92
93			/// \brief Creation of a new device
94			/// \return a shared pointer to the new device
95			/// \param name Name of the device
96			static DevicePtr_t create(const std::string& name);
97
98			/// \brief Copy of a device
99			/// \return A shared pointer to new device.
100			/// \param device Device to be copied.
101			/// The pinocchio model is not copied (only copy the pointer).
102			/// A new Pinocchio "data" is created.
103			static DevicePtr_t createCopy(const DevicePtr_t& device);
104			static DevicePtr_t createCopyConst(const DeviceConstPtr_t& device);
105
106			/// \}
107			// -----------------------------------------------------------------------
108			/// \name Set pinocchio models and datas
109			/// \{
110
111			/// Set pinocchio model.
112		19	void setModel(ModelPtr_t modelPtr) { model_ = modelPtr; }
113
114			/// Set pinocchio geom.
115		19	void setGeomModel(GeomModelPtr_t geomModelPtr) { geomModel_ = geomModelPtr; }
116
117			/// Set Pinocchio data corresponding to model
118			void setData(DataPtr_t dataPtr) {
119			d().data_ = dataPtr;
120			resizeState();
121			}
122			/// Create Pinocchio data from model.
123			void createData();
124
125			/// Set Pinocchio geomData corresponding to model
126			void setGeomData(GeomDataPtr_t geomDataPtr) {
127			d().geomData_ = geomDataPtr;
128			resizeState();
129			}
130			/// Create Pinocchio geomData from model.
131			void createGeomData();
132
133			/// Remove some joints from the configuration space.
134			virtual void removeJoints(const std::vector<std::string>& jointNames,
135			Configuration_t referenceConfig);
136
137			/// \}
138			// -----------------------------------------------------------------------
139			/// \name Joints
140			/// \{
141
142			/// This struct defines a linear relation between two joint values.
143			///
144			/// The configuration of \c JointConstraint::joint is expected to be \f$
145			/// q_{joint} = multiplier * q_{reference} + offset\f$.
146			/// where \f$q_{reference}\f$ is the configuration of
147			/// JointLinearConstraint::reference joint.
148			struct JointLinearConstraint {
149			value_type offset, multiplier;
150			JointPtr_t joint, reference;
151			};
152
153			/// Get root joint
154			JointPtr_t rootJoint() const;
155
156			/// Get root frame
157			Frame rootFrame() const;
158
159			/// Get number of joints
160			/// \note "universe" is not included in the count
161			size_type nbJoints() const;
162
163			/// Access i-th joint
164			/// \param i index of joint in device. This is 1 less than index of joint
165			/// in pinocchio vector of joints (pinocchio::ModelTpl::joints)
166			/// since "universe" is not included as index 0
167			JointPtr_t jointAt(const size_type& i) const;
168
169			/// Get the joint at configuration rank r
170			/// \return joint j such that j->rankInConfiguration () <=
171			/// r < j->rankInConfiguration () + j->configSize ()
172			JointPtr_t getJointAtConfigRank(const size_type& r) const;
173
174			/// Get the joint at velocity rank r
175			/// \return joint j such that j->rankInVelocity () <=
176			/// r < j->rankInVelocity () + j->numberDof ()
177			JointPtr_t getJointAtVelocityRank(const size_type& r) const;
178
179			/// Get joint by name
180			/// \param name name of the joint.
181			/// \throw runtime_error if device has no joint with this name
182			JointPtr_t getJointByName(const std::string& name) const;
183
184			/// Get joint by body name
185			/// \throw runtime_error if device has no body with this name
186			JointPtr_t getJointByBodyName(const std::string& name) const;
187
188			/// Get frame by name
189			/// \param name name of the frame.
190			/// \throw runtime_error if device has no frame with this name
191			Frame getFrameByName(const std::string& name) const;
192
193			/// Size of configuration vectors
194			/// Sum of joint dimensions and of extra configuration space dimension
195			size_type configSize() const;
196
197			/// Size of velocity vectors
198			/// Sum of joint number of degrees of freedom and of extra configuration
199			/// space dimension
200			size_type numberDof() const;
201
202			/// Returns a LiegroupSpace representing the configuration space.
203		9	const LiegroupSpacePtr_t& configSpace() const { return configSpace_; }
204
205			/// See Joint::RnxSOnConfigurationSpace
206		1	const LiegroupSpacePtr_t& RnxSOnConfigSpace() const {
207		1	return configSpaceRnxSOn_;
208			}
209
210			/// Get the neutral configuration
211			Configuration_t neutralConfiguration() const;
212
213			/// Add a joint constraint
214			void addJointConstraint(JointLinearConstraint constraint);
215
216		3	const std::vector<JointLinearConstraint>& jointConstraints() const {
217		3	return jointConstraints_;
218			}
219
220			/// \}
221			// -----------------------------------------------------------------------
222			/// \name Extra configuration space
223			/// \{
224
225			/// Get degrees of freedom to store internal values in configurations
226			///
227			/// In some applications, it is useful to store extra variables with
228			/// the configuration vector. For instance, when planning motions in
229			/// state space using roadmap based methods, the velocity of the robot
230			/// is stored in the nodes of the roadmap.
231		72382	ExtraConfigSpace& extraConfigSpace() { return extraConfigSpace_; }
232
233			/// Get degrees of freedom to store internal values in configurations
234			///
235			/// In some applications, it is useful to store extra variables with
236			/// the configuration vector. For instance, when planning motions in
237			/// state space using roadmap based methods, the velocity of the robot
238			/// is stored in the nodes of the roadmap.
239			const ExtraConfigSpace& extraConfigSpace() const { return extraConfigSpace_; }
240
241			/// Set dimension of extra configuration space
242		2	virtual void setDimensionExtraConfigSpace(const size_type& dimension) {
243		2	extraConfigSpace_.setDimension(dimension);
244		2	resizeState();
245		2	}
246
247			/// \}
248			// -----------------------------------------------------------------------
249			/// \name Grippers
250			/// \{
251
252			/// Add a gripper to the Device
253			void addGripper(const GripperPtr_t& gripper) { grippers_.push_back(gripper); }
254
255			/// Return list of grippers of the Device
256			Grippers_t& grippers() { return grippers_; }
257
258			/// Return list of grippers of the Device
259			const Grippers_t& grippers() const { return grippers_; }
260
261			/// \}
262			// -----------------------------------------------------------------------
263			/// \name Collision and distance computation
264			/// \{
265
266			/// Get the static obstacles
267			/// It corresponds to the geometries attached to the universe in
268			/// pinocchio.
269			BodyPtr_t obstacles() const;
270
271			/// Number of objects
272			size_type nbObjects() const;
273
274			/// Access the i-th object.
275			/// \sa Device::nbObjects() const
276			CollisionObjectPtr_t objectAt(const size_type& i) const;
277
278			/// Test collision of current configuration
279			/// \param stopAtFirstCollision act as named
280			/// \warning Users should call computeForwardKinematics first.
281			bool collisionTest(const bool stopAtFirstCollision = true);
282
283			/// Compute distances between pairs of objects stored in bodies
284			/// \warning Users should call computeForwardKinematics first.
285			void computeDistances();
286
287			/// Get result of distance computations
288			const DistanceResults_t& distanceResults() const;
289			/// \}
290			// -----------------------------------------------------------------------
291			/// \name Multithreading
292			/// See DeviceSync for how to enable thread safety.
293			/// \{
294
295			/// Set the maximum number of concurrent use of the Device.
296			void numberDeviceData(const size_type& s);
297
298			/// Get the number of DeviceData.
299			size_type numberDeviceData() const;
300
301			/// \}
302			// -----------------------------------------------------------------------
303
304			/// Print object in a stream
305			virtual std::ostream& print(std::ostream& os) const;
306
307			/// Compute an aligned bounding around the robot.
308			/// The bounding box is computed as follows:
309			/// - for each direct children of the universe joint:
310			/// - compute a bounding box of its center (using the bounds in translation)
311			/// - compute the maximal distance between the its center and
312			/// each bodies attach its subtree.
313			/// - sum the two.
314			/// - sum all the BB obtained.
315			coal::AABB computeAABB() const;
316
317			protected:
318			/// \brief Constructor
319			Device(const std::string& name);
320
321			/// \brief Initialization.
322			///
323			void init(const DeviceWkPtr_t& weakPtr);
324			/// \brief Initialization of of a clone device.
325			///
326			void initCopy(const DeviceWkPtr_t& weakPtr, const Device& other);
327
328			/// \brief Copy Constructor
329			Device(const Device& device);
330
331			private:
332			/// Resize configuration when changing data or extra-config.
333			void resizeState();
334
335			protected:
336			DeviceData d_;
337
338		19124	DeviceData& d() { return d_; }
339		✗	DeviceData const& d() const { return d_; }
340
341		32	inline void invalidate() { d_.invalidate(); }
342
343			std::string name_;
344			// Grippers
345			Grippers_t grippers_;
346			LiegroupSpacePtr_t configSpace_, configSpaceRnxSOn_;
347			// Extra configuration space
348			ExtraConfigSpace extraConfigSpace_;
349			// Joint linear constraints
350			std::vector<JointLinearConstraint> jointConstraints_;
351			DeviceWkPtr_t weakPtr_;
352
353			private:
354			Pool<DeviceData> datas_;
355
356			protected:
357	3/6 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken. ✓ Branch 9 taken 1 times. ✗ Branch 10 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken.	1	Device() {}
358
359			private:
360		4	HPP_SERIALIZABLE_SPLIT();
361			}; // class Device
362
363			void replaceGeometryByConvexHull(GeomModel& geomModel,
364			const std::vector<std::string>& geometryNames);
365
366		✗	inline std::ostream& operator<<(std::ostream& os,
367			const hpp::pinocchio::Device& device) {
368		✗	return device.print(os);
369			}
370
371			} // namespace pinocchio
372			} // namespace hpp
373
374		9	BOOST_CLASS_EXPORT_KEY(hpp::pinocchio::Device)
375
376			#endif // HPP_PINOCCHIO_DEVICE_HH
377