mia_hand_ros_control  rel 1.0.0
mia_transmission_interface.h
Go to the documentation of this file.
1 // Copyright (C) 2013, PAL Robotics S.L.
2 //
3 // Redistribution and use in source and binary forms, with or without
4 // modification, are permitted provided that the following conditions are met:
5 // * Redistributions of source code must retain the above copyright notice,
6 // this list of conditions and the following disclaimer.
7 // * Redistributions in binary form must reproduce the above copyright
8 // notice, this list of conditions and the following disclaimer in the
9 // documentation and/or other materials provided with the distribution.
10 // * Neither the name of PAL Robotics S.L. nor the names of its
11 // contributors may be used to endorse or promote products derived from
12 // this software without specific prior written permission.
13 //
14 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
15 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
16 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
17 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
18 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
19 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
20 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
21 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
22 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
23 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
24 // POSSIBILITY OF SUCH DAMAGE.
25 
26 /*********************************************************************************************************
27  Modifyed by Author: Prensilia srl
28  Desc: Transmission interfae of the MIA index finger
29 
30  version 1.0
31 
32 **********************************************************************************************************/
33 
34 
35 #ifndef TRANSMISSION_INTERFACE_TRANSMISSION_INTERFACE_H
36 #define TRANSMISSION_INTERFACE_TRANSMISSION_INTERFACE_H
37 
38 #include <map>
39 #include <string>
40 #include <vector>
41 
42 #include <hardware_interface/internal/resource_manager.h>
43 #include <transmission_interface/transmission.h>
45 #include <transmission_interface/transmission_interface_exception.h>
46 
47 namespace transmission_interface
48 {
49 
54  class MiaTransmissionHandle
55  {
56  public:
57 
62  std::string getName() const {return name_;}
63 
64  protected:
65 
66  std::string name_;
68  ActuatorData actuator_data_;
69  JointData joint_data_;
70  ActuatorData actuator_state_;
71 
80  MiaTransmissionHandle(const std::string& name,
82  const ActuatorData& actuator_data,
83  const JointData& joint_data,
84  const ActuatorData& actuator_state)
85  : name_(name),
86  transmission_(transmission),
87  actuator_data_(actuator_data),
88  joint_data_(joint_data),
89  actuator_state_(actuator_state)
90  {
91  // Precondition: Valid transmission
93  {
94  throw TransmissionInterfaceException("Unspecified transmission.");
95  }
96 
97  // Catch trivial error: All data vectors are empty (handle can't do anything without data)
98  if (actuator_data.position.empty() && actuator_data.velocity.empty() && actuator_data.effort.empty() &&
99  joint_data.position.empty() && joint_data.velocity.empty() && joint_data.effort.empty() &&
100  actuator_state.position.empty() && actuator_state.velocity.empty() && actuator_state.effort.empty())
101  {
102  throw TransmissionInterfaceException("All data vectors are empty. Transmission instance can't do anything!.");
103  }
104 
105  // Precondition: All non-empty data vectors must have sizes consistent with the transmission
106  if (!actuator_data.position.empty() && actuator_data.position.size() != transmission_->numActuators() )
107  {
108  throw TransmissionInterfaceException("Actuator position data size does not match transmission.");
109  }
110  if (!actuator_data.velocity.empty() && actuator_data.velocity.size() != transmission_->numActuators())
111  {
112  throw TransmissionInterfaceException("Actuator velocity data size does not match transmission.");
113  }
114  if (!actuator_data.effort.empty() && actuator_data.effort.size() != transmission_->numActuators())
115  {
116  throw TransmissionInterfaceException("Actuator effort data size does not match transmission.");
117  }
118 
119  if (!joint_data.position.empty() && joint_data.position.size() != transmission_->numJoints())
120  {
121  throw TransmissionInterfaceException("Joint position data size does not match transmission.");
122  }
123  if (!joint_data.velocity.empty() && joint_data.velocity.size() != transmission_->numJoints())
124  {
125  throw TransmissionInterfaceException("Joint velocity data size does not match transmission.");
126  }
127  if (!joint_data.effort.empty() && joint_data.effort.size() != transmission_->numJoints())
128  {
129  throw TransmissionInterfaceException("Joint effort data size does not match transmission.");
130  }
131 
132  // Added
133  if (!actuator_state.position.empty() && actuator_state.position.size() != transmission_->numActuators() )
134  {
135  throw TransmissionInterfaceException("Actuator position state size does not match transmission.");
136  }
137  if (!actuator_state.velocity.empty() && actuator_state.velocity.size() != transmission_->numActuators())
138  {
139  throw TransmissionInterfaceException("Actuator velocity state size does not match transmission.");
140  }
141  if (!actuator_state.effort.empty() && actuator_state.effort.size() != transmission_->numActuators())
142  {
143  throw TransmissionInterfaceException("Actuator effort state size does not match transmission.");
144  }
146 
147  // Precondition: Valid pointers to raw data
148  if (!hasValidPointers(actuator_data.position))
149  {
150  throw TransmissionInterfaceException("Actuator position data contains null pointers.");
151  }
152  if (!hasValidPointers(actuator_data.velocity))
153  {
154  throw TransmissionInterfaceException("Actuator velocity data contains null pointers.");
155  }
156  if (!hasValidPointers(actuator_data.effort))
157  {
158  throw TransmissionInterfaceException("Actuator effort data contains null pointers.");
159  }
160 
161  if (!hasValidPointers(joint_data.position))
162  {
163  throw TransmissionInterfaceException("Joint position data contains null pointers.");
164  }
165  if (!hasValidPointers(joint_data.velocity))
166  {
167  throw TransmissionInterfaceException("Joint velocity data contains null pointers.");
168  }
169  if (!hasValidPointers(joint_data.effort))
170  {
171  throw TransmissionInterfaceException("Joint effort data contains null pointers.");
172  }
173 
174  // Added
175  if (!hasValidPointers(actuator_state.position))
176  {
177  throw TransmissionInterfaceException("Actuator position state contains null pointers.");
178  }
179  if (!hasValidPointers(actuator_state.velocity))
180  {
181  throw TransmissionInterfaceException("Actuator velocity state contains null pointers.");
182  }
183  if (!hasValidPointers(actuator_state.effort))
184  {
185  throw TransmissionInterfaceException("Actuator effort state contains null pointers.");
186  }
187  }
188 
189  private:
190  static bool hasValidPointers(const std::vector<double*>& data)
191  {
192  for (std::vector<double*>::const_iterator it = data.begin(); it != data.end(); ++it)
193  {
194  if (!(*it)) {return false;}
195  }
196  return true;
197  }
198  };
199 
204  class MiaActuatorToJointStateHandle : public MiaTransmissionHandle
205  {
206  public:
207 
215  MiaActuatorToJointStateHandle(const std::string& name,
217  const ActuatorData& actuator_data,
218  const JointData& joint_data)
219  : MiaTransmissionHandle(name, transmission, actuator_data, joint_data, actuator_data) {}
220 
224  void propagate()
225  {
228  //transmission_->actuatorToJointEffort( actuator_data_, joint_data_); Unused
229  }
230  };
231 
232 
238  {
239  public:
240 
248  MiaActuatorToJointPositionHandle(const std::string& name,
250  const ActuatorData& actuator_data,
251  const JointData& joint_data)
252  : MiaTransmissionHandle(name, transmission, actuator_data, joint_data, actuator_data) {}
253 
258  };
259 
260 
265  class MiaActuatorToJointVelocityHandle : public MiaTransmissionHandle
266  {
267  public:
268 
276  MiaActuatorToJointVelocityHandle(const std::string& name,
278  const ActuatorData& actuator_data,
279  const JointData& joint_data)
280  : MiaTransmissionHandle(name, transmission, actuator_data, joint_data, actuator_data) {}
281 
286  };
287 
288 
293  class MiaJointToActuatorStateHandle : public MiaTransmissionHandle
294  {
295  public:
304  MiaJointToActuatorStateHandle(const std::string& name,
306  const ActuatorData& actuator_data,
307  const JointData& joint_data,
308  const ActuatorData& actuator_state)
309  : MiaTransmissionHandle(name, transmission, actuator_data, joint_data, actuator_state) {}
310 
314  void propagate()
315  {
317  transmission_->IndexjointToActuatorVelocity(joint_data_, actuator_state_, actuator_data_ ); // --> modified to get the actual actuator position state needed for tr
318  //transmission_->jointToActuatorEffort( joint_data_, actuator_data_);
319  }
320  /*\}*/
321  };
322 
323 
329  {
330  public:
331 
340  MiaJointToActuatorPositionHandle(const std::string& name,
342  const ActuatorData& actuator_data,
343  const JointData& joint_data,
344  const ActuatorData& actuator_state)
345  : MiaTransmissionHandle(name, transmission, actuator_data, joint_data, actuator_state) {}
346 
351  };
352 
353 
354 
359  class MiaJointToActuatorVelocityHandle : public MiaTransmissionHandle
360  {
361  public:
370  MiaJointToActuatorVelocityHandle(const std::string& name,
372  const ActuatorData& actuator_data,
373  const JointData& joint_data,
374  const ActuatorData& actuator_state)
375  : MiaTransmissionHandle(name, transmission, actuator_data, joint_data, actuator_state) {}
376 
380  void propagate() {transmission_->IndexjointToActuatorVelocity(joint_data_, actuator_state_, actuator_data_ );} // --> modified to get the actual actuator position state needed for tr
381  };
382 
383  template <class HandleType>
384  class MiaTransmissionInterface : public hardware_interface::ResourceManager<HandleType>
385  {
386  public:
387 
388  HandleType getHandle(const std::string& name)
389  {
390  // Rethrow exception with a meaningful type
391  try
392  {
393  return this->hardware_interface::ResourceManager<HandleType>::getHandle(name);
394  }
395  catch(const std::logic_error& e)
396  {
397  throw TransmissionInterfaceException(e.what());
398  }
399  }
400 
401  void propagate()
402  {
403  typedef typename hardware_interface::ResourceManager<HandleType>::ResourceMap::iterator ItratorType;
404  for (ItratorType it = this->resource_map_.begin(); it != this->resource_map_.end(); ++it)
405  {
406  it->second.propagate();
407  }
408  }
409  /*\}*/
410  };
411 
412  // Convenience typedefs
413 
414  class MiaActuatorToJointStateInterface : public MiaTransmissionInterface<MiaActuatorToJointStateHandle> {};
415 
416  class MiaActuatorToJointPositionInterface : public MiaTransmissionInterface<MiaActuatorToJointPositionHandle> {};
417 
418  class MiaActuatorToJointVelocityInterface : public MiaTransmissionInterface<MiaActuatorToJointVelocityHandle> {};
419 
420 
421  class MiaJointToActuatorStateInterface : public MiaTransmissionInterface<MiaJointToActuatorStateHandle> {};
422 
423  class MiaJointToActuatorPositionInterface : public MiaTransmissionInterface<MiaJointToActuatorPositionHandle> {};
424 
425  class MiaJointToActuatorVelocityInterface : public MiaTransmissionInterface<MiaJointToActuatorVelocityHandle> {};
426 
427 
428 } // transmission_interface
429 
430 #endif //TRANSMISSION_INTERFACE_TRANSMISSION_INTERFACE_H
transmission_interface::MiaActuatorToJointPositionHandle
Class handling for propagating actuator positions to joint positions for a given MiaIndexTransmission...
Definition: mia_transmission_interface.h:243
transmission_interface::MiaTransmissionInterface::getHandle
HandleType getHandle(const std::string &name)
Definition: mia_transmission_interface.h:394
transmission_interface::MiaActuatorToJointPositionHandle::MiaActuatorToJointPositionHandle
MiaActuatorToJointPositionHandle(const std::string &name, transmission_interface::MiaIndexTransmission *transmission, const ActuatorData &actuator_data, const JointData &joint_data)
Class constructor.
Definition: mia_transmission_interface.h:254
transmission_interface::MiaJointToActuatorStateHandle::propagate
void propagate()
Propagate joint state to actuator state for the stored MiaIndexTransmission instance.
Definition: mia_transmission_interface.h:320
transmission_interface::MiaActuatorToJointVelocityHandle::propagate
void propagate()
Propagate actuator velocity to joint velocity for the stored MiaIndexTransmission instance.
Definition: mia_transmission_interface.h:291
transmission_interface::MiaTransmissionHandle::actuator_state_
ActuatorData actuator_state_
Actuator-space state variables.
Definition: mia_transmission_interface.h:82
transmission_interface::MiaActuatorToJointStateHandle::propagate
void propagate()
Propagate actuator state to joint state for the stored MiaIndexTransmission instance.
Definition: mia_transmission_interface.h:230
transmission_interface::MiaTransmissionHandle::getName
std::string getName() const
Get the name of the transmission.
Definition: mia_transmission_interface.h:74
transmission_interface::MiaJointToActuatorStateInterface
Definition: mia_transmission_interface.h:427
transmission_interface
Definition: mia_index_transmission.h:23
transmission_interface::MiaJointToActuatorVelocityHandle::MiaJointToActuatorVelocityHandle
MiaJointToActuatorVelocityHandle(const std::string &name, transmission_interface::MiaIndexTransmission *transmission, const ActuatorData &actuator_data, const JointData &joint_data, const ActuatorData &actuator_state)
Class constructor.
Definition: mia_transmission_interface.h:376
transmission_interface::MiaIndexTransmission::actuatorToJointVelocity
void actuatorToJointVelocity(const transmission_interface::ActuatorData &ind_act_state, transmission_interface::JointData &ind_jnt_data) override
Transform velocity variables from actuator to joint space.
Definition: mia_index_transmission.cpp:256
transmission_interface::MiaIndexTransmission
Implementation of the no-linear Mia Index Transmission.
Definition: mia_index_transmission.h:34
transmission_interface::MiaTransmissionHandle::joint_data_
JointData joint_data_
Joint-space variables (target or actual state).
Definition: mia_transmission_interface.h:81
transmission_interface::MiaJointToActuatorPositionHandle
Class handling for propagating joint positions to actuator positions for a given MiaIndexTransmission...
Definition: mia_transmission_interface.h:334
transmission_interface::MiaActuatorToJointPositionHandle::propagate
void propagate()
Propagate actuator positions to joint positions for the stored MiaIndexTransmission instance.
Definition: mia_transmission_interface.h:263
mia_index_transmission.h
transmission_interface::MiaJointToActuatorVelocityInterface
Definition: mia_transmission_interface.h:431
transmission_interface::MiaTransmissionHandle::name_
std::string name_
ransmission name.
Definition: mia_transmission_interface.h:78
transmission_interface::MiaTransmissionHandle::transmission_
transmission_interface::MiaIndexTransmission * transmission_
MiaIndexTransmission interface.
Definition: mia_transmission_interface.h:79
transmission_interface::MiaJointToActuatorStateHandle::MiaJointToActuatorStateHandle
MiaJointToActuatorStateHandle(const std::string &name, transmission_interface::MiaIndexTransmission *transmission, const ActuatorData &actuator_data, const JointData &joint_data, const ActuatorData &actuator_state)
Class constructor.
Definition: mia_transmission_interface.h:310
transmission_interface::MiaIndexTransmission::IndexjointToActuatorVelocity
void IndexjointToActuatorVelocity(const transmission_interface::JointData &ind_jnt_data, const transmission_interface::ActuatorData &ind_act_state, transmission_interface::ActuatorData &ind_act_cmd)
Transform velocity variables from joint to actuator space.
Definition: mia_index_transmission.cpp:297
transmission_interface::MiaTransmissionHandle
Base class to handle the MiaIndexTransmission.
Definition: mia_transmission_interface.h:60
transmission_interface::MiaIndexTransmission::actuatorToJointPosition
void actuatorToJointPosition(const transmission_interface::ActuatorData &ind_act_state, transmission_interface::JointData &ind_jnt_data) override
Transform position variables from actuator to joint space.
Definition: mia_index_transmission.cpp:240
transmission_interface::MiaActuatorToJointStateHandle::MiaActuatorToJointStateHandle
MiaActuatorToJointStateHandle(const std::string &name, transmission_interface::MiaIndexTransmission *transmission, const ActuatorData &actuator_data, const JointData &joint_data)
Class constructor.
Definition: mia_transmission_interface.h:221
transmission_interface::MiaJointToActuatorPositionHandle::MiaJointToActuatorPositionHandle
MiaJointToActuatorPositionHandle(const std::string &name, transmission_interface::MiaIndexTransmission *transmission, const ActuatorData &actuator_data, const JointData &joint_data, const ActuatorData &actuator_state)
Class constructor.
Definition: mia_transmission_interface.h:346
transmission_interface::MiaTransmissionInterface::propagate
void propagate()
Definition: mia_transmission_interface.h:407
transmission_interface::MiaJointToActuatorPositionHandle::propagate
void propagate()
Propagate joint positions target to actuator positions target for stored MiaIndexTransmission instanc...
Definition: mia_transmission_interface.h:356
transmission_interface::MiaJointToActuatorPositionInterface
Definition: mia_transmission_interface.h:429
transmission_interface::MiaTransmissionHandle::hasValidPointers
static bool hasValidPointers(const std::vector< double * > &data)
Definition: mia_transmission_interface.h:202
transmission_interface::MiaJointToActuatorVelocityHandle::propagate
void propagate()
Propagate joint velocity target to the actuator velocity target for stored MiaIndexTransmission insta...
Definition: mia_transmission_interface.h:386
transmission_interface::MiaIndexTransmission::jointToActuatorPosition
void jointToActuatorPosition(const transmission_interface::JointData &ind_jnt_data, transmission_interface::ActuatorData &ind_act_cmd) override
Transform position variables from joint to actuator space.
Definition: mia_index_transmission.cpp:280
transmission_interface::MiaTransmissionHandle::actuator_data_
ActuatorData actuator_data_
Actuator-space variables (target or actual state).
Definition: mia_transmission_interface.h:80
transmission_interface::MiaActuatorToJointVelocityHandle::MiaActuatorToJointVelocityHandle
MiaActuatorToJointVelocityHandle(const std::string &name, transmission_interface::MiaIndexTransmission *transmission, const ActuatorData &actuator_data, const JointData &joint_data)
Class constructor.
Definition: mia_transmission_interface.h:282
transmission_interface::MiaTransmissionHandle::MiaTransmissionHandle
MiaTransmissionHandle(const std::string &name, transmission_interface::MiaIndexTransmission *transmission, const ActuatorData &actuator_data, const JointData &joint_data, const ActuatorData &actuator_state)
Class constructor.
Definition: mia_transmission_interface.h:92
transmission_interface::MiaTransmissionInterface
Definition: mia_transmission_interface.h:390