From 402bb9426951321ca2dbecac97afc809a7b6deca Mon Sep 17 00:00:00 2001 From: Paul Mathieu Date: Mon, 13 Jan 2014 12:21:24 +0100 Subject: [PATCH 1/7] joint_limits: use an open-loop policy for velocity staturation The feedback from the controller is way too slow to be used on an actual robot. A robot that had 15 rad.s^-2 on each wheel as an acceleration limit could not even reach 2 rad.s^-2 This is in line with ros_controllers#23 --- .../joint_limits_interface.h | 19 +++++++++++++------ .../test/joint_limits_interface_test.cpp | 17 +++++++++++++++-- 2 files changed, 28 insertions(+), 8 deletions(-) diff --git a/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.h b/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.h index a92e77613..5228b6630 100644 --- a/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.h +++ b/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.h @@ -406,11 +406,14 @@ class EffortJointSoftLimitsHandle class VelocityJointSaturationHandle { public: - VelocityJointSaturationHandle () {} + VelocityJointSaturationHandle () + : prev_cmd_(0.0) + {} VelocityJointSaturationHandle(const hardware_interface::JointHandle& jh, const JointLimits& limits) - : jh_(jh), - limits_(limits) + : jh_(jh) + , limits_(limits) + , prev_cmd_(0.0) { if (!limits.has_velocity_limits) { @@ -437,11 +440,10 @@ class VelocityJointSaturationHandle if (limits_.has_acceleration_limits) { assert(period.toSec() > 0.0); - const double vel = jh_.getVelocity(); const double dt = period.toSec(); - vel_low = std::max(vel - limits_.max_acceleration * dt, -limits_.max_velocity); - vel_high = std::min(vel + limits_.max_acceleration * dt, limits_.max_velocity); + vel_low = std::max(prev_cmd_ - limits_.max_acceleration * dt, -limits_.max_velocity); + vel_high = std::min(prev_cmd_ + limits_.max_acceleration * dt, limits_.max_velocity); } else { @@ -454,11 +456,16 @@ class VelocityJointSaturationHandle vel_low, vel_high); jh_.setCommand(vel_cmd); + + // Cache variables + prev_cmd_ = jh_.getCommand(); } private: hardware_interface::JointHandle jh_; JointLimits limits_; + + double prev_cmd_; }; /** \brief A handle used to enforce position, velocity, and acceleration limits of a velocity-controlled joint. */ diff --git a/joint_limits_interface/test/joint_limits_interface_test.cpp b/joint_limits_interface/test/joint_limits_interface_test.cpp index f58b1ca94..3be59dc7f 100644 --- a/joint_limits_interface/test/joint_limits_interface_test.cpp +++ b/joint_limits_interface/test/joint_limits_interface_test.cpp @@ -376,28 +376,41 @@ TEST_F(VelocityJointSaturationHandleTest, EnforceAccelerationBounds) pos = 0.0; double cmd; + const ros::Duration long_enough(1000.0); // An arbitrarily long time, sufficient to suppress acceleration limits // Positive velocity - vel = limits.max_velocity / 2.0; + cmd_handle.setCommand(limits.max_velocity / 2.0); // register last command + limits_handle.enforceLimits(long_enough); // make sure the prev_cmd is registered + // without triggering the acceleration limits cmd = limits.max_velocity * 2.0; // Try to go beyond +max velocity cmd_handle.setCommand(cmd); limits_handle.enforceLimits(period); EXPECT_NEAR(limits.max_velocity, cmd_handle.getCommand(), EPS); // Max velocity bounded by velocity limit + cmd_handle.setCommand(limits.max_velocity / 2.0); // register last command + limits_handle.enforceLimits(long_enough); // make sure the prev_cmd is registered + // without triggering the acceleration limits + cmd = -limits.max_velocity * 2.0; // Try to go beyond -max velocity cmd_handle.setCommand(cmd); limits_handle.enforceLimits(period); EXPECT_NEAR(-limits.max_velocity / 2.0, cmd_handle.getCommand(), EPS); // Max velocity bounded by acceleration limit // Negative velocity - vel = -limits.max_velocity / 2.0; + cmd_handle.setCommand(-limits.max_velocity / 2.0); // register last command + limits_handle.enforceLimits(long_enough); // make sure the prev_cmd is registered + // without triggering the acceleration limits cmd = limits.max_velocity * 2.0; // Try to go beyond +max velocity cmd_handle.setCommand(cmd); limits_handle.enforceLimits(period); EXPECT_NEAR(limits.max_velocity / 2.0, cmd_handle.getCommand(), EPS); // Max velocity bounded by acceleration limit + cmd_handle.setCommand(-limits.max_velocity / 2.0); // register last command + limits_handle.enforceLimits(long_enough); // make sure the prev_cmd is registered + // without triggering the acceleration limits + cmd = -limits.max_velocity * 2.0; // Try to go beyond -max velocity cmd_handle.setCommand(cmd); limits_handle.enforceLimits(period); From c1a69bcaa8bea9e0c309316295999f6eec3ee3f7 Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Thu, 7 Nov 2013 02:06:28 -0700 Subject: [PATCH 2/7] Created new hardware interface for switching between controller modes --- .../hardware_interface/joint_mode_interface.h | 108 ++++++++++++++++++ 1 file changed, 108 insertions(+) create mode 100644 hardware_interface/include/hardware_interface/joint_mode_interface.h diff --git a/hardware_interface/include/hardware_interface/joint_mode_interface.h b/hardware_interface/include/hardware_interface/joint_mode_interface.h new file mode 100644 index 000000000..3aa7dbc76 --- /dev/null +++ b/hardware_interface/include/hardware_interface/joint_mode_interface.h @@ -0,0 +1,108 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, University of Colorado, Boulder + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Univ of CO, Boulder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Dave Coleman + Desc: This interface is for switching a hardware interface between different controller modes + i.e. position, velocity, force +*/ + +#ifndef HARDWARE_INTERFACE_JOINT_MODE_INTERFACE_H +#define HARDWARE_INTERFACE_JOINT_MODE_INTERFACE_H + +#include +#include + +namespace hardware_interface +{ + +enum JointCommandModes { + MODE_POSITION = 1, + MODE_VELOCITY = 2, + MODE_EFFORT = 3, + MODE_OTHER = 4 +}; + +/** \brief A handle used to read and mode a single joint. */ +class JointModeHandle +{ +public: + + /** + * \param mode Which mode to start in + */ + JointModeHandle(std::string name, int* mode) + : name_(name), + mode_(mode) + { + if (!mode_) + { + throw HardwareInterfaceException("Cannot create mode interface. Mode data pointer is null."); + } + } + + std::string getName() const {return name_;} + + void setMode(int mode) {assert(mode_); *mode_ = mode;} + int getMode() const {assert(mode_); return *mode_;} + + // Helper function for console messages + std::string getModeName(int mode) + { + switch(mode) + { + case MODE_POSITION: + return "position"; + case MODE_VELOCITY: + return "velocity"; + case MODE_EFFORT: + return "effort"; + case MODE_OTHER: + return "other"; + } + return "unknown"; + } + +private: + int* mode_; + std::string name_; +}; + +/** \brief Hardware interface to support changing between control modes + * + */ +class JointModeInterface : public HardwareResourceManager {}; + +} // namespace + +#endif From 63c6ff8e1130489c6d6d2ca28c87e81cc2002884 Mon Sep 17 00:00:00 2001 From: Paul Mathieu Date: Thu, 8 May 2014 09:58:04 +0200 Subject: [PATCH 3/7] hardware_interface: fix initialization order --- .../include/hardware_interface/joint_mode_interface.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/hardware_interface/include/hardware_interface/joint_mode_interface.h b/hardware_interface/include/hardware_interface/joint_mode_interface.h index 3aa7dbc76..8afbf7b98 100644 --- a/hardware_interface/include/hardware_interface/joint_mode_interface.h +++ b/hardware_interface/include/hardware_interface/joint_mode_interface.h @@ -62,8 +62,8 @@ class JointModeHandle * \param mode Which mode to start in */ JointModeHandle(std::string name, int* mode) - : name_(name), - mode_(mode) + : mode_(mode) + , name_(name) { if (!mode_) { From 551b2d54f0d683df89ce1be6f8810b9153e4c7e1 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 29 Aug 2014 12:47:35 +0200 Subject: [PATCH 4/7] Fix argument types to use enum --- .../include/hardware_interface/joint_mode_interface.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/hardware_interface/include/hardware_interface/joint_mode_interface.h b/hardware_interface/include/hardware_interface/joint_mode_interface.h index 8afbf7b98..656367cca 100644 --- a/hardware_interface/include/hardware_interface/joint_mode_interface.h +++ b/hardware_interface/include/hardware_interface/joint_mode_interface.h @@ -61,7 +61,7 @@ class JointModeHandle /** * \param mode Which mode to start in */ - JointModeHandle(std::string name, int* mode) + JointModeHandle(std::string name, JointCommandModes* mode) : mode_(mode) , name_(name) { @@ -73,11 +73,11 @@ class JointModeHandle std::string getName() const {return name_;} - void setMode(int mode) {assert(mode_); *mode_ = mode;} + void setMode(JointCommandModes mode) {assert(mode_); *mode_ = mode;} int getMode() const {assert(mode_); return *mode_;} // Helper function for console messages - std::string getModeName(int mode) + std::string getModeName(JointCommandModes mode) { switch(mode) { @@ -94,7 +94,7 @@ class JointModeHandle } private: - int* mode_; + JointCommandModes* mode_; std::string name_; }; From 133d768c88bd33179f23879b43eb4e76c433be72 Mon Sep 17 00:00:00 2001 From: Hilario Tome Date: Fri, 9 Sep 2016 21:11:19 +0200 Subject: [PATCH 5/7] Modified structures to have absolute encoder and torque sensor parameters --- .../actuator_state_interface.h | 125 +++++++++++++++++- .../joint_state_interface.h | 116 +++++++++++++++- .../differential_transmission.h | 31 +++++ .../four_bar_linkage_transmission.h | 34 +++++ .../simple_transmission.h | 25 ++++ .../transmission_interface/transmission.h | 16 ++- .../transmission_interface.h | 46 ++++++- .../transmission_interface_loader.h | 12 +- .../src/joint_state_interface_provider.cpp | 87 +++++++++++- 9 files changed, 479 insertions(+), 13 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator_state_interface.h b/hardware_interface/include/hardware_interface/actuator_state_interface.h index 9c23808de..5e62c04ab 100644 --- a/hardware_interface/include/hardware_interface/actuator_state_interface.h +++ b/hardware_interface/include/hardware_interface/actuator_state_interface.h @@ -41,7 +41,7 @@ namespace hardware_interface class ActuatorStateHandle { public: - ActuatorStateHandle() : name_(), pos_(0), vel_(0), eff_(0) {} + ActuatorStateHandle() : name_(), pos_(0), vel_(0), eff_(0), absolute_pos_(0), torque_sensor_(0) {} /** * \param name The name of the actuator @@ -49,8 +49,9 @@ class ActuatorStateHandle * \param vel A pointer to the storage for this actuator's velocity * \param eff A pointer to the storage for this actuator's effort (force or torque) */ + ActuatorStateHandle(const std::string& name, const double* pos, const double* vel, const double* eff) - : name_(name), pos_(pos), vel_(vel), eff_(eff) + : name_(name), pos_(pos), vel_(vel), eff_(eff), absolute_pos_(0), torque_sensor_(0) { if (!pos) { @@ -64,22 +65,142 @@ class ActuatorStateHandle { throw HardwareInterfaceException("Cannot create handle '" + name + "'. Effort data pointer is null."); } + + } + + ActuatorStateHandle(const std::string& name, const double* pos, const double* vel, const double* eff, + const double* absolute_pos) + : name_(name), pos_(pos), vel_(vel), eff_(eff), absolute_pos_(absolute_pos), torque_sensor_(0) + { + if (!pos) + { + throw HardwareInterfaceException("Cannot create handle '" + name + "'. Position data pointer is null."); + } + if (!vel) + { + throw HardwareInterfaceException("Cannot create handle '" + name + "'. Velocity data pointer is null."); + } + if (!eff) + { + throw HardwareInterfaceException("Cannot create handle '" + name + "'. Effort data pointer is null."); + } + if (!absolute_pos) + { + throw HardwareInterfaceException("Cannot create handle '" + name + "'. Absolute data pointer is null."); + } + + } + + // Dummy bool parameter to differentiate from absolute encoder constructor + ActuatorStateHandle(const std::string& name, const double* pos, const double* vel, const double* eff, + const double *torque_sensor, bool ) + : name_(name), pos_(pos), vel_(vel), eff_(eff), absolute_pos_(0), torque_sensor_(torque_sensor) + { + if (!pos) + { + throw HardwareInterfaceException("Cannot create handle '" + name + "'. Position data pointer is null."); + } + if (!vel) + { + throw HardwareInterfaceException("Cannot create handle '" + name + "'. Velocity data pointer is null."); + } + if (!eff) + { + throw HardwareInterfaceException("Cannot create handle '" + name + "'. Effort data pointer is null."); + } + if(!torque_sensor){ + throw HardwareInterfaceException("Cannot create handle '" + name + "'. Torque sensor data pointer is null."); + } + + } + + ActuatorStateHandle(const std::string& name, const double* pos, const double* vel, const double* eff, + const double* absolute_pos, const double *torque_sensor) + : name_(name), pos_(pos), vel_(vel), eff_(eff), absolute_pos_(absolute_pos), torque_sensor_(torque_sensor) + { + if (!pos) + { + throw HardwareInterfaceException("Cannot create handle '" + name + "'. Position data pointer is null."); + } + if (!vel) + { + throw HardwareInterfaceException("Cannot create handle '" + name + "'. Velocity data pointer is null."); + } + if (!eff) + { + throw HardwareInterfaceException("Cannot create handle '" + name + "'. Effort data pointer is null."); + } + + if (!absolute_pos) + { + throw HardwareInterfaceException("Cannot create handle '" + name + "'. Absolute data pointer is null."); + } + if(!torque_sensor){ + throw HardwareInterfaceException("Cannot create handle '" + name + "'. Torque sensor data pointer is null."); + } + } std::string getName() const {return name_;} double getPosition() const {assert(pos_); return *pos_;} double getVelocity() const {assert(vel_); return *vel_;} double getEffort() const {assert(eff_); return *eff_;} + double getAbsolutePosition() const { + assert(absolute_pos_); + if(!absolute_pos_){ + throw std::runtime_error("Actuator does not support absolute encoders"); + } + return *absolute_pos_; + } + double getTorqueSensor() const { + assert(torque_sensor_); + if(!torque_sensor_){ + throw std::runtime_error("Actuator does not support torque sensors"); + } + return *torque_sensor_; + } const double* getPositionPtr() const {return pos_;} const double* getVelocityPtr() const {return vel_;} const double* getEffortPtr() const {return eff_;} + const double* getAbsolutePositionPtr() const { + if(!absolute_pos_){ + throw std::runtime_error("Actuator does not support absolute encoders"); + } + return absolute_pos_; + } + const double* getTorqueSensorPtr() const { + if(!torque_sensor_){ + throw std::runtime_error("Actuator does not support torque sensors"); + } + return torque_sensor_; + } + + bool hasAbsolutePosition(){ + if(!absolute_pos_){ + return false; + } + else{ + return true; + } + } + + bool hasTorqueSensor(){ + if(!torque_sensor_){ + return false; + } + else{ + return true; + } + } private: std::string name_; const double* pos_; const double* vel_; const double* eff_; + const double* absolute_pos_; + const double* torque_sensor_; }; /** \brief Hardware interface to support reading the state of an array of actuators diff --git a/hardware_interface/include/hardware_interface/joint_state_interface.h b/hardware_interface/include/hardware_interface/joint_state_interface.h index 3b6da0838..932c85ee1 100644 --- a/hardware_interface/include/hardware_interface/joint_state_interface.h +++ b/hardware_interface/include/hardware_interface/joint_state_interface.h @@ -41,7 +41,7 @@ namespace hardware_interface class JointStateHandle { public: - JointStateHandle() : name_(), pos_(0), vel_(0), eff_(0) {} + JointStateHandle() : name_(), pos_(0), vel_(0), eff_(0), absolute_pos_(0), torque_sensor_(0) {} /** * \param name The name of the joint @@ -49,8 +49,84 @@ class JointStateHandle * \param vel A pointer to the storage for this joint's velocity * \param eff A pointer to the storage for this joint's effort (force or torque) */ + + JointStateHandle(const std::string& name, const double* pos, const double* vel, const double* eff, + const double* absolute_pos, const double *torque_sensor) + : name_(name), pos_(pos), vel_(vel), eff_(eff), absolute_pos_(absolute_pos), torque_sensor_(torque_sensor) + { + + if (!pos) + { + throw HardwareInterfaceException("Cannot create handle '" + name + "'. Position data pointer is null."); + } + if (!vel) + { + throw HardwareInterfaceException("Cannot create handle '" + name + "'. Velocity data pointer is null."); + } + if (!eff) + { + throw HardwareInterfaceException("Cannot create handle '" + name + "'. Effort data pointer is null."); + } + if (!absolute_pos) + { + throw HardwareInterfaceException("Cannot create handle '" + name + "'. Absolute data pointer is null."); + } + if (!torque_sensor) + { + throw HardwareInterfaceException("Cannot create handle '" + name + "'. Torque sensor data pointer is null."); + } + } + + JointStateHandle(const std::string& name, const double* pos, const double* vel, const double* eff, + const double* absolute_pos) + : name_(name), pos_(pos), vel_(vel), eff_(eff), absolute_pos_(absolute_pos), torque_sensor_(0) + { + + if (!pos) + { + throw HardwareInterfaceException("Cannot create handle '" + name + "'. Position data pointer is null."); + } + if (!vel) + { + throw HardwareInterfaceException("Cannot create handle '" + name + "'. Velocity data pointer is null."); + } + if (!eff) + { + throw HardwareInterfaceException("Cannot create handle '" + name + "'. Effort data pointer is null."); + } + if (!absolute_pos) + { + throw HardwareInterfaceException("Cannot create handle '" + name + "'. Absolute data pointer is null."); + } + } + + + JointStateHandle(const std::string& name, const double* pos, const double* vel, const double* eff, + const double *torque_sensor, bool ) + : name_(name), pos_(pos), vel_(vel), eff_(eff), absolute_pos_(0), torque_sensor_(torque_sensor) + { + + if (!pos) + { + throw HardwareInterfaceException("Cannot create handle '" + name + "'. Position data pointer is null."); + } + if (!vel) + { + throw HardwareInterfaceException("Cannot create handle '" + name + "'. Velocity data pointer is null."); + } + if (!eff) + { + throw HardwareInterfaceException("Cannot create handle '" + name + "'. Effort data pointer is null."); + } + if (!torque_sensor) + { + throw HardwareInterfaceException("Cannot create handle '" + name + "'. Torque sensor data pointer is null."); + } + } + + JointStateHandle(const std::string& name, const double* pos, const double* vel, const double* eff) - : name_(name), pos_(pos), vel_(vel), eff_(eff) + : name_(name), pos_(pos), vel_(vel), eff_(eff), absolute_pos_(0), torque_sensor_(0) { if (!pos) { @@ -64,6 +140,7 @@ class JointStateHandle { throw HardwareInterfaceException("Cannot create handle '" + name + "'. Effort data pointer is null."); } + } std::string getName() const {return name_;} @@ -71,11 +148,46 @@ class JointStateHandle double getVelocity() const {assert(vel_); return *vel_;} double getEffort() const {assert(eff_); return *eff_;} + double getAbsolutePosition() const { + assert(absolute_pos_); + if(!hasAbsolutePosition()){ + throw std::runtime_error("Joint state handle does not contain absolute encoder position information"); + } + return *absolute_pos_; + } + double getTorqueSensor() const { + assert(torque_sensor_); + if(!hasTorqueSensor()){ + throw std::runtime_error("Joint state handle does not contain torque sensor information"); + } + return *torque_sensor_; + } + + bool hasAbsolutePosition() const { + if(absolute_pos_){ + return true; + } + return false; + } + + bool hasTorqueSensor() const + { + if(torque_sensor_){ + return true; + } + else{ + return false; + } + } + private: std::string name_; const double* pos_; const double* vel_; const double* eff_; + + const double* absolute_pos_; + const double* torque_sensor_; }; /** \brief Hardware interface to support reading the state of an array of joints diff --git a/transmission_interface/include/transmission_interface/differential_transmission.h b/transmission_interface/include/transmission_interface/differential_transmission.h index b992bc37d..45cc86451 100644 --- a/transmission_interface/include/transmission_interface/differential_transmission.h +++ b/transmission_interface/include/transmission_interface/differential_transmission.h @@ -157,6 +157,11 @@ class DifferentialTransmission : public Transmission void actuatorToJointPosition(const ActuatorData& act_data, JointData& jnt_data); + void actuatorToJointAbsolutePosition(const ActuatorData& act_data, + JointData& jnt_data); + + void actuatorToJointTorqueSensor(const ActuatorData& act_data, + JointData& jnt_data); /** * \brief Transform \e effort variables from joint to actuator space. * \param[in] jnt_data Joint-space variables. @@ -264,6 +269,32 @@ inline void DifferentialTransmission::actuatorToJointPosition(const ActuatorData *jnt_data.position[1] = (*act_data.position[0] / ar[0] - *act_data.position[1] / ar[1]) / (2.0 * jr[1]) + jnt_offset_[1]; } +inline void DifferentialTransmission::actuatorToJointAbsolutePosition(const ActuatorData& act_data, + JointData& jnt_data) +{ + assert(numActuators() == act_data.absolute_position.size() && numJoints() == jnt_data.absolute_position.size()); + assert(act_data.position[0] && act_data.absolute_position[1] && jnt_data.absolute_position[0] && jnt_data.absolute_position[1]); + + std::vector& ar = act_reduction_; + std::vector& jr = jnt_reduction_; + + *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]; + *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]; +} + +inline void DifferentialTransmission::actuatorToJointTorqueSensor(const ActuatorData& act_data, + JointData& jnt_data) +{ + assert(numActuators() == act_data.torque_sensor.size() && numJoints() == jnt_data.torque_sensor.size()); + assert(act_data.torque_sensor[0] && act_data.torque_sensor[1] && jnt_data.torque_sensor[0] && jnt_data.torque_sensor[1]); + + std::vector& ar = act_reduction_; + std::vector& jr = jnt_reduction_; + + *jnt_data.torque_sensor[0] = jr[0] * (*act_data.torque_sensor[0] * ar[0] + *act_data.torque_sensor[1] * ar[1]); + *jnt_data.torque_sensor[1] = jr[1] * (*act_data.torque_sensor[0] * ar[0] - *act_data.torque_sensor[1] * ar[1]); +} + inline void DifferentialTransmission::jointToActuatorEffort(const JointData& jnt_data, ActuatorData& act_data) { diff --git a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.h b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.h index 97f7938ca..23dc26e54 100644 --- a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.h +++ b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.h @@ -157,6 +157,12 @@ class FourBarLinkageTransmission : public Transmission void actuatorToJointPosition(const ActuatorData& act_data, JointData& jnt_data); + void actuatorToJointAbsolutePosition(const ActuatorData& act_data, + JointData& jnt_data); + + void actuatorToJointTorqueSensor(const ActuatorData& act_data, + JointData& jnt_data); + /** * \brief Transform \e effort variables from joint to actuator space. * \param[in] jnt_data Joint-space variables. @@ -263,6 +269,34 @@ inline void FourBarLinkageTransmission::actuatorToJointPosition(const ActuatorDa + jnt_offset_[1]; } +void FourBarLinkageTransmission::actuatorToJointAbsolutePosition(const ActuatorData& act_data, + JointData& jnt_data){ + + assert(numActuators() == act_data.absolute_position.size() && numJoints() == jnt_data.absolute_position.size()); + assert(act_data.absolute_position[0] && act_data.absolute_position[1] && jnt_data.absolute_position[0] && jnt_data.absolute_position[1]); + + std::vector& ar = act_reduction_; + std::vector& jr = jnt_reduction_; + + *jnt_data.absolute_position[0] = *act_data.absolute_position[0] /(jr[0] * ar[0]) + jnt_offset_[0]; + *jnt_data.absolute_position[1] = (*act_data.absolute_position[1] / ar[1] - *act_data.absolute_position[0] / (jr[0] * ar[0])) / jr[1] + + jnt_offset_[1]; +} + +void FourBarLinkageTransmission::actuatorToJointTorqueSensor(const ActuatorData& act_data, + JointData& jnt_data){ + + assert(numActuators() == act_data.torque_sensor.size() && numJoints() == jnt_data.torque_sensor.size()); + assert(act_data.torque_sensor[0] && act_data.torque_sensor[1] && jnt_data.torque_sensor[0] && jnt_data.torque_sensor[1]); + + std::vector& ar = act_reduction_; + std::vector& jr = jnt_reduction_; + + *jnt_data.torque_sensor[0] = jr[0] * (*act_data.torque_sensor[0] * ar[0]); + *jnt_data.torque_sensor[1] = jr[1] * (*act_data.torque_sensor[1] * ar[1] - *act_data.torque_sensor[0] * ar[0] * jr[0]); +} + + inline void FourBarLinkageTransmission::jointToActuatorEffort(const JointData& jnt_data, ActuatorData& act_data) { diff --git a/transmission_interface/include/transmission_interface/simple_transmission.h b/transmission_interface/include/transmission_interface/simple_transmission.h index 0fec01158..74e0dbaa8 100644 --- a/transmission_interface/include/transmission_interface/simple_transmission.h +++ b/transmission_interface/include/transmission_interface/simple_transmission.h @@ -133,6 +133,12 @@ class SimpleTransmission : public Transmission void actuatorToJointPosition(const ActuatorData& act_data, JointData& jnt_data); + void actuatorToJointAbsolutePosition(const ActuatorData& act_data, + JointData& jnt_data); + + void actuatorToJointTorqueSensor(const ActuatorData& act_data, + JointData& jnt_data); + /** * \brief Transform \e effort variables from joint to actuator space. * \param[in] jnt_data Joint-space variables. @@ -213,6 +219,25 @@ inline void SimpleTransmission::actuatorToJointPosition(const ActuatorData& act_ *jnt_data.position[0] = *act_data.position[0] / reduction_ + jnt_offset_; } +inline void SimpleTransmission::actuatorToJointAbsolutePosition(const ActuatorData& act_data, + JointData& jnt_data){ + + assert(numActuators() == act_data.absolute_position.size() && numJoints() == jnt_data.absolute_position.size()); + assert(act_data.absolute_position[0] && jnt_data.absolute_position[0]); + + *jnt_data.absolute_position[0] = *act_data.absolute_position[0] / reduction_ + jnt_offset_; +} + +inline void SimpleTransmission::actuatorToJointTorqueSensor(const ActuatorData& act_data, + JointData& jnt_data){ + + assert(numActuators() == act_data.torque_sensor.size() && numJoints() == jnt_data.torque_sensor.size()); + assert(act_data.torque_sensor[0] && jnt_data.torque_sensor[0]); + + *jnt_data.torque_sensor[0] = *act_data.torque_sensor[0] * reduction_; +} + + inline void SimpleTransmission::jointToActuatorEffort(const JointData& jnt_data, ActuatorData& act_data) { diff --git a/transmission_interface/include/transmission_interface/transmission.h b/transmission_interface/include/transmission_interface/transmission.h index 787c96020..fabe9fbed 100644 --- a/transmission_interface/include/transmission_interface/transmission.h +++ b/transmission_interface/include/transmission_interface/transmission.h @@ -34,7 +34,7 @@ #include #include #include - +#include namespace transmission_interface { @@ -52,6 +52,8 @@ struct ActuatorData std::vector position; std::vector velocity; std::vector effort; + std::vector absolute_position; + std::vector torque_sensor; }; /** @@ -63,6 +65,8 @@ struct JointData std::vector position; std::vector velocity; std::vector effort; + std::vector absolute_position; + std::vector torque_sensor; }; /** @@ -122,6 +126,16 @@ class Transmission virtual void actuatorToJointPosition(const ActuatorData& act_data, JointData& jnt_data) = 0; + virtual void actuatorToJointAbsolutePosition(const ActuatorData& act_data, + JointData&){ + throw std::runtime_error("transmission does not support actuator to joint absolute position"); + } + + virtual void actuatorToJointTorqueSensor(const ActuatorData& act_data, + JointData&){ + throw std::runtime_error("transmission does not support actuator to joint torque sensor"); + } + /** * \brief Transform \e effort variables from joint to actuator space. * \param[in] jnt_data Joint-space variables. diff --git a/transmission_interface/include/transmission_interface/transmission_interface.h b/transmission_interface/include/transmission_interface/transmission_interface.h index 0db43d59b..12f2aeb19 100644 --- a/transmission_interface/include/transmission_interface/transmission_interface.h +++ b/transmission_interface/include/transmission_interface/transmission_interface.h @@ -86,7 +86,7 @@ class TransmissionHandle if (actuator_data.position.empty() && actuator_data.velocity.empty() && actuator_data.effort.empty() && joint_data.position.empty() && joint_data.velocity.empty() && joint_data.effort.empty()) { - throw TransmissionInterfaceException("All data vectors are empty. Transmission instance can't do anything!."); + throw TransmissionInterfaceException("All data vectors are empty. Transmission instance can't do anything!."); } // Precondition: All non-empty data vectors must have sizes consistent with the transmission @@ -102,6 +102,14 @@ class TransmissionHandle { throw TransmissionInterfaceException("Actuator effort data size does not match transmission."); } + if (!actuator_data.absolute_position.empty() && actuator_data.absolute_position.size() != transmission_->numActuators()) + { + throw TransmissionInterfaceException("Actuator absolute position data size does not match transmission."); + } + if (!actuator_data.absolute_position.empty() && actuator_data.absolute_position.size() != transmission_->numActuators()) + { + throw TransmissionInterfaceException("Actuator torque sensor data size does not match transmission."); + } if (!joint_data.position.empty() && joint_data.position.size() != transmission_->numJoints()) { @@ -115,6 +123,14 @@ class TransmissionHandle { throw TransmissionInterfaceException("Joint effort data size does not match transmission."); } + if (!joint_data.absolute_position.empty() && joint_data.absolute_position.size() != transmission_->numJoints()) + { + throw TransmissionInterfaceException("Joint absolute position data size does not match transmission."); + } + if (!joint_data.torque_sensor.empty() && joint_data.torque_sensor.size() != transmission_->numJoints()) + { + throw TransmissionInterfaceException("Joint torque sensor data size does not match transmission."); + } // Precondition: Valid pointers to raw data if (!hasValidPointers(actuator_data.position)) @@ -129,6 +145,14 @@ class TransmissionHandle { throw TransmissionInterfaceException("Actuator effort data contains null pointers."); } + if (!actuator_data.absolute_position.empty() && !hasValidPointers(actuator_data.absolute_position)) + { + throw TransmissionInterfaceException("Actuator absolute position data contains null pointers."); + } + if (!actuator_data.torque_sensor.empty() && !hasValidPointers(actuator_data.torque_sensor)) + { + throw TransmissionInterfaceException("Actuator torque sensor data contains null pointers."); + } if (!hasValidPointers(joint_data.position)) { @@ -142,6 +166,15 @@ class TransmissionHandle { throw TransmissionInterfaceException("Joint effort data contains null pointers."); } + if (!joint_data.absolute_position.empty() && !hasValidPointers(joint_data.absolute_position)) + { + throw TransmissionInterfaceException("Joint absolute position data contains null pointers."); + } + if (!joint_data.torque_sensor.empty() && !hasValidPointers(joint_data.torque_sensor)) + { + throw TransmissionInterfaceException("Joint torque sensor data contains null pointers."); + } + } private: @@ -175,7 +208,16 @@ class ActuatorToJointStateHandle : public TransmissionHandle { transmission_->actuatorToJointPosition(actuator_data_, joint_data_); transmission_->actuatorToJointVelocity(actuator_data_, joint_data_); - transmission_->actuatorToJointEffort( actuator_data_, joint_data_); + transmission_->actuatorToJointEffort(actuator_data_, joint_data_); + + //if(!actuator_data_.absolute_position.empty() && transmission_->hasActuatorToJointAbsolutePosition()){ + if(!actuator_data_.absolute_position.empty()){ + transmission_->actuatorToJointAbsolutePosition(actuator_data_, joint_data_); + } + // if(!actuator_data_.torque_sensor.empty() && transmission_->hasActuatorToJointTorqueSensor()){ + if(!actuator_data_.torque_sensor.empty()){ + transmission_->actuatorToJointTorqueSensor(actuator_data_, joint_data_); + } } /*\}*/ }; diff --git a/transmission_interface/include/transmission_interface/transmission_interface_loader.h b/transmission_interface/include/transmission_interface/transmission_interface_loader.h index 222c49c19..c010a1559 100644 --- a/transmission_interface/include/transmission_interface/transmission_interface_loader.h +++ b/transmission_interface/include/transmission_interface/transmission_interface_loader.h @@ -110,7 +110,11 @@ struct RawJointData effort(std::numeric_limits::quiet_NaN()), position_cmd(std::numeric_limits::quiet_NaN()), velocity_cmd(std::numeric_limits::quiet_NaN()), - effort_cmd(std::numeric_limits::quiet_NaN()) + effort_cmd(std::numeric_limits::quiet_NaN()), + absolute_position(std::numeric_limits::quiet_NaN()), + torque_sensor(std::numeric_limits::quiet_NaN()), + hasAbsolutePosition(true), + hasTorqueSensor(true) {} double position; @@ -119,6 +123,12 @@ struct RawJointData double position_cmd; double velocity_cmd; double effort_cmd; + + double absolute_position; + double torque_sensor; + + bool hasAbsolutePosition; + bool hasTorqueSensor; }; typedef std::map RawJointDataMap; diff --git a/transmission_interface/src/joint_state_interface_provider.cpp b/transmission_interface/src/joint_state_interface_provider.cpp index 29ecf6af8..027230836 100644 --- a/transmission_interface/src/joint_state_interface_provider.cpp +++ b/transmission_interface/src/joint_state_interface_provider.cpp @@ -59,11 +59,39 @@ bool JointStateInterfaceProvider::updateJointInterfaces(const TransmissionInfo& // Update hardware interface using hardware_interface::JointStateHandle; RawJointData& raw_joint_data = raw_joint_data_map[name]; // Add joint if it does not yet exist - JointStateHandle handle(name, - &raw_joint_data.position, - &raw_joint_data.velocity, - &raw_joint_data.effort); - interface.registerHandle(handle); + if(raw_joint_data.absolute_position && raw_joint_data.torque_sensor){ + JointStateHandle handle(name, + &raw_joint_data.position, + &raw_joint_data.velocity, + &raw_joint_data.effort, + &raw_joint_data.absolute_position, + &raw_joint_data.torque_sensor); + interface.registerHandle(handle); + } + else if(raw_joint_data.absolute_position){ + JointStateHandle handle(name, + &raw_joint_data.position, + &raw_joint_data.velocity, + &raw_joint_data.effort, + &raw_joint_data.absolute_position); + interface.registerHandle(handle); + } + else if(raw_joint_data.torque_sensor){ + JointStateHandle handle(name, + &raw_joint_data.position, + &raw_joint_data.velocity, + &raw_joint_data.effort, + &raw_joint_data.torque_sensor, true); + interface.registerHandle(handle); + } + else{ + JointStateHandle handle(name, + &raw_joint_data.position, + &raw_joint_data.velocity, + &raw_joint_data.effort); + interface.registerHandle(handle); + } + } return true; } @@ -77,6 +105,28 @@ bool JointStateInterfaceProvider::getJointStateData(const TransmissionInfo& tran jnt_state_data.velocity.resize(dim); jnt_state_data.effort.resize(dim); + bool hasAbsolutePosition = true; + bool hasTorqueSensor = true; + + for (unsigned int i = 0; i < dim; ++i) + { + const std::string& joint_name = transmission_info.joints_[i].name_; + RawJointDataMap::const_iterator raw_joint_data_it = raw_joint_data_map.find(joint_name); + if (raw_joint_data_it == raw_joint_data_map.end()) {return false;} // Joint name not found! + const RawJointData& raw_joint_data = raw_joint_data_it->second; + + hasAbsolutePosition = hasAbsolutePosition && raw_joint_data.hasAbsolutePosition; + hasTorqueSensor = hasTorqueSensor && raw_joint_data.hasTorqueSensor; + } + + if(hasAbsolutePosition){ + jnt_state_data.absolute_position.resize(dim); + } + + if(hasTorqueSensor){ + jnt_state_data.torque_sensor.resize(dim); + } + for (unsigned int i = 0; i < dim; ++i) { const std::string& joint_name = transmission_info.joints_[i].name_; @@ -88,6 +138,13 @@ bool JointStateInterfaceProvider::getJointStateData(const TransmissionInfo& tran jnt_state_data.position[i] = const_cast(&(raw_joint_data.position)); jnt_state_data.velocity[i] = const_cast(&(raw_joint_data.velocity)); jnt_state_data.effort[i] = const_cast(&(raw_joint_data.effort)); + if(hasAbsolutePosition){ + jnt_state_data.absolute_position[i] = const_cast(&(raw_joint_data.absolute_position)); + } + if(hasTorqueSensor){ + jnt_state_data.torque_sensor[i] = const_cast(&(raw_joint_data.torque_sensor)); + } + } return true; @@ -112,12 +169,32 @@ bool JointStateInterfaceProvider::getActuatorStateData(const TransmissionInfo& act_state_data.velocity.resize(dim); act_state_data.effort.resize(dim); + bool hasAbsolutePositionInterface = true; + bool hasTorqueSensorInterface = true; + for (unsigned int i = 0; i < dim; ++i){ + hasAbsolutePositionInterface = hasAbsolutePositionInterface && handles[i].hasAbsolutePosition(); + hasTorqueSensorInterface = hasTorqueSensorInterface && handles[i].hasTorqueSensor(); + } + + if(hasAbsolutePositionInterface){ + act_state_data.absolute_position.resize(dim); + } + if(hasTorqueSensorInterface){ + act_state_data.torque_sensor.resize(dim); + } + for (unsigned int i = 0; i < dim; ++i) { // TODO: Get rid of these const casts! act_state_data.position[i] = const_cast(handles[i].getPositionPtr()); act_state_data.velocity[i] = const_cast(handles[i].getVelocityPtr()); act_state_data.effort[i] = const_cast(handles[i].getEffortPtr()); + if(hasAbsolutePositionInterface){ + act_state_data.absolute_position[i] = const_cast(handles[i].getAbsolutePositionPtr()); + } + if(hasTorqueSensorInterface){ + act_state_data.torque_sensor[i] = const_cast(handles[i].getTorqueSensorPtr()); + } } return true; } From 9592ad6b339c363487f2965d1ae1a2134539978f Mon Sep 17 00:00:00 2001 From: Hilario Tome Date: Tue, 11 Oct 2016 14:55:36 +0200 Subject: [PATCH 6/7] Add torque sensor and absolute encoder support to transmissions and adjust tests Add pointer accessors for torque sensor and absoute position encoders --- .../actuator_state_interface.h | 116 ++++++++++-------- .../joint_command_interface.h | 1 + .../hardware_interface/joint_mode_interface.h | 5 +- .../joint_state_interface.h | 116 +++++++++++------- .../differential_transmission.h | 93 ++++++++++---- .../differential_transmission_loader.h | 3 +- .../four_bar_linkage_transmission.h | 37 ++++-- .../simple_transmission.h | 11 +- .../transmission_interface/transmission.h | 15 ++- .../transmission_interface.h | 13 +- .../transmission_interface_loader.h | 1 - .../src/differential_transmission_loader.cpp | 24 +++- .../src/joint_state_interface_provider.cpp | 48 +++++--- 13 files changed, 316 insertions(+), 167 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator_state_interface.h b/hardware_interface/include/hardware_interface/actuator_state_interface.h index 5e62c04ab..7d2f3e275 100644 --- a/hardware_interface/include/hardware_interface/actuator_state_interface.h +++ b/hardware_interface/include/hardware_interface/actuator_state_interface.h @@ -37,7 +37,11 @@ namespace hardware_interface { -/** A handle used to read the state of a single actuator. */ +/** + * \brief A handle used to read the state of a single actuator. + * Currently, position, velocity and effort fields are required + * while absolute position and torque sensors are optional. + */ class ActuatorStateHandle { public: @@ -49,7 +53,6 @@ class ActuatorStateHandle * \param vel A pointer to the storage for this actuator's velocity * \param eff A pointer to the storage for this actuator's effort (force or torque) */ - ActuatorStateHandle(const std::string& name, const double* pos, const double* vel, const double* eff) : name_(name), pos_(pos), vel_(vel), eff_(eff), absolute_pos_(0), torque_sensor_(0) { @@ -65,12 +68,19 @@ class ActuatorStateHandle { throw HardwareInterfaceException("Cannot create handle '" + name + "'. Effort data pointer is null."); } - } + /** + * \param name The name of the actuator + * \param pos A pointer to the storage for this actuator's position + * \param vel A pointer to the storage for this actuator's velocity + * \param eff A pointer to the storage for this actuator's effort (force or torque) + * \param absolute_pos A pointer to the storage for this actuator's absolute position encoder + * \param torque_sensor A pointer to the storage for this actuator's torque sensor + */ ActuatorStateHandle(const std::string& name, const double* pos, const double* vel, const double* eff, - const double* absolute_pos) - : name_(name), pos_(pos), vel_(vel), eff_(eff), absolute_pos_(absolute_pos), torque_sensor_(0) + const double* absolute_pos, const double* torque_sensor) + : name_(name), pos_(pos), vel_(vel), eff_(eff), absolute_pos_(absolute_pos), torque_sensor_(torque_sensor) { if (!pos) { @@ -84,17 +94,27 @@ class ActuatorStateHandle { throw HardwareInterfaceException("Cannot create handle '" + name + "'. Effort data pointer is null."); } + if (!absolute_pos) { throw HardwareInterfaceException("Cannot create handle '" + name + "'. Absolute data pointer is null."); } - + if (!torque_sensor) + { + throw HardwareInterfaceException("Cannot create handle '" + name + "'. Torque sensor data pointer is null."); + } } - // Dummy bool parameter to differentiate from absolute encoder constructor + /** + * \param name The name of the actuator + * \param pos A pointer to the storage for this actuator's position + * \param vel A pointer to the storage for this actuator's velocity + * \param eff A pointer to the storage for this actuator's effort (force or torque) + * \param absolute_pos A pointer to the storage for this actuator's absolute position encoder + */ ActuatorStateHandle(const std::string& name, const double* pos, const double* vel, const double* eff, - const double *torque_sensor, bool ) - : name_(name), pos_(pos), vel_(vel), eff_(eff), absolute_pos_(0), torque_sensor_(torque_sensor) + const double* absolute_pos) + : name_(name), pos_(pos), vel_(vel), eff_(eff), absolute_pos_(absolute_pos), torque_sensor_(0) { if (!pos) { @@ -108,15 +128,23 @@ class ActuatorStateHandle { throw HardwareInterfaceException("Cannot create handle '" + name + "'. Effort data pointer is null."); } - if(!torque_sensor){ - throw HardwareInterfaceException("Cannot create handle '" + name + "'. Torque sensor data pointer is null."); + if (!absolute_pos) + { + throw HardwareInterfaceException("Cannot create handle '" + name + "'. Absolute data pointer is null."); } - } + /** + * \param name The name of the actuator + * \param pos A pointer to the storage for this actuator's position + * \param vel A pointer to the storage for this actuator's velocity + * \param eff A pointer to the storage for this actuator's effort (force or torque) + * \param torque_sensor A pointer to the storage for this actuator's torque sensor + * \param bool Dummy parameter to differentiate from absolute encoder constructor + */ ActuatorStateHandle(const std::string& name, const double* pos, const double* vel, const double* eff, - const double* absolute_pos, const double *torque_sensor) - : name_(name), pos_(pos), vel_(vel), eff_(eff), absolute_pos_(absolute_pos), torque_sensor_(torque_sensor) + const double* torque_sensor, bool) + : name_(name), pos_(pos), vel_(vel), eff_(eff), absolute_pos_(0), torque_sensor_(torque_sensor) { if (!pos) { @@ -130,31 +158,30 @@ class ActuatorStateHandle { throw HardwareInterfaceException("Cannot create handle '" + name + "'. Effort data pointer is null."); } - - if (!absolute_pos) + if (!torque_sensor) { - throw HardwareInterfaceException("Cannot create handle '" + name + "'. Absolute data pointer is null."); - } - if(!torque_sensor){ throw HardwareInterfaceException("Cannot create handle '" + name + "'. Torque sensor data pointer is null."); } - } std::string getName() const {return name_;} double getPosition() const {assert(pos_); return *pos_;} double getVelocity() const {assert(vel_); return *vel_;} double getEffort() const {assert(eff_); return *eff_;} - double getAbsolutePosition() const { - assert(absolute_pos_); - if(!absolute_pos_){ - throw std::runtime_error("Actuator does not support absolute encoders"); + + double getAbsolutePosition() const + { + if(!hasAbsolutePosition()) + { + throw std::runtime_error("Actuator does not support absolute encoders"); } return *absolute_pos_; } - double getTorqueSensor() const { - assert(torque_sensor_); - if(!torque_sensor_){ + + double getTorqueSensor() const + { + if(!hasTorqueSensor()) + { throw std::runtime_error("Actuator does not support torque sensors"); } return *torque_sensor_; @@ -163,36 +190,27 @@ class ActuatorStateHandle const double* getPositionPtr() const {return pos_;} const double* getVelocityPtr() const {return vel_;} const double* getEffortPtr() const {return eff_;} - const double* getAbsolutePositionPtr() const { - if(!absolute_pos_){ - throw std::runtime_error("Actuator does not support absolute encoders"); + + const double* getAbsolutePositionPtr() const + { + if(!hasAbsolutePosition()) + { + throw std::runtime_error("Actuator does not support absolute encoders"); } return absolute_pos_; } - const double* getTorqueSensorPtr() const { - if(!torque_sensor_){ + + const double* getTorqueSensorPtr() const + { + if(!hasTorqueSensor()) + { throw std::runtime_error("Actuator does not support torque sensors"); } return torque_sensor_; } - bool hasAbsolutePosition(){ - if(!absolute_pos_){ - return false; - } - else{ - return true; - } - } - - bool hasTorqueSensor(){ - if(!torque_sensor_){ - return false; - } - else{ - return true; - } - } + bool hasAbsolutePosition() const {return absolute_pos_;} + bool hasTorqueSensor() const {return torque_sensor_;} private: std::string name_; diff --git a/hardware_interface/include/hardware_interface/joint_command_interface.h b/hardware_interface/include/hardware_interface/joint_command_interface.h index 8e505fb3b..fa453876c 100644 --- a/hardware_interface/include/hardware_interface/joint_command_interface.h +++ b/hardware_interface/include/hardware_interface/joint_command_interface.h @@ -59,6 +59,7 @@ class JointHandle : public JointStateHandle void setCommand(double command) {assert(cmd_); *cmd_ = command;} double getCommand() const {assert(cmd_); return *cmd_;} + const double* getCommandPtr() const {assert(cmd_); return cmd_;} private: double* cmd_; diff --git a/hardware_interface/include/hardware_interface/joint_mode_interface.h b/hardware_interface/include/hardware_interface/joint_mode_interface.h index 656367cca..1e5f0ab1c 100644 --- a/hardware_interface/include/hardware_interface/joint_mode_interface.h +++ b/hardware_interface/include/hardware_interface/joint_mode_interface.h @@ -73,8 +73,9 @@ class JointModeHandle std::string getName() const {return name_;} - void setMode(JointCommandModes mode) {assert(mode_); *mode_ = mode;} - int getMode() const {assert(mode_); return *mode_;} + void setMode(JointCommandModes mode) {assert(mode_); *mode_ = mode;} + int getMode() const {assert(mode_); return *mode_;} + const int* getModePtr() const {assert(mode_); return mode_;} // Helper function for console messages std::string getModeName(JointCommandModes mode) diff --git a/hardware_interface/include/hardware_interface/joint_state_interface.h b/hardware_interface/include/hardware_interface/joint_state_interface.h index 932c85ee1..12e384633 100644 --- a/hardware_interface/include/hardware_interface/joint_state_interface.h +++ b/hardware_interface/include/hardware_interface/joint_state_interface.h @@ -37,7 +37,11 @@ namespace hardware_interface { -/** A handle used to read the state of a single joint. */ +/** + * \brief A handle used to read the state of a single joint. + * Currently, position, velocity and effort fields are required + * while absolute position and torque sensors are optional. + */ class JointStateHandle { public: @@ -49,12 +53,35 @@ class JointStateHandle * \param vel A pointer to the storage for this joint's velocity * \param eff A pointer to the storage for this joint's effort (force or torque) */ + JointStateHandle(const std::string& name, const double* pos, const double* vel, const double* eff) + : name_(name), pos_(pos), vel_(vel), eff_(eff), absolute_pos_(0), torque_sensor_(0) + { + if (!pos) + { + throw HardwareInterfaceException("Cannot create handle '" + name + "'. Position data pointer is null."); + } + if (!vel) + { + throw HardwareInterfaceException("Cannot create handle '" + name + "'. Velocity data pointer is null."); + } + if (!eff) + { + throw HardwareInterfaceException("Cannot create handle '" + name + "'. Effort data pointer is null."); + } + } + /** + * \param name The name of the joint + * \param pos A pointer to the storage for this joint's position + * \param vel A pointer to the storage for this joint's velocity + * \param eff A pointer to the storage for this joint's effort (force or torque) + * \param absolute_pos A pointer to the storage for this joint's absolute position encoder + * \param torque_sensor A pointer to the storage for this joint's torque sensor + */ JointStateHandle(const std::string& name, const double* pos, const double* vel, const double* eff, - const double* absolute_pos, const double *torque_sensor) + const double* absolute_pos, const double* torque_sensor) : name_(name), pos_(pos), vel_(vel), eff_(eff), absolute_pos_(absolute_pos), torque_sensor_(torque_sensor) { - if (!pos) { throw HardwareInterfaceException("Cannot create handle '" + name + "'. Position data pointer is null."); @@ -77,11 +104,17 @@ class JointStateHandle } } + /** + * \param name The name of the joint + * \param pos A pointer to the storage for this joint's position + * \param vel A pointer to the storage for this joint's velocity + * \param eff A pointer to the storage for this joint's effort (force or torque) + * \param absolute_pos A pointer to the storage for this joint's absolute position encoder + */ JointStateHandle(const std::string& name, const double* pos, const double* vel, const double* eff, const double* absolute_pos) : name_(name), pos_(pos), vel_(vel), eff_(eff), absolute_pos_(absolute_pos), torque_sensor_(0) { - if (!pos) { throw HardwareInterfaceException("Cannot create handle '" + name + "'. Position data pointer is null."); @@ -100,12 +133,18 @@ class JointStateHandle } } - + /** + * \param name The name of the joint + * \param pos A pointer to the storage for this joint's position + * \param vel A pointer to the storage for this joint's velocity + * \param eff A pointer to the storage for this joint's effort (force or torque) + * \param torque_sensor A pointer to the storage for this joint's torque sensor + * \param bool Dummy parameter to differentiate from absolute encoder constructor + */ JointStateHandle(const std::string& name, const double* pos, const double* vel, const double* eff, - const double *torque_sensor, bool ) + const double* torque_sensor, bool) : name_(name), pos_(pos), vel_(vel), eff_(eff), absolute_pos_(0), torque_sensor_(torque_sensor) { - if (!pos) { throw HardwareInterfaceException("Cannot create handle '" + name + "'. Position data pointer is null."); @@ -124,68 +163,59 @@ class JointStateHandle } } - - JointStateHandle(const std::string& name, const double* pos, const double* vel, const double* eff) - : name_(name), pos_(pos), vel_(vel), eff_(eff), absolute_pos_(0), torque_sensor_(0) - { - if (!pos) - { - throw HardwareInterfaceException("Cannot create handle '" + name + "'. Position data pointer is null."); - } - if (!vel) - { - throw HardwareInterfaceException("Cannot create handle '" + name + "'. Velocity data pointer is null."); - } - if (!eff) - { - throw HardwareInterfaceException("Cannot create handle '" + name + "'. Effort data pointer is null."); - } - - } - std::string getName() const {return name_;} double getPosition() const {assert(pos_); return *pos_;} double getVelocity() const {assert(vel_); return *vel_;} double getEffort() const {assert(eff_); return *eff_;} - double getAbsolutePosition() const { - assert(absolute_pos_); - if(!hasAbsolutePosition()){ + double getAbsolutePosition() const + { + if(!hasAbsolutePosition()) + { throw std::runtime_error("Joint state handle does not contain absolute encoder position information"); } return *absolute_pos_; } - double getTorqueSensor() const { - assert(torque_sensor_); - if(!hasTorqueSensor()){ + + double getTorqueSensor() const + { + if(!hasTorqueSensor()) + { throw std::runtime_error("Joint state handle does not contain torque sensor information"); } return *torque_sensor_; } - bool hasAbsolutePosition() const { - if(absolute_pos_){ - return true; + const double* getPositionPtr() const {return pos_;} + const double* getVelocityPtr() const {return vel_;} + const double* getEffortPtr() const {return eff_;} + + const double* getAbsolutePositionPtr() const + { + if(!hasAbsolutePosition()) + { + throw std::runtime_error("Joint state handle does not contain torque sensor information"); } - return false; + return absolute_pos_; } - bool hasTorqueSensor() const + const double* getTorqueSensorPtr() const { - if(torque_sensor_){ - return true; - } - else{ - return false; + if(!hasTorqueSensor()) + { + throw std::runtime_error("Joint state handle does not contain torque sensor information"); } + return torque_sensor_; } + bool hasAbsolutePosition() const {return absolute_pos_;} + bool hasTorqueSensor() const {return torque_sensor_;} + private: std::string name_; const double* pos_; const double* vel_; const double* eff_; - const double* absolute_pos_; const double* torque_sensor_; }; diff --git a/transmission_interface/include/transmission_interface/differential_transmission.h b/transmission_interface/include/transmission_interface/differential_transmission.h index 45cc86451..39c6928a9 100644 --- a/transmission_interface/include/transmission_interface/differential_transmission.h +++ b/transmission_interface/include/transmission_interface/differential_transmission.h @@ -125,6 +125,18 @@ class DifferentialTransmission : public Transmission */ DifferentialTransmission(const std::vector& actuator_reduction, const std::vector& joint_reduction, + const std::vector& joint_offset); + + /** + * \param actuator_reduction Reduction ratio of actuators. + * \param joint_reduction Reduction ratio of joints. + * \param ignore_transmission_for_absolute_encoders Whether to take the data directly from absolute encoders instead of calculating through transmission. + * \param joint_offset Joint position offset used in the position mappings. + * \pre Nonzero actuator and joint reduction values. + */ + DifferentialTransmission(const std::vector& actuator_reduction, + const std::vector& joint_reduction, + bool ignore_transmission_for_absolute_encoders = false, const std::vector& joint_offset = std::vector(2, 0.0)); /** @@ -157,11 +169,26 @@ class DifferentialTransmission : public Transmission void actuatorToJointPosition(const ActuatorData& act_data, JointData& jnt_data); + /** + * \brief Transform \e absolute encoder values from actuator to joint space. + * \param[in] act_data Actuator-space variables. + * \param[out] jnt_data Joint-space variables. + * \pre Actuator, joint position and absolute encoder position vectors must have the same size. + * To call this method it is not required that all other data vectors contain valid data, and can even remain empty. + */ void actuatorToJointAbsolutePosition(const ActuatorData& act_data, JointData& jnt_data); + /** + * \brief Transform \e torque sensor values from actuator to joint space. + * \param[in] act_data Actuator-space variables. + * \param[out] jnt_data Joint-space variables. + * \pre Actuator, joint position and torque sensor vectors must have the same size. + * To call this method it is not required that all other data vectors contain valid data, and can even remain empty. + */ void actuatorToJointTorqueSensor(const ActuatorData& act_data, - JointData& jnt_data); + JointData& jnt_data); + /** * \brief Transform \e effort variables from joint to actuator space. * \param[in] jnt_data Joint-space variables. @@ -194,6 +221,8 @@ class DifferentialTransmission : public Transmission std::size_t numActuators() const {return 2;} std::size_t numJoints() const {return 2;} + bool hasActuatorToJointAbsolutePosition() const {return true;} + bool hasActuatorToJointTorqueSensor() const {return true;} const std::vector& getActuatorReduction() const {return act_reduction_;} const std::vector& getJointReduction() const {return jnt_reduction_;} @@ -203,15 +232,24 @@ class DifferentialTransmission : public Transmission std::vector act_reduction_; std::vector jnt_reduction_; std::vector jnt_offset_; + bool ignore_transmission_for_absolute_encoders_; }; inline DifferentialTransmission::DifferentialTransmission(const std::vector& actuator_reduction, const std::vector& joint_reduction, const std::vector& joint_offset) + : DifferentialTransmission(actuator_reduction, joint_reduction, false, joint_offset) +{} + +inline DifferentialTransmission::DifferentialTransmission(const std::vector& actuator_reduction, + const std::vector& joint_reduction, + const bool ignore_transmission_for_absolute_encoders, + const std::vector& joint_offset) : Transmission(), act_reduction_(actuator_reduction), jnt_reduction_(joint_reduction), - jnt_offset_(joint_offset) + jnt_offset_(joint_offset), + ignore_transmission_for_absolute_encoders_(ignore_transmission_for_absolute_encoders) { if (numActuators() != act_reduction_.size() || numJoints() != jnt_reduction_.size() || @@ -223,8 +261,7 @@ inline DifferentialTransmission::DifferentialTransmission(const std::vector& ar = act_reduction_; - std::vector& jr = jnt_reduction_; + const std::vector& ar = act_reduction_; + const std::vector& jr = jnt_reduction_; *jnt_data.effort[0] = jr[0] * (*act_data.effort[0] * ar[0] + *act_data.effort[1] * ar[1]); *jnt_data.effort[1] = jr[1] * (*act_data.effort[0] * ar[0] - *act_data.effort[1] * ar[1]); @@ -249,8 +286,8 @@ inline void DifferentialTransmission::actuatorToJointVelocity(const ActuatorData assert(numActuators() == act_data.velocity.size() && numJoints() == jnt_data.velocity.size()); assert(act_data.velocity[0] && act_data.velocity[1] && jnt_data.velocity[0] && jnt_data.velocity[1]); - std::vector& ar = act_reduction_; - std::vector& jr = jnt_reduction_; + const std::vector& ar = act_reduction_; + const std::vector& jr = jnt_reduction_; *jnt_data.velocity[0] = (*act_data.velocity[0] / ar[0] + *act_data.velocity[1] / ar[1]) / (2.0 * jr[0]); *jnt_data.velocity[1] = (*act_data.velocity[0] / ar[0] - *act_data.velocity[1] / ar[1]) / (2.0 * jr[1]); @@ -262,34 +299,42 @@ inline void DifferentialTransmission::actuatorToJointPosition(const ActuatorData assert(numActuators() == act_data.position.size() && numJoints() == jnt_data.position.size()); assert(act_data.position[0] && act_data.position[1] && jnt_data.position[0] && jnt_data.position[1]); - std::vector& ar = act_reduction_; - std::vector& jr = jnt_reduction_; + const std::vector& ar = act_reduction_; + const std::vector& jr = jnt_reduction_; *jnt_data.position[0] = (*act_data.position[0] / ar[0] + *act_data.position[1] / ar[1]) / (2.0 * jr[0]) + jnt_offset_[0]; *jnt_data.position[1] = (*act_data.position[0] / ar[0] - *act_data.position[1] / ar[1]) / (2.0 * jr[1]) + jnt_offset_[1]; } inline void DifferentialTransmission::actuatorToJointAbsolutePosition(const ActuatorData& act_data, - JointData& jnt_data) + JointData& jnt_data) { assert(numActuators() == act_data.absolute_position.size() && numJoints() == jnt_data.absolute_position.size()); assert(act_data.position[0] && act_data.absolute_position[1] && jnt_data.absolute_position[0] && jnt_data.absolute_position[1]); - std::vector& ar = act_reduction_; - std::vector& jr = jnt_reduction_; + const std::vector& ar = act_reduction_; + const std::vector& jr = jnt_reduction_; - *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]; - *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]; + if(!ignore_transmission_for_absolute_encoders_) + { + *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]; + *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]; + } + else + { + *jnt_data.absolute_position[0] = *act_data.absolute_position[1]; + *jnt_data.absolute_position[1] = *act_data.absolute_position[0]; + } } inline void DifferentialTransmission::actuatorToJointTorqueSensor(const ActuatorData& act_data, - JointData& jnt_data) + JointData& jnt_data) { assert(numActuators() == act_data.torque_sensor.size() && numJoints() == jnt_data.torque_sensor.size()); assert(act_data.torque_sensor[0] && act_data.torque_sensor[1] && jnt_data.torque_sensor[0] && jnt_data.torque_sensor[1]); - std::vector& ar = act_reduction_; - std::vector& jr = jnt_reduction_; + const std::vector& ar = act_reduction_; + const std::vector& jr = jnt_reduction_; *jnt_data.torque_sensor[0] = jr[0] * (*act_data.torque_sensor[0] * ar[0] + *act_data.torque_sensor[1] * ar[1]); *jnt_data.torque_sensor[1] = jr[1] * (*act_data.torque_sensor[0] * ar[0] - *act_data.torque_sensor[1] * ar[1]); @@ -301,8 +346,8 @@ inline void DifferentialTransmission::jointToActuatorEffort(const JointData& assert(numActuators() == act_data.effort.size() && numJoints() == jnt_data.effort.size()); assert(act_data.effort[0] && act_data.effort[1] && jnt_data.effort[0] && jnt_data.effort[1]); - std::vector& ar = act_reduction_; - std::vector& jr = jnt_reduction_; + const std::vector& ar = act_reduction_; + const std::vector& jr = jnt_reduction_; *act_data.effort[0] = (*jnt_data.effort[0] / jr[0] + *jnt_data.effort[1] / jr[1]) / (2.0 * ar[0]); *act_data.effort[1] = (*jnt_data.effort[0] / jr[0] - *jnt_data.effort[1] / jr[1]) / (2.0 * ar[1]); @@ -314,8 +359,8 @@ inline void DifferentialTransmission::jointToActuatorVelocity(const JointData& assert(numActuators() == act_data.velocity.size() && numJoints() == jnt_data.velocity.size()); assert(act_data.velocity[0] && act_data.velocity[1] && jnt_data.velocity[0] && jnt_data.velocity[1]); - std::vector& ar = act_reduction_; - std::vector& jr = jnt_reduction_; + const std::vector& ar = act_reduction_; + const std::vector& jr = jnt_reduction_; *act_data.velocity[0] = (*jnt_data.velocity[0] * jr[0] + *jnt_data.velocity[1] * jr[1]) * ar[0]; *act_data.velocity[1] = (*jnt_data.velocity[0] * jr[0] - *jnt_data.velocity[1] * jr[1]) * ar[1]; @@ -327,8 +372,8 @@ inline void DifferentialTransmission::jointToActuatorPosition(const JointData& assert(numActuators() == act_data.position.size() && numJoints() == jnt_data.position.size()); assert(act_data.position[0] && act_data.position[1] && jnt_data.position[0] && jnt_data.position[1]); - std::vector& ar = act_reduction_; - std::vector& jr = jnt_reduction_; + const std::vector& ar = act_reduction_; + const std::vector& jr = jnt_reduction_; double jnt_pos_off[2] = {*jnt_data.position[0] - jnt_offset_[0], *jnt_data.position[1] - jnt_offset_[1]}; diff --git a/transmission_interface/include/transmission_interface/differential_transmission_loader.h b/transmission_interface/include/transmission_interface/differential_transmission_loader.h index aa326f62e..a53447f26 100644 --- a/transmission_interface/include/transmission_interface/differential_transmission_loader.h +++ b/transmission_interface/include/transmission_interface/differential_transmission_loader.h @@ -53,7 +53,8 @@ class DifferentialTransmissionLoader : public TransmissionLoader static bool getJointConfig(const TransmissionInfo& transmission_info, std::vector& joint_reduction, - std::vector& joint_offset); + std::vector& joint_offset, + bool& ignore_transmission_for_absolute_encoders); }; } // namespace diff --git a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.h b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.h index 23dc26e54..2b4254b37 100644 --- a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.h +++ b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.h @@ -156,12 +156,25 @@ class FourBarLinkageTransmission : public Transmission */ void actuatorToJointPosition(const ActuatorData& act_data, JointData& jnt_data); - + + /** + * \brief Transform \e absolute encoder values from actuator to joint space. + * \param[in] act_data Actuator-space variables. + * \param[out] jnt_data Joint-space variables. + * \pre Actuator, joint position and absolute encoder position vectors must have the same size. + * To call this method it is not required that all other data vectors contain valid data, and can even remain empty. + */ void actuatorToJointAbsolutePosition(const ActuatorData& act_data, JointData& jnt_data); + /** + * \brief Transform \e torque sensor values from actuator to joint space. + * \param[in] act_data Actuator-space variables. + * \param[out] jnt_data Joint-space variables. + * \pre Actuator, joint position and torque sensor vectors must have the same size. + */ void actuatorToJointTorqueSensor(const ActuatorData& act_data, - JointData& jnt_data); + JointData& jnt_data); /** * \brief Transform \e effort variables from joint to actuator space. @@ -195,6 +208,8 @@ class FourBarLinkageTransmission : public Transmission std::size_t numActuators() const {return 2;} std::size_t numJoints() const {return 2;} + bool hasActuatorToJointAbsolutePosition() const {return true;} + bool hasActuatorToJointTorqueSensor() const {return true;} const std::vector& getActuatorReduction() const {return act_reduction_;} const std::vector& getJointReduction() const {return jnt_reduction_;} @@ -270,27 +285,29 @@ inline void FourBarLinkageTransmission::actuatorToJointPosition(const ActuatorDa } void FourBarLinkageTransmission::actuatorToJointAbsolutePosition(const ActuatorData& act_data, - JointData& jnt_data){ + JointData& jnt_data) +{ assert(numActuators() == act_data.absolute_position.size() && numJoints() == jnt_data.absolute_position.size()); assert(act_data.absolute_position[0] && act_data.absolute_position[1] && jnt_data.absolute_position[0] && jnt_data.absolute_position[1]); - std::vector& ar = act_reduction_; - std::vector& jr = jnt_reduction_; + const std::vector& ar = act_reduction_; + const std::vector& jr = jnt_reduction_; *jnt_data.absolute_position[0] = *act_data.absolute_position[0] /(jr[0] * ar[0]) + jnt_offset_[0]; - *jnt_data.absolute_position[1] = (*act_data.absolute_position[1] / ar[1] - *act_data.absolute_position[0] / (jr[0] * ar[0])) / jr[1] - + jnt_offset_[1]; + *jnt_data.absolute_position[1] = (*act_data.absolute_position[1] / ar[1] - *act_data.absolute_position[0] / (jr[0] * ar[0])) / + jr[1] + jnt_offset_[1]; } void FourBarLinkageTransmission::actuatorToJointTorqueSensor(const ActuatorData& act_data, - JointData& jnt_data){ + JointData& jnt_data) +{ assert(numActuators() == act_data.torque_sensor.size() && numJoints() == jnt_data.torque_sensor.size()); assert(act_data.torque_sensor[0] && act_data.torque_sensor[1] && jnt_data.torque_sensor[0] && jnt_data.torque_sensor[1]); - std::vector& ar = act_reduction_; - std::vector& jr = jnt_reduction_; + const std::vector& ar = act_reduction_; + const std::vector& jr = jnt_reduction_; *jnt_data.torque_sensor[0] = jr[0] * (*act_data.torque_sensor[0] * ar[0]); *jnt_data.torque_sensor[1] = jr[1] * (*act_data.torque_sensor[1] * ar[1] - *act_data.torque_sensor[0] * ar[0] * jr[0]); diff --git a/transmission_interface/include/transmission_interface/simple_transmission.h b/transmission_interface/include/transmission_interface/simple_transmission.h index 74e0dbaa8..8b9ea82d4 100644 --- a/transmission_interface/include/transmission_interface/simple_transmission.h +++ b/transmission_interface/include/transmission_interface/simple_transmission.h @@ -137,7 +137,7 @@ class SimpleTransmission : public Transmission JointData& jnt_data); void actuatorToJointTorqueSensor(const ActuatorData& act_data, - JointData& jnt_data); + JointData& jnt_data); /** * \brief Transform \e effort variables from joint to actuator space. @@ -171,10 +171,13 @@ class SimpleTransmission : public Transmission std::size_t numActuators() const {return 1;} std::size_t numJoints() const {return 1;} + bool hasActuatorToJointAbsolutePosition() const {return true;} + bool hasActuatorToJointTorqueSensor() const {return true;} double getActuatorReduction() const {return reduction_;} double getJointOffset() const {return jnt_offset_;} + private: double reduction_; double jnt_offset_; @@ -220,7 +223,8 @@ inline void SimpleTransmission::actuatorToJointPosition(const ActuatorData& act_ } inline void SimpleTransmission::actuatorToJointAbsolutePosition(const ActuatorData& act_data, - JointData& jnt_data){ + JointData& jnt_data) +{ assert(numActuators() == act_data.absolute_position.size() && numJoints() == jnt_data.absolute_position.size()); assert(act_data.absolute_position[0] && jnt_data.absolute_position[0]); @@ -229,7 +233,8 @@ inline void SimpleTransmission::actuatorToJointAbsolutePosition(const ActuatorDa } inline void SimpleTransmission::actuatorToJointTorqueSensor(const ActuatorData& act_data, - JointData& jnt_data){ + JointData& jnt_data) +{ assert(numActuators() == act_data.torque_sensor.size() && numJoints() == jnt_data.torque_sensor.size()); assert(act_data.torque_sensor[0] && jnt_data.torque_sensor[0]); diff --git a/transmission_interface/include/transmission_interface/transmission.h b/transmission_interface/include/transmission_interface/transmission.h index fabe9fbed..b1abb8afe 100644 --- a/transmission_interface/include/transmission_interface/transmission.h +++ b/transmission_interface/include/transmission_interface/transmission.h @@ -126,16 +126,21 @@ class Transmission virtual void actuatorToJointPosition(const ActuatorData& act_data, JointData& jnt_data) = 0; - virtual void actuatorToJointAbsolutePosition(const ActuatorData& act_data, - JointData&){ + virtual void actuatorToJointAbsolutePosition(const ActuatorData&, + JointData&) + { throw std::runtime_error("transmission does not support actuator to joint absolute position"); } - virtual void actuatorToJointTorqueSensor(const ActuatorData& act_data, - JointData&){ + virtual void actuatorToJointTorqueSensor(const ActuatorData&, + JointData&) + { throw std::runtime_error("transmission does not support actuator to joint torque sensor"); } + virtual bool hasActuatorToJointAbsolutePosition() const {return false;} + virtual bool hasActuatorToJointTorqueSensor() const {return false;} + /** * \brief Transform \e effort variables from joint to actuator space. * \param[in] jnt_data Joint-space variables. @@ -173,7 +178,7 @@ class Transmission virtual std::size_t numActuators() const = 0; /** \return Number of joints managed by transmission, ie. the dimension of the joint space. */ - virtual std::size_t numJoints() const = 0; + virtual std::size_t numJoints() const = 0; }; typedef std::shared_ptr TransmissionSharedPtr; diff --git a/transmission_interface/include/transmission_interface/transmission_interface.h b/transmission_interface/include/transmission_interface/transmission_interface.h index 12f2aeb19..0a29c0f66 100644 --- a/transmission_interface/include/transmission_interface/transmission_interface.h +++ b/transmission_interface/include/transmission_interface/transmission_interface.h @@ -210,13 +210,14 @@ class ActuatorToJointStateHandle : public TransmissionHandle transmission_->actuatorToJointVelocity(actuator_data_, joint_data_); transmission_->actuatorToJointEffort(actuator_data_, joint_data_); - //if(!actuator_data_.absolute_position.empty() && transmission_->hasActuatorToJointAbsolutePosition()){ - if(!actuator_data_.absolute_position.empty()){ - transmission_->actuatorToJointAbsolutePosition(actuator_data_, joint_data_); + if(!actuator_data_.absolute_position.empty() && transmission_->hasActuatorToJointAbsolutePosition()) + { + transmission_->actuatorToJointAbsolutePosition(actuator_data_, joint_data_); } - // if(!actuator_data_.torque_sensor.empty() && transmission_->hasActuatorToJointTorqueSensor()){ - if(!actuator_data_.torque_sensor.empty()){ - transmission_->actuatorToJointTorqueSensor(actuator_data_, joint_data_); + + if(!actuator_data_.torque_sensor.empty() && transmission_->hasActuatorToJointTorqueSensor()) + { + transmission_->actuatorToJointTorqueSensor(actuator_data_, joint_data_); } } /*\}*/ diff --git a/transmission_interface/include/transmission_interface/transmission_interface_loader.h b/transmission_interface/include/transmission_interface/transmission_interface_loader.h index c010a1559..13ed05ceb 100644 --- a/transmission_interface/include/transmission_interface/transmission_interface_loader.h +++ b/transmission_interface/include/transmission_interface/transmission_interface_loader.h @@ -123,7 +123,6 @@ struct RawJointData double position_cmd; double velocity_cmd; double effort_cmd; - double absolute_position; double torque_sensor; diff --git a/transmission_interface/src/differential_transmission_loader.cpp b/transmission_interface/src/differential_transmission_loader.cpp index 599b2fb68..c4eaf87d3 100644 --- a/transmission_interface/src/differential_transmission_loader.cpp +++ b/transmission_interface/src/differential_transmission_loader.cpp @@ -53,9 +53,11 @@ DifferentialTransmissionLoader::load(const TransmissionInfo& transmission_info) std::vector jnt_reduction; std::vector jnt_offset; + bool ignore_transmission_for_absolute_encoders; const bool jnt_config_ok = getJointConfig(transmission_info, jnt_reduction, - jnt_offset); + jnt_offset, + ignore_transmission_for_absolute_encoders); if (!jnt_config_ok) {return TransmissionSharedPtr();} @@ -64,6 +66,7 @@ DifferentialTransmissionLoader::load(const TransmissionInfo& transmission_info) { TransmissionSharedPtr transmission(new DifferentialTransmission(act_reduction, jnt_reduction, + ignore_transmission_for_absolute_encoders, jnt_offset)); return transmission; } @@ -117,8 +120,8 @@ bool DifferentialTransmissionLoader::getActuatorConfig(const TransmissionInfo& t if (act_roles[0] == act_roles[1]) { ROS_ERROR_STREAM_NAMED("parser", "Actuators '" << act_names[0] << "' and '" << act_names[1] << - "' of transmission '" << transmission_info.name_ << - "' must have different roles. Both specify '" << act_roles[0] << "'."); + "' of transmission '" << transmission_info.name_ << + "' must have different roles. Both specify '" << act_roles[0] << "'."); return false; } @@ -154,7 +157,8 @@ bool DifferentialTransmissionLoader::getActuatorConfig(const TransmissionInfo& t bool DifferentialTransmissionLoader::getJointConfig(const TransmissionInfo& transmission_info, std::vector& joint_reduction, - std::vector& joint_offset) + std::vector& joint_offset, + bool& ignore_transmission_for_absolute_encoders) { const std::string JOINT1_ROLE = "joint1"; const std::string JOINT2_ROLE = "joint2"; @@ -194,8 +198,8 @@ bool DifferentialTransmissionLoader::getJointConfig(const TransmissionInfo& tran if (jnt_roles[0] == jnt_roles[1]) { ROS_ERROR_STREAM_NAMED("parser", "Joints '" << jnt_names[0] << "' and '" << jnt_names[1] << - "' of transmission '" << transmission_info.name_ << - "' must have different roles. Both specify '" << jnt_roles[0] << "'."); + "' of transmission '" << transmission_info.name_ << + "' must have different roles. Both specify '" << jnt_roles[0] << "'."); return false; } @@ -239,6 +243,14 @@ bool DifferentialTransmissionLoader::getJointConfig(const TransmissionInfo& tran if (offset_status == BAD_TYPE) {return false;} } + // Parse if transmision has to be ignored for absolute encoders + ignore_transmission_for_absolute_encoders = true; + for (unsigned int i = 0; i < 2; ++i) + { + const TiXmlElement* role_el = jnt_elements[i].FirstChildElement("ignoreTransmissionAbsoluteEncoder"); + ignore_transmission_for_absolute_encoders &= (role_el != nullptr); + } + return true; } diff --git a/transmission_interface/src/joint_state_interface_provider.cpp b/transmission_interface/src/joint_state_interface_provider.cpp index 027230836..8db4b776f 100644 --- a/transmission_interface/src/joint_state_interface_provider.cpp +++ b/transmission_interface/src/joint_state_interface_provider.cpp @@ -59,7 +59,8 @@ bool JointStateInterfaceProvider::updateJointInterfaces(const TransmissionInfo& // Update hardware interface using hardware_interface::JointStateHandle; RawJointData& raw_joint_data = raw_joint_data_map[name]; // Add joint if it does not yet exist - if(raw_joint_data.absolute_position && raw_joint_data.torque_sensor){ + if(raw_joint_data.hasAbsolutePosition && raw_joint_data.hasTorqueSensor) + { JointStateHandle handle(name, &raw_joint_data.position, &raw_joint_data.velocity, @@ -68,7 +69,8 @@ bool JointStateInterfaceProvider::updateJointInterfaces(const TransmissionInfo& &raw_joint_data.torque_sensor); interface.registerHandle(handle); } - else if(raw_joint_data.absolute_position){ + else if(raw_joint_data.hasAbsolutePosition) + { JointStateHandle handle(name, &raw_joint_data.position, &raw_joint_data.velocity, @@ -76,7 +78,8 @@ bool JointStateInterfaceProvider::updateJointInterfaces(const TransmissionInfo& &raw_joint_data.absolute_position); interface.registerHandle(handle); } - else if(raw_joint_data.torque_sensor){ + else if(raw_joint_data.hasTorqueSensor) + { JointStateHandle handle(name, &raw_joint_data.position, &raw_joint_data.velocity, @@ -84,7 +87,8 @@ bool JointStateInterfaceProvider::updateJointInterfaces(const TransmissionInfo& &raw_joint_data.torque_sensor, true); interface.registerHandle(handle); } - else{ + else + { JointStateHandle handle(name, &raw_joint_data.position, &raw_joint_data.velocity, @@ -119,11 +123,13 @@ bool JointStateInterfaceProvider::getJointStateData(const TransmissionInfo& tran hasTorqueSensor = hasTorqueSensor && raw_joint_data.hasTorqueSensor; } - if(hasAbsolutePosition){ + if(hasAbsolutePosition) + { jnt_state_data.absolute_position.resize(dim); } - if(hasTorqueSensor){ + if(hasTorqueSensor) + { jnt_state_data.torque_sensor.resize(dim); } @@ -138,13 +144,14 @@ bool JointStateInterfaceProvider::getJointStateData(const TransmissionInfo& tran jnt_state_data.position[i] = const_cast(&(raw_joint_data.position)); jnt_state_data.velocity[i] = const_cast(&(raw_joint_data.velocity)); jnt_state_data.effort[i] = const_cast(&(raw_joint_data.effort)); - if(hasAbsolutePosition){ - jnt_state_data.absolute_position[i] = const_cast(&(raw_joint_data.absolute_position)); + if(hasAbsolutePosition) + { + jnt_state_data.absolute_position[i] = const_cast(&(raw_joint_data.absolute_position)); } - if(hasTorqueSensor){ - jnt_state_data.torque_sensor[i] = const_cast(&(raw_joint_data.torque_sensor)); + if(hasTorqueSensor) + { + jnt_state_data.torque_sensor[i] = const_cast(&(raw_joint_data.torque_sensor)); } - } return true; @@ -171,15 +178,20 @@ bool JointStateInterfaceProvider::getActuatorStateData(const TransmissionInfo& bool hasAbsolutePositionInterface = true; bool hasTorqueSensorInterface = true; - for (unsigned int i = 0; i < dim; ++i){ + + for (unsigned int i = 0; i < dim; ++i) + { hasAbsolutePositionInterface = hasAbsolutePositionInterface && handles[i].hasAbsolutePosition(); hasTorqueSensorInterface = hasTorqueSensorInterface && handles[i].hasTorqueSensor(); } - if(hasAbsolutePositionInterface){ + if(hasAbsolutePositionInterface) + { act_state_data.absolute_position.resize(dim); } - if(hasTorqueSensorInterface){ + + if(hasTorqueSensorInterface) + { act_state_data.torque_sensor.resize(dim); } @@ -189,11 +201,13 @@ bool JointStateInterfaceProvider::getActuatorStateData(const TransmissionInfo& act_state_data.position[i] = const_cast(handles[i].getPositionPtr()); act_state_data.velocity[i] = const_cast(handles[i].getVelocityPtr()); act_state_data.effort[i] = const_cast(handles[i].getEffortPtr()); - if(hasAbsolutePositionInterface){ + if(hasAbsolutePositionInterface) + { act_state_data.absolute_position[i] = const_cast(handles[i].getAbsolutePositionPtr()); } - if(hasTorqueSensorInterface){ - act_state_data.torque_sensor[i] = const_cast(handles[i].getTorqueSensorPtr()); + if(hasTorqueSensorInterface) + { + act_state_data.torque_sensor[i] = const_cast(handles[i].getTorqueSensorPtr()); } } return true; From 49512bf4a67ac56861d3495fb38d2c89d500f790 Mon Sep 17 00:00:00 2001 From: Hilario Tome Date: Wed, 9 Nov 2016 11:37:46 +0100 Subject: [PATCH 7/7] Extend joint mode interface --- .../hardware_interface/joint_mode_interface.h | 109 ++++++++++-------- 1 file changed, 59 insertions(+), 50 deletions(-) diff --git a/hardware_interface/include/hardware_interface/joint_mode_interface.h b/hardware_interface/include/hardware_interface/joint_mode_interface.h index 1e5f0ab1c..258ff0333 100644 --- a/hardware_interface/include/hardware_interface/joint_mode_interface.h +++ b/hardware_interface/include/hardware_interface/joint_mode_interface.h @@ -34,11 +34,10 @@ /* Author: Dave Coleman Desc: This interface is for switching a hardware interface between different controller modes - i.e. position, velocity, force + i.e. position, velocity, effort */ -#ifndef HARDWARE_INTERFACE_JOINT_MODE_INTERFACE_H -#define HARDWARE_INTERFACE_JOINT_MODE_INTERFACE_H +#pragma once #include #include @@ -46,64 +45,74 @@ namespace hardware_interface { -enum JointCommandModes { - MODE_POSITION = 1, - MODE_VELOCITY = 2, - MODE_EFFORT = 3, - MODE_OTHER = 4 -}; - -/** \brief A handle used to read and mode a single joint. */ -class JointModeHandle -{ -public: + enum class JointCommandModes + { + BEGIN = -1, + MODE_POSITION = 0, + MODE_VELOCITY = 1, + MODE_EFFORT = 2, + NOMODE = 3, + EMERGENCY_STOP = 4, + SWITCHING = 5, + ERROR = 6 + }; - /** - * \param mode Which mode to start in - */ - JointModeHandle(std::string name, JointCommandModes* mode) - : mode_(mode) - , name_(name) + /** \brief A handle used to read and mode a single joint. */ + class JointModeHandle { - if (!mode_) + public: + + JointModeHandle(): + name_(), mode_(0){} + + /** \param mode Which mode to start in */ + JointModeHandle(std::string name, JointCommandModes* mode) + : mode_(mode) + , name_(name) { - throw HardwareInterfaceException("Cannot create mode interface. Mode data pointer is null."); + if (!mode_) + { + throw HardwareInterfaceException("Cannot create mode interface. Mode data pointer is null."); + } } - } - std::string getName() const {return name_;} + std::string getName() const {return name_;} void setMode(JointCommandModes mode) {assert(mode_); *mode_ = mode;} - int getMode() const {assert(mode_); return *mode_;} - const int* getModePtr() const {assert(mode_); return mode_;} + JointCommandModes getMode() const {assert(mode_); return *mode_;} + const JointCommandModes* getModePtr() const {assert(mode_); return mode_;} - // Helper function for console messages - std::string getModeName(JointCommandModes mode) - { - switch(mode) + // Helper function for console messages + std::string getModeName(JointCommandModes mode) const { - case MODE_POSITION: - return "position"; - case MODE_VELOCITY: - return "velocity"; - case MODE_EFFORT: - return "effort"; - case MODE_OTHER: - return "other"; + switch(mode) + { + case JointCommandModes::BEGIN: + return "not_operational"; + case JointCommandModes::MODE_POSITION: + return "position"; + case JointCommandModes::MODE_VELOCITY: + return "velocity"; + case JointCommandModes::MODE_EFFORT: + return "effort"; + case JointCommandModes::NOMODE: + return "other"; + case JointCommandModes::EMERGENCY_STOP: + return "emergency_stop"; + case JointCommandModes::SWITCHING: + return "switching"; + case JointCommandModes::ERROR: + return "error"; + } + return "unknown"; } - return "unknown"; - } -private: - JointCommandModes* mode_; - std::string name_; -}; + private: + JointCommandModes* mode_; + std::string name_; + }; -/** \brief Hardware interface to support changing between control modes - * - */ -class JointModeInterface : public HardwareResourceManager {}; + /** \brief Hardware interface to support changing between control modes */ + class JointModeInterface : public HardwareResourceManager {}; } // namespace - -#endif