GCC Code Coverage Report

Directory: ./
File: include/hpp/pinocchio/device-sync.hh
Date: 2025-05-04 12:09:19
Exec Total Coverage
Lines: 26 42 61.9%
Branches: 8 24 33.3%
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_SYNC_HH
32 #define HPP_PINOCCHIO_DEVICE_SYNC_HH
33
34 #include <hpp/pinocchio/deprecated.hh>
35 #include <hpp/pinocchio/device-data.hh>
36 #include <hpp/pinocchio/fwd.hh>
37
38 namespace hpp {
39 namespace pinocchio {
40 /// Abstract class representing a Device.
41 class HPP_PINOCCHIO_DLLAPI AbstractDevice {
42 public:
43 /// \name Access to pinocchio API
44 /// \{
45
46 /// Access to pinocchio model
47 ModelConstPtr_t modelPtr() const { return model_; }
48 /// Access to pinocchio model
49 16292 ModelPtr_t modelPtr() { return model_; }
50 /// Access to pinocchio model
51 2291 const Model& model() const {
52
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2290 times.
 2291 assert(model_);
53 2290 return *model_;
54 }
55 /// Access to pinocchio model
56 163834 Model& model() {
57
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 163834 times.
 163834 assert(model_);
58 163834 return *model_;
59 }
60
61 /// Access to pinocchio geomModel
62 GeomModelConstPtr_t geomModelPtr() const { return geomModel_; }
63 /// Access to pinocchio geomModel
64 1000 GeomModelPtr_t geomModelPtr() { return geomModel_; }
65 /// Access to pinocchio geomModel
66 ✗ const GeomModel& geomModel() const {
67 ✗ assert(geomModel_);
68 ✗ return *geomModel_;
69 }
70 /// Access to pinocchio geomModel
71 152 GeomModel& geomModel() {
72
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 152 times.
 152 assert(geomModel_);
73 152 return *geomModel_;
74 }
75
76 /// Access to Pinocchio data/
77 DataConstPtr_t dataPtr() const { return d().data_; }
78 /// Access to Pinocchio data/
79 11773 DataPtr_t dataPtr() { return d().data_; }
80 /// Access to Pinocchio data/
81 ✗ const Data& data() const {
82 ✗ assert(d().data_);
83 ✗ return *d().data_;
84 }
85 /// Access to Pinocchio data/
86 3341 Data& data() {
87
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 3341 times.
 3341 assert(d().data_);
88 3341 return *d().data_;
89 }
90
91 /// Access to Pinocchio geomData/
92 GeomDataConstPtr_t geomDataPtr() const { return d().geomData_; }
93 /// Access to Pinocchio geomData/
94 81 GeomDataPtr_t geomDataPtr() { return d().geomData_; }
95 /// Access to Pinocchio geomData/
96 ✗ const GeomData& geomData() const {
97 ✗ assert(d().geomData_);
98 ✗ return *d().geomData_;
99 }
100 /// Access to Pinocchio geomData/
101 222 GeomData& geomData() {
102
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 222 times.
 222 assert(d().geomData_);
103 222 return *d().geomData_;
104 }
105
106 /// \}
107 // -----------------------------------------------------------------------
108 /// \name Current state
109 /// \{
110
111 /// Get current configuration
112 const Configuration_t& currentConfiguration() const {
113 return d().currentConfiguration_;
114 }
115 /// Set current configuration
116 /// \return True if the current configuration was modified and false if
117 /// the current configuration did not change.
118 virtual bool currentConfiguration(ConfigurationIn_t configuration);
119
120 /// Get current velocity
121 const vector_t& currentVelocity() const { return d().currentVelocity_; }
122
123 /// Set current velocity
124 bool currentVelocity(vectorIn_t velocity);
125
126 /// Get current acceleration
127 const vector_t& currentAcceleration() const {
128 return d().currentAcceleration_;
129 }
130
131 /// Set current acceleration
132 bool currentAcceleration(vectorIn_t acceleration);
133 /// \}
134 // -----------------------------------------------------------------------
135 /// \name Mass and center of mass
136 /// \{
137
138 /// Get mass of robot
139 const value_type& mass() const;
140
141 /// Get position of center of mass
142 const vector3_t& positionCenterOfMass() const;
143
144 /// Get Jacobian of center of mass with respect to configuration
145 const ComJacobian_t& jacobianCenterOfMass() const;
146
147 /// \}
148 // -----------------------------------------------------------------------
149 /// \name Forward kinematics
150 /// \{
151
152 /// \deprecated Use computeForwardKinematics(Computation_t) instead to select
153 /// what should be computed.
154 HPP_PINOCCHIO_DEPRECATED void controlComputation(const Computation_t&) {}
155 /// \deprecated returns COMPUTE_ALL
156 ✗ HPP_PINOCCHIO_DEPRECATED Computation_t computationFlag() const {
157 ✗ return COMPUTE_ALL;
158 }
159 /// Compute forward kinematics computing everything
160 void computeForwardKinematics() { computeForwardKinematics(COMPUTE_ALL); }
161 /// Compute forward kinematics with custom computation flag
162 /// \param flag to customise the computation. This should be a bitwise OR
163 /// between Computation_t values.
164 2012 void computeForwardKinematics(int flag) {
165
1/2
✓ Branch 3 taken 2012 times.
✗ Branch 4 not taken.
 2012 d().computeForwardKinematics(modelPtr(), flag);
166 2012 }
167 /// Compute frame forward kinematics
168 /// \note call AbstractDevice::computeForwardKinematics.
169 void computeFramesForwardKinematics() {
170 d().computeFramesForwardKinematics(modelPtr());
171 }
172 /// Update the geometry placement to the currentConfiguration
173 ✗ void updateGeometryPlacements() {
174 ✗ d().updateGeometryPlacements(modelPtr(), geomModelPtr());
175 }
176 /// \}
177 // -----------------------------------------------------------------------
178 protected:
179 AbstractDevice();
180 AbstractDevice(const ModelPtr_t& m, const GeomModelPtr_t& gm);
181
182 virtual DeviceData& d() = 0;
183 virtual DeviceData const& d() const = 0;
184
185 // Pinocchio objects
186 ModelPtr_t model_;
187 GeomModelPtr_t geomModel_;
188 }; // class AbstractDevice
189
190 /// A thread-safe access to a Device.
191 ///
192 /// To ensure thread safety, one can do
193 /// \code{cpp}
194 /// // In some place of the code:
195 /// DevicePtr_t device = ...;
196 /// device->numberDeviceData (4);
197 ///
198 /// // Acquires a lock on the device.
199 /// DeviceSync deviceSync (device);
200 /// deviceSync.currentConfiguration(q);
201 /// deviceSync.computeForwardKinematics(JOINT_POSITION | JACOBIAN);
202 ///
203 /// JointPtr_t joint = ...;
204 /// joint->currentTransformation (deviceSync.d());
205 ///
206 /// CollisionObjectPtr_t body = ...;
207 /// body->fcl (deviceSync.d());
208 /// body->getTransform (deviceSync.d());
209 ///
210 /// // The lock is release when deviceSync is destroyed.
211 /// \endcode
212 class HPP_PINOCCHIO_DLLAPI DeviceSync : public AbstractDevice {
213 public:
214 /// Constructor
215 /// \param device to lock
216 /// \param lock whether to acquire the lock.
217 DeviceSync(const DevicePtr_t& device, bool lock = true);
218
219 /// Destructor.
220 /// The lock is released if necessary.
221 virtual ~DeviceSync();
222
223 /// Accessor to the locked DeviceData.
224 /// \note this asserts that this isLocked()
225 3965 DeviceData& d() {
226
2/2
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 3982 times.
 3965 assert(isLocked());
227 3982 return *d_;
228 }
229 /// Const accessor to the locked DeviceData.
230 /// \note this asserts that this isLocked()
231 ✗ DeviceData const& d() const {
232 ✗ assert(isLocked());
233 ✗ return *d_;
234 }
235
236 /// Lock the current DeviceData
237 void lock();
238 /// Check if the current DeviceData is locked
239 6950 bool isLocked() const { return d_ != NULL; }
240 /// Unlock the current DeviceData
241 void unlock();
242
243 private:
244 DevicePtr_t device_;
245 DeviceData* d_;
246 }; // class DeviceSync
247 } // namespace pinocchio
248 } // namespace hpp
249
250 #endif // HPP_PINOCCHIO_DEVICE_SYNC_HH
251