differential_transmission.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 <cassert>
34 #include <string>
35 #include <vector>
36 
39 
40 namespace transmission_interface
41 {
42 
118 {
119 public:
126  DifferentialTransmission(const std::vector<double>& actuator_reduction,
127  const std::vector<double>& joint_reduction,
128  const std::vector<double>& joint_offset);
129 
137  DifferentialTransmission(const std::vector<double>& actuator_reduction,
138  const std::vector<double>& joint_reduction,
139  bool ignore_transmission_for_absolute_encoders = false,
140  const std::vector<double>& joint_offset = std::vector<double>(2, 0.0));
141 
149  void actuatorToJointEffort(const ActuatorData& act_data,
150  JointData& jnt_data) override;
151 
159  void actuatorToJointVelocity(const ActuatorData& act_data,
160  JointData& jnt_data) override;
161 
169  void actuatorToJointPosition(const ActuatorData& act_data,
170  JointData& jnt_data) override;
171 
179  void actuatorToJointAbsolutePosition(const ActuatorData& act_data,
180  JointData& jnt_data) override;
181 
189  void actuatorToJointTorqueSensor(const ActuatorData& act_data,
190  JointData& jnt_data) override;
191 
199  void jointToActuatorEffort(const JointData& jnt_data,
200  ActuatorData& act_data) override;
201 
209  void jointToActuatorVelocity(const JointData& jnt_data,
210  ActuatorData& act_data) override;
211 
219  void jointToActuatorPosition(const JointData& jnt_data,
220  ActuatorData& act_data) override;
221 
222  std::size_t numActuators() const override {return 2;}
223  std::size_t numJoints() const override {return 2;}
224  bool hasActuatorToJointAbsolutePosition() const override {return true;}
225  bool hasActuatorToJointTorqueSensor() const override {return true;}
226 
227  const std::vector<double>& getActuatorReduction() const {return act_reduction_;}
228  const std::vector<double>& getJointReduction() const {return jnt_reduction_;}
229  const std::vector<double>& getJointOffset() const {return jnt_offset_;}
230 
231 protected:
232  std::vector<double> act_reduction_;
233  std::vector<double> jnt_reduction_;
234  std::vector<double> jnt_offset_;
236 };
237 
238 inline DifferentialTransmission::DifferentialTransmission(const std::vector<double>& actuator_reduction,
239  const std::vector<double>& joint_reduction,
240  const std::vector<double>& joint_offset)
241  : DifferentialTransmission(actuator_reduction, joint_reduction, false, joint_offset)
242 {}
243 
244 inline DifferentialTransmission::DifferentialTransmission(const std::vector<double>& actuator_reduction,
245  const std::vector<double>& joint_reduction,
246  const bool ignore_transmission_for_absolute_encoders,
247  const std::vector<double>& joint_offset)
248  : act_reduction_(actuator_reduction),
249  jnt_reduction_(joint_reduction),
250  jnt_offset_(joint_offset),
251  ignore_transmission_for_absolute_encoders_(ignore_transmission_for_absolute_encoders)
252 {
253  if (numActuators() != act_reduction_.size() ||
254  numJoints() != jnt_reduction_.size() ||
255  numJoints() != jnt_offset_.size())
256  {
257  throw TransmissionInterfaceException("Reduction and offset vectors of a differential transmission must have size 2.");
258  }
259 
260  if (0.0 == act_reduction_[0] ||
261  0.0 == act_reduction_[1] ||
262  0.0 == jnt_reduction_[0] ||
263  0.0 == jnt_reduction_[1])
264  {
265  throw TransmissionInterfaceException("Transmission reduction ratios cannot be zero.");
266  }
267 }
268 
270  JointData& jnt_data)
271 {
272  assert(numActuators() == act_data.effort.size() && numJoints() == jnt_data.effort.size());
273  assert(act_data.effort[0] && act_data.effort[1] && jnt_data.effort[0] && jnt_data.effort[1]);
274 
275  const std::vector<double>& ar = act_reduction_;
276  const std::vector<double>& jr = jnt_reduction_;
277 
278  *jnt_data.effort[0] = jr[0] * (*act_data.effort[0] * ar[0] + *act_data.effort[1] * ar[1]);
279  *jnt_data.effort[1] = jr[1] * (*act_data.effort[0] * ar[0] - *act_data.effort[1] * ar[1]);
280 }
281 
283  JointData& jnt_data)
284 {
285  assert(numActuators() == act_data.velocity.size() && numJoints() == jnt_data.velocity.size());
286  assert(act_data.velocity[0] && act_data.velocity[1] && jnt_data.velocity[0] && jnt_data.velocity[1]);
287 
288  const std::vector<double>& ar = act_reduction_;
289  const std::vector<double>& jr = jnt_reduction_;
290 
291  *jnt_data.velocity[0] = (*act_data.velocity[0] / ar[0] + *act_data.velocity[1] / ar[1]) / (2.0 * jr[0]);
292  *jnt_data.velocity[1] = (*act_data.velocity[0] / ar[0] - *act_data.velocity[1] / ar[1]) / (2.0 * jr[1]);
293 }
294 
296  JointData& jnt_data)
297 {
298  assert(numActuators() == act_data.position.size() && numJoints() == jnt_data.position.size());
299  assert(act_data.position[0] && act_data.position[1] && jnt_data.position[0] && jnt_data.position[1]);
300 
301  const std::vector<double>& ar = act_reduction_;
302  const std::vector<double>& jr = jnt_reduction_;
303 
304  *jnt_data.position[0] = (*act_data.position[0] / ar[0] + *act_data.position[1] / ar[1]) / (2.0 * jr[0]) + jnt_offset_[0];
305  *jnt_data.position[1] = (*act_data.position[0] / ar[0] - *act_data.position[1] / ar[1]) / (2.0 * jr[1]) + jnt_offset_[1];
306 }
307 
309  JointData& jnt_data)
310 {
311  assert(numActuators() == act_data.absolute_position.size() && numJoints() == jnt_data.absolute_position.size());
312  assert(act_data.position[0] && act_data.absolute_position[1] && jnt_data.absolute_position[0] && jnt_data.absolute_position[1]);
313 
314  const std::vector<double>& ar = act_reduction_;
315  const std::vector<double>& jr = jnt_reduction_;
316 
318  {
319  *jnt_data.absolute_position[0] = (*act_data.absolute_position[0] / ar[0] + *act_data.absolute_position[1] / ar[1]) / (2.0 * jr[0]) + jnt_offset_[0];
320  *jnt_data.absolute_position[1] = (*act_data.absolute_position[0] / ar[0] - *act_data.absolute_position[1] / ar[1]) / (2.0 * jr[1]) + jnt_offset_[1];
321  }
322  else
323  {
324  *jnt_data.absolute_position[0] = *act_data.absolute_position[1];
325  *jnt_data.absolute_position[1] = *act_data.absolute_position[0];
326  }
327 }
328 
330  JointData& jnt_data)
331 {
332  assert(numActuators() == act_data.torque_sensor.size() && numJoints() == jnt_data.torque_sensor.size());
333  assert(act_data.torque_sensor[0] && act_data.torque_sensor[1] && jnt_data.torque_sensor[0] && jnt_data.torque_sensor[1]);
334 
335  const std::vector<double>& ar = act_reduction_;
336  const std::vector<double>& jr = jnt_reduction_;
337 
338  *jnt_data.torque_sensor[0] = jr[0] * (*act_data.torque_sensor[0] * ar[0] + *act_data.torque_sensor[1] * ar[1]);
339  *jnt_data.torque_sensor[1] = jr[1] * (*act_data.torque_sensor[0] * ar[0] - *act_data.torque_sensor[1] * ar[1]);
340 }
341 
343  ActuatorData& act_data)
344 {
345  assert(numActuators() == act_data.effort.size() && numJoints() == jnt_data.effort.size());
346  assert(act_data.effort[0] && act_data.effort[1] && jnt_data.effort[0] && jnt_data.effort[1]);
347 
348  const std::vector<double>& ar = act_reduction_;
349  const std::vector<double>& jr = jnt_reduction_;
350 
351  *act_data.effort[0] = (*jnt_data.effort[0] / jr[0] + *jnt_data.effort[1] / jr[1]) / (2.0 * ar[0]);
352  *act_data.effort[1] = (*jnt_data.effort[0] / jr[0] - *jnt_data.effort[1] / jr[1]) / (2.0 * ar[1]);
353 }
354 
356  ActuatorData& act_data)
357 {
358  assert(numActuators() == act_data.velocity.size() && numJoints() == jnt_data.velocity.size());
359  assert(act_data.velocity[0] && act_data.velocity[1] && jnt_data.velocity[0] && jnt_data.velocity[1]);
360 
361  const std::vector<double>& ar = act_reduction_;
362  const std::vector<double>& jr = jnt_reduction_;
363 
364  *act_data.velocity[0] = (*jnt_data.velocity[0] * jr[0] + *jnt_data.velocity[1] * jr[1]) * ar[0];
365  *act_data.velocity[1] = (*jnt_data.velocity[0] * jr[0] - *jnt_data.velocity[1] * jr[1]) * ar[1];
366 }
367 
369  ActuatorData& act_data)
370 {
371  assert(numActuators() == act_data.position.size() && numJoints() == jnt_data.position.size());
372  assert(act_data.position[0] && act_data.position[1] && jnt_data.position[0] && jnt_data.position[1]);
373 
374  const std::vector<double>& ar = act_reduction_;
375  const std::vector<double>& jr = jnt_reduction_;
376 
377  double jnt_pos_off[2] = {*jnt_data.position[0] - jnt_offset_[0], *jnt_data.position[1] - jnt_offset_[1]};
378 
379  *act_data.position[0] = (jnt_pos_off[0] * jr[0] + jnt_pos_off[1] * jr[1]) * ar[0];
380  *act_data.position[1] = (jnt_pos_off[0] * jr[0] - jnt_pos_off[1] * jr[1]) * ar[1];
381 }
382 
383 } // transmission_interface
std::vector< double * > absolute_position
Definition: transmission.h:68
std::vector< double * > velocity
Definition: transmission.h:66
void actuatorToJointTorqueSensor(const ActuatorData &act_data, JointData &jnt_data) override
Transform torque sensor values from actuator to joint space.
std::vector< double * > absolute_position
Definition: transmission.h:55
void jointToActuatorEffort(const JointData &jnt_data, ActuatorData &act_data) override
Transform effort variables from joint to actuator space.
Contains pointers to raw data representing the position, velocity and acceleration of a transmission&#39;...
Definition: transmission.h:63
void jointToActuatorVelocity(const JointData &jnt_data, ActuatorData &act_data) override
Transform velocity variables from joint to actuator space.
const std::vector< double > & getJointReduction() const
std::vector< double * > position
Definition: transmission.h:52
void jointToActuatorPosition(const JointData &jnt_data, ActuatorData &act_data) override
Transform position variables from joint to actuator space.
const std::vector< double > & getJointOffset() const
std::vector< double * > torque_sensor
Definition: transmission.h:69
std::vector< double * > torque_sensor
Definition: transmission.h:56
void actuatorToJointVelocity(const ActuatorData &act_data, JointData &jnt_data) override
Transform velocity variables from actuator to joint space.
void actuatorToJointAbsolutePosition(const ActuatorData &act_data, JointData &jnt_data) override
Transform absolute encoder values from actuator to joint space.
void actuatorToJointEffort(const ActuatorData &act_data, JointData &jnt_data) override
Transform effort variables from actuator to joint space.
const std::vector< double > & getActuatorReduction() const
Contains pointers to raw data representing the position, velocity and acceleration of a transmission&#39;...
Definition: transmission.h:50
std::vector< double * > velocity
Definition: transmission.h:53
std::vector< double * > position
Definition: transmission.h:65
std::vector< double * > effort
Definition: transmission.h:67
std::vector< double * > effort
Definition: transmission.h:54
Abstract base class for representing mechanical transmissions.
Definition: transmission.h:91
DifferentialTransmission(const std::vector< double > &actuator_reduction, const std::vector< double > &joint_reduction, const std::vector< double > &joint_offset)
Implementation of a differential transmission.
void actuatorToJointPosition(const ActuatorData &act_data, JointData &jnt_data) override
Transform position variables from actuator to joint space.


transmission_interface
Author(s): Adolfo Rodriguez Tsouroukdissian
autogenerated on Mon Feb 28 2022 23:30:26