transmission_interface.h
Go to the documentation of this file.
1 // Copyright (C) 2013, PAL Robotics S.L.
3 //
4 // Redistribution and use in source and binary forms, with or without
5 // modification, are permitted provided that the following conditions are met:
6 // * Redistributions of source code must retain the above copyright notice,
7 // this list of conditions and the following disclaimer.
8 // * Redistributions in binary form must reproduce the above copyright
9 // notice, this list of conditions and the following disclaimer in the
10 // documentation and/or other materials provided with the distribution.
11 // * Neither the name of PAL Robotics S.L. nor the names of its
12 // contributors may be used to endorse or promote products derived from
13 // this software without specific prior written permission.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
19 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
20 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
21 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
22 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
23 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
24 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
25 // POSSIBILITY OF SUCH DAMAGE.
27 
29 
30 #pragma once
31 
32 
33 #include <map>
34 #include <string>
35 #include <vector>
36 
40 
41 namespace transmission_interface
42 {
43 
49 {
50 public:
52  std::string getName() const {return name_;}
53 
54 protected:
55  std::string name_;
59 
70  TransmissionHandle(const std::string& name,
71  Transmission* transmission,
72  const ActuatorData& actuator_data,
73  const JointData& joint_data)
74  : name_(name),
75  transmission_(transmission),
76  actuator_data_(actuator_data),
77  joint_data_(joint_data)
78  {
79  // Precondition: Valid transmission
80  if (!transmission_)
81  {
82  throw TransmissionInterfaceException("Unspecified transmission.");
83  }
84 
85  // Catch trivial error: All data vectors are empty (handle can't do anything without data)
86  if (actuator_data.position.empty() && actuator_data.velocity.empty() && actuator_data.effort.empty() &&
87  joint_data.position.empty() && joint_data.velocity.empty() && joint_data.effort.empty())
88  {
89  throw TransmissionInterfaceException("All data vectors are empty. Transmission instance can't do anything!.");
90  }
91 
92  // Precondition: All non-empty data vectors must have sizes consistent with the transmission
93  if (!actuator_data.position.empty() && actuator_data.position.size() != transmission_->numActuators())
94  {
95  throw TransmissionInterfaceException("Actuator position data size does not match transmission.");
96  }
97  if (!actuator_data.velocity.empty() && actuator_data.velocity.size() != transmission_->numActuators())
98  {
99  throw TransmissionInterfaceException("Actuator velocity data size does not match transmission.");
100  }
101  if (!actuator_data.effort.empty() && actuator_data.effort.size() != transmission_->numActuators())
102  {
103  throw TransmissionInterfaceException("Actuator effort data size does not match transmission.");
104  }
105  if (!actuator_data.absolute_position.empty() && actuator_data.absolute_position.size() != transmission_->numActuators())
106  {
107  throw TransmissionInterfaceException("Actuator absolute position data size does not match transmission.");
108  }
109  if (!actuator_data.absolute_position.empty() && actuator_data.absolute_position.size() != transmission_->numActuators())
110  {
111  throw TransmissionInterfaceException("Actuator torque sensor data size does not match transmission.");
112  }
113 
114  if (!joint_data.position.empty() && joint_data.position.size() != transmission_->numJoints())
115  {
116  throw TransmissionInterfaceException("Joint position data size does not match transmission.");
117  }
118  if (!joint_data.velocity.empty() && joint_data.velocity.size() != transmission_->numJoints())
119  {
120  throw TransmissionInterfaceException("Joint velocity data size does not match transmission.");
121  }
122  if (!joint_data.effort.empty() && joint_data.effort.size() != transmission_->numJoints())
123  {
124  throw TransmissionInterfaceException("Joint effort data size does not match transmission.");
125  }
126  if (!joint_data.absolute_position.empty() && joint_data.absolute_position.size() != transmission_->numJoints())
127  {
128  throw TransmissionInterfaceException("Joint absolute position data size does not match transmission.");
129  }
130  if (!joint_data.torque_sensor.empty() && joint_data.torque_sensor.size() != transmission_->numJoints())
131  {
132  throw TransmissionInterfaceException("Joint torque sensor data size does not match transmission.");
133  }
134 
135  // Precondition: Valid pointers to raw data
136  if (!hasValidPointers(actuator_data.position))
137  {
138  throw TransmissionInterfaceException("Actuator position data contains null pointers.");
139  }
140  if (!hasValidPointers(actuator_data.velocity))
141  {
142  throw TransmissionInterfaceException("Actuator velocity data contains null pointers.");
143  }
144  if (!hasValidPointers(actuator_data.effort))
145  {
146  throw TransmissionInterfaceException("Actuator effort data contains null pointers.");
147  }
148  if (!actuator_data.absolute_position.empty() && !hasValidPointers(actuator_data.absolute_position))
149  {
150  throw TransmissionInterfaceException("Actuator absolute position data contains null pointers.");
151  }
152  if (!actuator_data.torque_sensor.empty() && !hasValidPointers(actuator_data.torque_sensor))
153  {
154  throw TransmissionInterfaceException("Actuator torque sensor data contains null pointers.");
155  }
156 
157  if (!hasValidPointers(joint_data.position))
158  {
159  throw TransmissionInterfaceException("Joint position data contains null pointers.");
160  }
161  if (!hasValidPointers(joint_data.velocity))
162  {
163  throw TransmissionInterfaceException("Joint velocity data contains null pointers.");
164  }
165  if (!hasValidPointers(joint_data.effort))
166  {
167  throw TransmissionInterfaceException("Joint effort data contains null pointers.");
168  }
169  if (!joint_data.absolute_position.empty() && !hasValidPointers(joint_data.absolute_position))
170  {
171  throw TransmissionInterfaceException("Joint absolute position data contains null pointers.");
172  }
173  if (!joint_data.torque_sensor.empty() && !hasValidPointers(joint_data.torque_sensor))
174  {
175  throw TransmissionInterfaceException("Joint torque sensor data contains null pointers.");
176  }
177 
178  }
179 
180 private:
181  static bool hasValidPointers(const std::vector<double*>& data)
182  {
183  for (const auto& ptr : data)
184  {
185  if (!ptr) {return false;}
186  }
187  return true;
188  }
189 };
190 
195 {
196 public:
198  ActuatorToJointStateHandle(const std::string& name,
199  Transmission* transmission,
200  const ActuatorData& actuator_data,
201  const JointData& joint_data)
202  : TransmissionHandle(name, transmission, actuator_data, joint_data) {}
203 
207  void propagate()
208  {
212 
214  {
216  }
217 
219  {
221  }
222  }
223  /*\}*/
224 };
225 
226 
229 {
230 public:
232  ActuatorToJointPositionHandle(const std::string& name,
233  Transmission* transmission,
234  const ActuatorData& actuator_data,
235  const JointData& joint_data)
236  : TransmissionHandle(name, transmission, actuator_data, joint_data) {}
237 
242  /*\}*/
243 };
244 
245 
248 {
249 public:
251  ActuatorToJointVelocityHandle(const std::string& name,
252  Transmission* transmission,
253  const ActuatorData& actuator_data,
254  const JointData& joint_data)
255  : TransmissionHandle(name, transmission, actuator_data, joint_data) {}
256 
261  /*\}*/
262 };
263 
264 
267 {
268 public:
270  ActuatorToJointEffortHandle(const std::string& name,
271  Transmission* transmission,
272  const ActuatorData& actuator_data,
273  const JointData& joint_data)
274  : TransmissionHandle(name, transmission, actuator_data, joint_data) {}
275 
280  /*\}*/
281 };
282 
283 
288 {
289 public:
291  JointToActuatorStateHandle(const std::string& name,
292  Transmission* transmission,
293  const ActuatorData& actuator_data,
294  const JointData& joint_data)
295  : TransmissionHandle(name, transmission, actuator_data, joint_data) {}
296 
300  void propagate()
301  {
305  }
306  /*\}*/
307 };
308 
309 
312 {
313 public:
315  JointToActuatorPositionHandle(const std::string& name,
316  Transmission* transmission,
317  const ActuatorData& actuator_data,
318  const JointData& joint_data)
319  : TransmissionHandle(name, transmission, actuator_data, joint_data) {}
320 
325  /*\}*/
326 };
327 
328 
331 {
332 public:
334  JointToActuatorVelocityHandle(const std::string& name,
335  Transmission* transmission,
336  const ActuatorData& actuator_data,
337  const JointData& joint_data)
338  : TransmissionHandle(name, transmission, actuator_data, joint_data) {}
339 
344  /*\}*/
345 };
346 
347 
350 {
351 public:
353  JointToActuatorEffortHandle(const std::string& name,
354  Transmission* transmission,
355  const ActuatorData& actuator_data,
356  const JointData& joint_data)
357  : TransmissionHandle(name, transmission, actuator_data, joint_data) {}
358 
363  /*\}*/
364 };
365 
383 template <class HandleType>
385 {
386 public:
387 
388  HandleType getHandle(const std::string& name)
389  {
390  // Rethrow exception with a meaningful type
391  try
392  {
394  }
395  catch(const std::logic_error& e)
396  {
397  throw TransmissionInterfaceException(e.what());
398  }
399  }
400 
404  void propagate()
405  {
406  for (auto&& resource_name_and_handle : this->resource_map_)
407  {
408  resource_name_and_handle.second.propagate();
409  }
410  }
411  /*\}*/
412 };
413 
414 // Convenience typedefs
415 
417 class ActuatorToJointStateInterface : public TransmissionInterface<ActuatorToJointStateHandle> {};
418 
420 class ActuatorToJointPositionInterface : public TransmissionInterface<ActuatorToJointPositionHandle> {};
421 
423 class ActuatorToJointVelocityInterface : public TransmissionInterface<ActuatorToJointVelocityHandle> {};
424 
426 class ActuatorToJointEffortInterface : public TransmissionInterface<ActuatorToJointEffortHandle> {};
427 
429 class JointToActuatorStateInterface : public TransmissionInterface<JointToActuatorStateHandle> {};
430 
432 class JointToActuatorPositionInterface : public TransmissionInterface<JointToActuatorPositionHandle> {};
433 
435 class JointToActuatorVelocityInterface : public TransmissionInterface<JointToActuatorVelocityHandle> {};
436 
438 class JointToActuatorEffortInterface : public TransmissionInterface<JointToActuatorEffortHandle> {};
439 
440 } // transmission_interface
virtual void jointToActuatorVelocity(const JointData &jnt_data, ActuatorData &act_data)=0
Transform velocity variables from joint to actuator space.
ActuatorToJointEffortHandle(const std::string &name, Transmission *transmission, const ActuatorData &actuator_data, const JointData &joint_data)
void propagate()
Propagate joint velocities to actuator velocities for the stored transmission.
Handle for propagating actuator efforts to joint efforts for a given transmission.
virtual std::size_t numJoints() const =0
std::vector< double * > absolute_position
Definition: transmission.h:68
std::vector< double * > velocity
Definition: transmission.h:66
virtual void actuatorToJointPosition(const ActuatorData &act_data, JointData &jnt_data)=0
Transform position variables from actuator to joint space.
virtual void actuatorToJointTorqueSensor(const ActuatorData &, JointData &)
Definition: transmission.h:135
std::vector< double * > absolute_position
Definition: transmission.h:55
void propagate()
Propagate the transmission maps of all managed handles.
Interface for propagating a given map on a set of transmissions.
ActuatorToJointStateHandle(const std::string &name, Transmission *transmission, const ActuatorData &actuator_data, const JointData &joint_data)
Contains pointers to raw data representing the position, velocity and acceleration of a transmission&#39;...
Definition: transmission.h:63
std::vector< double * > position
Definition: transmission.h:52
virtual void actuatorToJointEffort(const ActuatorData &act_data, JointData &jnt_data)=0
Transform effort variables from actuator to joint space.
virtual void actuatorToJointAbsolutePosition(const ActuatorData &, JointData &)
Definition: transmission.h:129
std::vector< double * > torque_sensor
Definition: transmission.h:69
Handle for propagating actuator positions to joint positions for a given transmission.
void propagate()
Propagate actuator positions to joint positions for the stored transmission.
JointToActuatorStateHandle(const std::string &name, Transmission *transmission, const ActuatorData &actuator_data, const JointData &joint_data)
TransmissionHandle(const std::string &name, Transmission *transmission, const ActuatorData &actuator_data, const JointData &joint_data)
std::vector< double * > torque_sensor
Definition: transmission.h:56
JointToActuatorVelocityHandle(const std::string &name, Transmission *transmission, const ActuatorData &actuator_data, const JointData &joint_data)
void propagate()
Propagate actuator efforts to joint efforts for the stored transmission.
ActuatorToJointPositionHandle(const std::string &name, Transmission *transmission, const ActuatorData &actuator_data, const JointData &joint_data)
Handle for propagating actuator velocities to joint velocities for a given transmission.
JointToActuatorEffortHandle(const std::string &name, Transmission *transmission, const ActuatorData &actuator_data, const JointData &joint_data)
ResourceHandle getHandle(const std::string &name)
Handle for propagating actuator state (position, velocity and effort) to joint state for a given tran...
Handle for propagating joint efforts to actuator efforts for a given transmission.
static bool hasValidPointers(const std::vector< double *> &data)
Handle for propagating joint velocities to actuator velocities for a given transmission.
virtual bool hasActuatorToJointTorqueSensor() const
Definition: transmission.h:142
void propagate()
Propagate joint efforts to actuator efforts for the stored transmission.
Contains pointers to raw data representing the position, velocity and acceleration of a transmission&#39;...
Definition: transmission.h:50
virtual bool hasActuatorToJointAbsolutePosition() const
Definition: transmission.h:141
virtual void jointToActuatorPosition(const JointData &jnt_data, ActuatorData &act_data)=0
Transform position variables from joint to actuator space.
void propagate()
Propagate actuator velocities to joint velocities for the stored transmission.
std::vector< double * > velocity
Definition: transmission.h:53
ActuatorToJointVelocityHandle(const std::string &name, Transmission *transmission, const ActuatorData &actuator_data, const JointData &joint_data)
std::vector< double * > position
Definition: transmission.h:65
virtual void jointToActuatorEffort(const JointData &jnt_data, ActuatorData &act_data)=0
Transform effort variables from joint to actuator space.
JointToActuatorPositionHandle(const std::string &name, Transmission *transmission, const ActuatorData &actuator_data, const JointData &joint_data)
std::vector< double * > effort
Definition: transmission.h:67
void propagate()
Propagate joint positions to actuator positions for the stored transmission.
virtual std::size_t numActuators() const =0
Handle for propagating a single map (position, velocity, or effort) on a single transmission (eg...
std::vector< double * > effort
Definition: transmission.h:54
virtual void actuatorToJointVelocity(const ActuatorData &act_data, JointData &jnt_data)=0
Transform velocity variables from actuator to joint space.
void propagate()
Propagate joint state to actuator state for the stored transmission.
Handle for propagating joint positions to actuator positions for a given transmission.
Abstract base class for representing mechanical transmissions.
Definition: transmission.h:91
Handle for propagating joint state (position, velocity and effort) to actuator state for a given tran...
HandleType getHandle(const std::string &name)
void propagate()
Propagate actuator state to joint state for the stored transmission.


transmission_interface
Author(s): Adolfo Rodriguez Tsouroukdissian
autogenerated on Sun May 10 2020 03:14:56