From ecaf7d0cd6e2521684ca32212ede0d06f717ea0b Mon Sep 17 00:00:00 2001 From: Kirsty Date: Tue, 19 Nov 2019 15:23:34 +0000 Subject: [PATCH 01/27] adding control mode to actuator state --- .../include/ros_ethercat_model/hardware_interface.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp b/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp index f6a9aa4..160766d 100644 --- a/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp +++ b/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp @@ -37,6 +37,7 @@ #include #include +#include #include @@ -78,7 +79,6 @@ class ActuatorState double clutch_position_; //!< Position of output of actuator, distally to the clutch. - double last_commanded_current_; //!< Current computed based on effort specified in ActuatorCommand (in amps) double last_measured_current_; //!< The measured current (in amps) @@ -88,6 +88,7 @@ class ActuatorState double max_effort_; //!< Absolute torque limit for actuator (derived from motor current limit). (in Nm) double motor_voltage_; //!< Motor voltage (in volts) + hardware_interface::ActuatorCommandMode control_mode_; // switch between pwm and effort }; class ActuatorCommand From 98fdeb098d53bcd00d173c7fc2bbfd79525a75c3 Mon Sep 17 00:00:00 2001 From: Kirsty Date: Tue, 19 Nov 2019 15:26:30 +0000 Subject: [PATCH 02/27] pwm default --- .../include/ros_ethercat_model/hardware_interface.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp b/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp index 160766d..cec473f 100644 --- a/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp +++ b/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp @@ -61,7 +61,8 @@ class ActuatorState last_measured_effort_(0.0), max_effort_(0.0), motor_voltage_(0.0), - flags_(0) + flags_(0), + control_mode_(hardware_interface::PWM) { } From 73e7a50f3d2af4d316fe89cdf1eb2668c2bf6e42 Mon Sep 17 00:00:00 2001 From: Kirsty Date: Fri, 22 Nov 2019 17:08:19 +0000 Subject: [PATCH 03/27] moving command interface changes --- .../actuator_command_interface.h | 84 +++++++++++++++++++ .../ros_ethercat_model/hardware_interface.hpp | 5 +- 2 files changed, 87 insertions(+), 2 deletions(-) create mode 100644 ros_ethercat_model/include/ros_ethercat_model/actuator_command_interface.h diff --git a/ros_ethercat_model/include/ros_ethercat_model/actuator_command_interface.h b/ros_ethercat_model/include/ros_ethercat_model/actuator_command_interface.h new file mode 100644 index 0000000..ec4c591 --- /dev/null +++ b/ros_ethercat_model/include/ros_ethercat_model/actuator_command_interface.h @@ -0,0 +1,84 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2012, hiDOF INC. +// Copyright (C) 2013, PAL Robotics S.L. +// +// 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 hiDOF, Inc. 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. +////////////////////////////////////////////////////////////////////////////// + +#ifndef ROS_ETHERCAT_MODEL_ACTUATOR_COMMAND_INTERFACE_H +#define ROS_ETHERCAT_MODEL_ACTUATOR_COMMAND_INTERFACE_H + +#include + +namespace ros_ethercat_model +{ + +typedef enum ActuatorCommandMode +{ + COMMAND_MODE_PWM = 0, + COMMAND_MODE_EFFORT = 1 +} +ActuatorCommandMode; + +inline std::vector command_types_to_string() +{ + std::vector command_type_strings; + + command_type_strings.push_back("pwm"); + command_type_strings.push_back("effort"); + + return command_type_strings; +} + + +/** \brief A handle used to read and command a single actuator. */ +class ActuatorHandle : public hardware_interface::ActuatorHandle +{ +public: + ActuatorHandle() : hardware_interface::ActuatorHandle(), cmd_(0), cmd_type_(0) {} + ActuatorHandle(const ActuatorStateHandle& as, double* cmd, ActuatorCommandMode* cmd_type = 0) + : hardware_interface::ActuatorHandle(as, cmd), cmd_(0), cmd_type_(0) {} + + void setCommand(double command, ActuatorCommandMode command_type) {assert(cmd_); *cmd_ = command; *cmd_type_ = command_type;} + +private: + double* cmd_; + ActuatorCommandMode* cmd_type_; +}; + +class ActuatorCommandInterface : public hardware_interface::HardwareResourceManager {}; + +/// \ref ActuatorCommandInterface for commanding effort-based actuators +class EffortActuatorInterface : public ActuatorCommandInterface {}; + +/// \ref ActuatorCommandInterface for commanding velocity-based actuators +class VelocityActuatorInterface : public ActuatorCommandInterface {}; + +/// \ref ActuatorCommandInterface for commanding position-based actuators +class PositionActuatorInterface : public ActuatorCommandInterface {}; + + +} + +#endif // ROS_ETHERCAT_MODEL_ACTUATOR_COMMAND_INTERFACE_H diff --git a/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp b/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp index cec473f..ced1aa3 100644 --- a/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp +++ b/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp @@ -38,6 +38,7 @@ #include #include #include +#include #include @@ -62,7 +63,7 @@ class ActuatorState max_effort_(0.0), motor_voltage_(0.0), flags_(0), - control_mode_(hardware_interface::PWM) + control_mode_(COMMAND_MODE_PWM) { } @@ -89,7 +90,7 @@ class ActuatorState double max_effort_; //!< Absolute torque limit for actuator (derived from motor current limit). (in Nm) double motor_voltage_; //!< Motor voltage (in volts) - hardware_interface::ActuatorCommandMode control_mode_; // switch between pwm and effort + ActuatorCommandMode control_mode_; // switch between pwm and effort }; class ActuatorCommand From b5dc3afcc8cca275516f8741168e1ea774536132 Mon Sep 17 00:00:00 2001 From: Kirsty Date: Mon, 25 Nov 2019 12:12:06 +0000 Subject: [PATCH 04/27] fix --- .../include/ros_ethercat_model/actuator_command_interface.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros_ethercat_model/include/ros_ethercat_model/actuator_command_interface.h b/ros_ethercat_model/include/ros_ethercat_model/actuator_command_interface.h index ec4c591..8d722a3 100644 --- a/ros_ethercat_model/include/ros_ethercat_model/actuator_command_interface.h +++ b/ros_ethercat_model/include/ros_ethercat_model/actuator_command_interface.h @@ -58,7 +58,7 @@ class ActuatorHandle : public hardware_interface::ActuatorHandle public: ActuatorHandle() : hardware_interface::ActuatorHandle(), cmd_(0), cmd_type_(0) {} ActuatorHandle(const ActuatorStateHandle& as, double* cmd, ActuatorCommandMode* cmd_type = 0) - : hardware_interface::ActuatorHandle(as, cmd), cmd_(0), cmd_type_(0) {} + : hardware_interface::ActuatorHandle(as, cmd), cmd_(cmd), cmd_type_(cmd_type) {} void setCommand(double command, ActuatorCommandMode command_type) {assert(cmd_); *cmd_ = command; *cmd_type_ = command_type;} From e1558f6e95f94387207136dd331650d5fa33a78b Mon Sep 17 00:00:00 2001 From: Kirsty Date: Mon, 25 Nov 2019 14:12:54 +0000 Subject: [PATCH 05/27] remove unused include --- .../include/ros_ethercat_model/hardware_interface.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp b/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp index ced1aa3..64b0af1 100644 --- a/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp +++ b/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp @@ -37,7 +37,6 @@ #include #include -#include #include #include From 14246dbf84bd98c18a7f07850b83474ac44a2e87 Mon Sep 17 00:00:00 2001 From: Kirsty Date: Mon, 25 Nov 2019 14:19:52 +0000 Subject: [PATCH 06/27] lint --- .../ros_ethercat_model/actuator_command_interface.h | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/ros_ethercat_model/include/ros_ethercat_model/actuator_command_interface.h b/ros_ethercat_model/include/ros_ethercat_model/actuator_command_interface.h index 8d722a3..c4624b6 100644 --- a/ros_ethercat_model/include/ros_ethercat_model/actuator_command_interface.h +++ b/ros_ethercat_model/include/ros_ethercat_model/actuator_command_interface.h @@ -30,6 +30,8 @@ #define ROS_ETHERCAT_MODEL_ACTUATOR_COMMAND_INTERFACE_H #include +#include +#include namespace ros_ethercat_model { @@ -60,7 +62,8 @@ class ActuatorHandle : public hardware_interface::ActuatorHandle ActuatorHandle(const ActuatorStateHandle& as, double* cmd, ActuatorCommandMode* cmd_type = 0) : hardware_interface::ActuatorHandle(as, cmd), cmd_(cmd), cmd_type_(cmd_type) {} - void setCommand(double command, ActuatorCommandMode command_type) {assert(cmd_); *cmd_ = command; *cmd_type_ = command_type;} + void setCommand(double command, ActuatorCommandMode command_type) {assert(cmd_); *cmd_ = command; + *cmd_type_ = command_type;} private: double* cmd_; @@ -79,6 +82,6 @@ class VelocityActuatorInterface : public ActuatorCommandInterface {}; class PositionActuatorInterface : public ActuatorCommandInterface {}; -} +} // namespace ros_ethercat_model #endif // ROS_ETHERCAT_MODEL_ACTUATOR_COMMAND_INTERFACE_H From a5e0d9bb24aff5ce2a39581de9d5467bf2dad65c Mon Sep 17 00:00:00 2001 From: Kirsty Date: Mon, 25 Nov 2019 14:25:14 +0000 Subject: [PATCH 07/27] formatting of method braces --- .../ros_ethercat_model/actuator_command_interface.h | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/ros_ethercat_model/include/ros_ethercat_model/actuator_command_interface.h b/ros_ethercat_model/include/ros_ethercat_model/actuator_command_interface.h index c4624b6..ff67bcf 100644 --- a/ros_ethercat_model/include/ros_ethercat_model/actuator_command_interface.h +++ b/ros_ethercat_model/include/ros_ethercat_model/actuator_command_interface.h @@ -62,8 +62,11 @@ class ActuatorHandle : public hardware_interface::ActuatorHandle ActuatorHandle(const ActuatorStateHandle& as, double* cmd, ActuatorCommandMode* cmd_type = 0) : hardware_interface::ActuatorHandle(as, cmd), cmd_(cmd), cmd_type_(cmd_type) {} - void setCommand(double command, ActuatorCommandMode command_type) {assert(cmd_); *cmd_ = command; - *cmd_type_ = command_type;} + void setCommand(double command, ActuatorCommandMode command_type) + { + assert(cmd_); *cmd_ = command; + *cmd_type_ = command_type; + } private: double* cmd_; From 2b8ef730a841c183ebbef691877304bacd9666cf Mon Sep 17 00:00:00 2001 From: Kirsty Date: Mon, 25 Nov 2019 15:00:27 +0000 Subject: [PATCH 08/27] copyright and command mode name change --- .../actuator_command_interface.h | 35 ++++--------------- .../ros_ethercat_model/hardware_interface.hpp | 4 +-- 2 files changed, 8 insertions(+), 31 deletions(-) diff --git a/ros_ethercat_model/include/ros_ethercat_model/actuator_command_interface.h b/ros_ethercat_model/include/ros_ethercat_model/actuator_command_interface.h index ff67bcf..00fbab0 100644 --- a/ros_ethercat_model/include/ros_ethercat_model/actuator_command_interface.h +++ b/ros_ethercat_model/include/ros_ethercat_model/actuator_command_interface.h @@ -1,30 +1,7 @@ -/////////////////////////////////////////////////////////////////////////////// -// Copyright (C) 2012, hiDOF INC. -// Copyright (C) 2013, PAL Robotics S.L. -// -// 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 hiDOF, Inc. 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. -////////////////////////////////////////////////////////////////////////////// +/* +* Copyright (C) 2019 Shadow Robot Company Ltd - All Rights Reserved. Proprietary and Confidential. +* Unauthorized copying of the content in this file, via any medium is strictly prohibited. +*/ #ifndef ROS_ETHERCAT_MODEL_ACTUATOR_COMMAND_INTERFACE_H #define ROS_ETHERCAT_MODEL_ACTUATOR_COMMAND_INTERFACE_H @@ -38,8 +15,8 @@ namespace ros_ethercat_model typedef enum ActuatorCommandMode { - COMMAND_MODE_PWM = 0, - COMMAND_MODE_EFFORT = 1 + COMMAND_TYPE_PWM = 0, + COMMAND_TYPE_EFFORT = 1 } ActuatorCommandMode; diff --git a/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp b/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp index d192743..2c92042 100644 --- a/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp +++ b/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp @@ -62,7 +62,7 @@ class ActuatorState max_effort_(0.0), motor_voltage_(0.0), flags_(0), - control_mode_(COMMAND_MODE_PWM) + command_type_(COMMAND_TYPE_PWM) { } @@ -89,7 +89,7 @@ class ActuatorState double max_effort_; //!< Absolute torque limit for actuator (derived from motor current limit). (in Nm) double motor_voltage_; //!< Motor voltage (in volts) - ActuatorCommandMode control_mode_; // switch between pwm and effort + ActuatorCommandMode command_type_; // switch between pwm and effort }; class ActuatorCommand From c9311d9570761bee1fd13ffcb9558e725eb22b6b Mon Sep 17 00:00:00 2001 From: dg-shadow Date: Thu, 20 Feb 2020 12:35:45 +0000 Subject: [PATCH 09/27] added raw position field --- ros_ethercat_model/include/ros_ethercat_model/joint.hpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ros_ethercat_model/include/ros_ethercat_model/joint.hpp b/ros_ethercat_model/include/ros_ethercat_model/joint.hpp index 8d54e4b..5cb0be4 100644 --- a/ros_ethercat_model/include/ros_ethercat_model/joint.hpp +++ b/ros_ethercat_model/include/ros_ethercat_model/joint.hpp @@ -149,6 +149,9 @@ class JointState /// The joint position in radians or meters (read-only variable) double position_; + // The raw value coming from the position sensor + int position_raw_; + /// The joint velocity in radians/sec or meters/sec (read-only variable) double velocity_; @@ -176,6 +179,7 @@ class JointState /// Constructor JointState() : position_(0.0), + position_raw_(0), velocity_(0.0), effort_(0.0), commanded_position_(0.0), From cc06ba0bf0abcf9d4ac8d7a3773df69540fbfbda Mon Sep 17 00:00:00 2001 From: dg-shadow Date: Thu, 20 Feb 2020 12:47:26 +0000 Subject: [PATCH 10/27] added bsd license --- .../actuator_command_interface.h | 36 +++++++++++++++++-- 1 file changed, 34 insertions(+), 2 deletions(-) diff --git a/ros_ethercat_model/include/ros_ethercat_model/actuator_command_interface.h b/ros_ethercat_model/include/ros_ethercat_model/actuator_command_interface.h index 00fbab0..7d9151a 100644 --- a/ros_ethercat_model/include/ros_ethercat_model/actuator_command_interface.h +++ b/ros_ethercat_model/include/ros_ethercat_model/actuator_command_interface.h @@ -1,6 +1,38 @@ /* -* Copyright (C) 2019 Shadow Robot Company Ltd - All Rights Reserved. Proprietary and Confidential. -* Unauthorized copying of the content in this file, via any medium is strictly prohibited. +* +* Software License Agreement (BSD License) +* +* Copyright (c) 2020, Shadow Robot Company Ltd. +* 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 Willow Garage 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. +*********************************************************************/ + */ #ifndef ROS_ETHERCAT_MODEL_ACTUATOR_COMMAND_INTERFACE_H From be66706c29835ff88b4a7b5053e1a7d820b863da Mon Sep 17 00:00:00 2001 From: dg-shadow Date: Thu, 20 Feb 2020 13:40:03 +0000 Subject: [PATCH 11/27] empty commit to force rebuild From 94be80a1120c1836dc1cb7af762bcb5387409774 Mon Sep 17 00:00:00 2001 From: dg-shadow Date: Thu, 20 Feb 2020 13:55:39 +0000 Subject: [PATCH 12/27] wasn't building --- .../include/ros_ethercat_model/actuator_command_interface.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/ros_ethercat_model/include/ros_ethercat_model/actuator_command_interface.h b/ros_ethercat_model/include/ros_ethercat_model/actuator_command_interface.h index 7d9151a..1e5353c 100644 --- a/ros_ethercat_model/include/ros_ethercat_model/actuator_command_interface.h +++ b/ros_ethercat_model/include/ros_ethercat_model/actuator_command_interface.h @@ -33,8 +33,6 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -*/ - #ifndef ROS_ETHERCAT_MODEL_ACTUATOR_COMMAND_INTERFACE_H #define ROS_ETHERCAT_MODEL_ACTUATOR_COMMAND_INTERFACE_H From 7d259b2ad310832385838eb63562a82cbf3ad8f4 Mon Sep 17 00:00:00 2001 From: dg-shadow Date: Tue, 25 Feb 2020 10:39:31 +0000 Subject: [PATCH 13/27] revert to correct default behaviour --- ros_ethercat_model/launch/joint_state_publisher.launch | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/ros_ethercat_model/launch/joint_state_publisher.launch b/ros_ethercat_model/launch/joint_state_publisher.launch index 1d93e74..ad181ad 100644 --- a/ros_ethercat_model/launch/joint_state_publisher.launch +++ b/ros_ethercat_model/launch/joint_state_publisher.launch @@ -1,8 +1,11 @@ + + - + + From 1293ff1fc7ced3196c20dabb49646729781cd337 Mon Sep 17 00:00:00 2001 From: dg Date: Tue, 28 Apr 2020 15:23:08 +0000 Subject: [PATCH 14/27] added init macro --- .../include/ros_ethercat_model/imu_state.hpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/ros_ethercat_model/include/ros_ethercat_model/imu_state.hpp b/ros_ethercat_model/include/ros_ethercat_model/imu_state.hpp index 7872d57..73ea956 100644 --- a/ros_ethercat_model/include/ros_ethercat_model/imu_state.hpp +++ b/ros_ethercat_model/include/ros_ethercat_model/imu_state.hpp @@ -43,6 +43,8 @@ #include #include +#define INIT_ARRAY(array, x) for (size_t n = 0; n < (sizeof(array) / sizeof(array[0])); ++n) { array[n] = x; } + using std::string; namespace ros_ethercat_model @@ -71,6 +73,17 @@ class ImuState data_.linear_acceleration_covariance = linear_acceleration_covariance_; }; ImuState(){} + + void initialiseToZero(void) + { + INIT_ARRAY(orientation_, 0.0); + INIT_ARRAY(angular_velocity_, 0.0); + INIT_ARRAY(linear_acceleration_, 0.0); + INIT_ARRAY(orientation_covariance_, 0.0); + INIT_ARRAY(angular_velocity_covariance_, 0.0); + INIT_ARRAY(linear_acceleration_covariance_, 0.0); + } + }; }; // namespace ros_ethercat_model From fe5ef643aabd517aa81dd8c639d1f70ef9b1f129 Mon Sep 17 00:00:00 2001 From: dg-shadow Date: Tue, 23 Jun 2020 11:36:29 +0100 Subject: [PATCH 15/27] Every time it builds it's a triumph --- .../actuator_command_interface.h | 34 +++++++++---------- .../ros_ethercat_model/hardware_interface.hpp | 21 +++++++++++- 2 files changed, 36 insertions(+), 19 deletions(-) diff --git a/ros_ethercat_model/include/ros_ethercat_model/actuator_command_interface.h b/ros_ethercat_model/include/ros_ethercat_model/actuator_command_interface.h index 1e5353c..7638fe9 100644 --- a/ros_ethercat_model/include/ros_ethercat_model/actuator_command_interface.h +++ b/ros_ethercat_model/include/ros_ethercat_model/actuator_command_interface.h @@ -37,29 +37,14 @@ #define ROS_ETHERCAT_MODEL_ACTUATOR_COMMAND_INTERFACE_H #include +#include + #include #include -namespace ros_ethercat_model -{ - -typedef enum ActuatorCommandMode -{ - COMMAND_TYPE_PWM = 0, - COMMAND_TYPE_EFFORT = 1 -} -ActuatorCommandMode; -inline std::vector command_types_to_string() +namespace ros_ethercat_model { - std::vector command_type_strings; - - command_type_strings.push_back("pwm"); - command_type_strings.push_back("effort"); - - return command_type_strings; -} - /** \brief A handle used to read and command a single actuator. */ class ActuatorHandle : public hardware_interface::ActuatorHandle @@ -69,13 +54,26 @@ class ActuatorHandle : public hardware_interface::ActuatorHandle ActuatorHandle(const ActuatorStateHandle& as, double* cmd, ActuatorCommandMode* cmd_type = 0) : hardware_interface::ActuatorHandle(as, cmd), cmd_(cmd), cmd_type_(cmd_type) {} + void setCommand(double command, ActuatorCommandMode command_type) { assert(cmd_); *cmd_ = command; *cmd_type_ = command_type; } + void setState(ActuatorState* as) + { + a_ = as; + } + + ActuatorState * getState(void) + { + assert(a_); + return a_; + } + private: + ActuatorState *a_; double* cmd_; ActuatorCommandMode* cmd_type_; }; diff --git a/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp b/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp index 2c92042..7346322 100644 --- a/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp +++ b/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp @@ -37,7 +37,7 @@ #include #include -#include +//#include #include @@ -45,6 +45,25 @@ namespace ros_ethercat_model { +typedef enum ActuatorCommandMode +{ + COMMAND_TYPE_PWM = 0, + COMMAND_TYPE_EFFORT = 1 +} +ActuatorCommandMode; + +inline std::vector command_types_to_string() +{ + std::vector command_type_strings; + + command_type_strings.push_back("pwm"); + command_type_strings.push_back("effort"); + + return command_type_strings; +} + + + class ActuatorState { public: From a980de351252bbcce3f08739012787bb4a98c633 Mon Sep 17 00:00:00 2001 From: dg-shadow Date: Tue, 7 Jul 2020 09:11:53 +0100 Subject: [PATCH 16/27] added pwm field --- .../include/ros_ethercat_model/hardware_interface.hpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp b/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp index 7346322..a9c3d11 100644 --- a/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp +++ b/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp @@ -81,6 +81,7 @@ class ActuatorState max_effort_(0.0), motor_voltage_(0.0), flags_(0), + pwm_(0), command_type_(COMMAND_TYPE_PWM) { } @@ -95,8 +96,8 @@ class ActuatorState double commanded_effort_; double temperature_; //!< Measured motor temperature in degrees C - unsigned int flags_; //!< Motor state - + unsigned int flags_; //!< Motor info glags + signed int pwm_; //!< PWM currently applied to motor double clutch_position_; //!< Position of output of actuator, distally to the clutch. double last_commanded_current_; //!< Current computed based on effort specified in ActuatorCommand (in amps) From 4e93b9d5d841231bad642b5f487b93970249edbb Mon Sep 17 00:00:00 2001 From: dg-shadow Date: Tue, 11 Aug 2020 13:29:20 +0100 Subject: [PATCH 17/27] added place holder to state --- .../include/ros_ethercat_model/hardware_interface.hpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp b/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp index a9c3d11..4dcb09f 100644 --- a/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp +++ b/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp @@ -62,6 +62,11 @@ inline std::vector command_types_to_string() return command_type_strings; } +typedef struct __attribute__((__packed__)) ActuatorOdometry +{ + volatile uint32_t odo_1; + volatile uint32_t odo_2; +} ActuatorOdometry; class ActuatorState @@ -82,7 +87,9 @@ class ActuatorState motor_voltage_(0.0), flags_(0), pwm_(0), + command_type_(COMMAND_TYPE_PWM) + { } @@ -110,6 +117,7 @@ class ActuatorState double motor_voltage_; //!< Motor voltage (in volts) ActuatorCommandMode command_type_; // switch between pwm and effort + ActuatorOdometry odometry_; }; class ActuatorCommand From 8a46604b7d5289888b48b40f4d3fcb53c4f3462e Mon Sep 17 00:00:00 2001 From: dg-shadow Date: Thu, 8 Oct 2020 07:48:06 +0100 Subject: [PATCH 18/27] added extra fields --- ros_ethercat_hardware/src/ethercat_hardware.cpp | 2 ++ ros_ethercat_loop/src/main.cpp | 2 +- .../include/ros_ethercat_model/hardware_interface.hpp | 2 ++ 3 files changed, 5 insertions(+), 1 deletion(-) diff --git a/ros_ethercat_hardware/src/ethercat_hardware.cpp b/ros_ethercat_hardware/src/ethercat_hardware.cpp index 646a223..69651bb 100644 --- a/ros_ethercat_hardware/src/ethercat_hardware.cpp +++ b/ros_ethercat_hardware/src/ethercat_hardware.cpp @@ -1030,6 +1030,7 @@ void EthercatHardware::loadNonEthercatDevices() void EthercatHardware::collectDiagnostics() { + ROS_WARN_STREAM("collect diagnostics"); if (NULL == oob_com_) return; @@ -1051,6 +1052,7 @@ void EthercatHardware::collectDiagnostics() // Worry about locking for single value? diagnostics_.device_count_ = status.get_adp(); + } for (unsigned i = 0; i < slaves_.size(); ++i) diff --git a/ros_ethercat_loop/src/main.cpp b/ros_ethercat_loop/src/main.cpp index 61832fb..d7428aa 100644 --- a/ros_ethercat_loop/src/main.cpp +++ b/ros_ethercat_loop/src/main.cpp @@ -207,6 +207,7 @@ void *diagnosticLoop(void *args) ptr_vector* ec = (ptr_vector*) args; struct timespec tick; clock_gettime(CLOCK_MONOTONIC, &tick); + ROS_WARN_STREAM("dl"); while (!g_quit) { for (ptr_vector::iterator eh = ec->begin(); eh != ec->end(); ++eh) @@ -672,4 +673,3 @@ int main(int argc, char *argv[]) return rv; } - diff --git a/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp b/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp index 4dcb09f..f5349e5 100644 --- a/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp +++ b/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp @@ -66,6 +66,8 @@ typedef struct __attribute__((__packed__)) ActuatorOdometry { volatile uint32_t odo_1; volatile uint32_t odo_2; + volatile uint32_t odo_3; + volatile uint32_t odo_4; } ActuatorOdometry; From d3ec85fbc4b89a50fa69a849f7b9b34a98179bdb Mon Sep 17 00:00:00 2001 From: dg-shadow Date: Mon, 2 Nov 2020 13:19:12 +0000 Subject: [PATCH 19/27] added clutch slip --- .../include/ros_ethercat_model/hardware_interface.hpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp b/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp index 4dcb09f..5b3dcde 100644 --- a/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp +++ b/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp @@ -66,6 +66,9 @@ typedef struct __attribute__((__packed__)) ActuatorOdometry { volatile uint32_t odo_1; volatile uint32_t odo_2; + volatile uint32_t odo_3; + volatile uint32_t odo_4; + } ActuatorOdometry; @@ -87,7 +90,7 @@ class ActuatorState motor_voltage_(0.0), flags_(0), pwm_(0), - + clutch_slip_(0), command_type_(COMMAND_TYPE_PWM) { @@ -115,9 +118,12 @@ class ActuatorState double max_effort_; //!< Absolute torque limit for actuator (derived from motor current limit). (in Nm) + int clutch_slip_; + double motor_voltage_; //!< Motor voltage (in volts) ActuatorCommandMode command_type_; // switch between pwm and effort ActuatorOdometry odometry_; + }; class ActuatorCommand From 025ee5628d012a16cd43602361387d0f2d30a535 Mon Sep 17 00:00:00 2001 From: dg-shadow Date: Thu, 3 Jun 2021 17:12:52 +0100 Subject: [PATCH 20/27] added member fields to actuator state --- .../include/ros_ethercat_model/hardware_interface.hpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp b/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp index 0f5af48..cfb98a1 100644 --- a/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp +++ b/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp @@ -81,6 +81,7 @@ class ActuatorState velocity_(0), effort_(0), commanded_effort_(0), + last_commanded_current_(0.0), last_measured_current_(0.0), last_commanded_effort_(0.0), @@ -119,6 +120,9 @@ class ActuatorState int clutch_slip_; + ros::Time last_command_time_; + ros::Duration command_timeout_; + double motor_voltage_; //!< Motor voltage (in volts) ActuatorCommandMode command_type_; // switch between pwm and effort ActuatorOdometry odometry_; From 98fb68eeef089fe67cad5de371f16a4057fb4aca Mon Sep 17 00:00:00 2001 From: dg-shadow Date: Mon, 6 Dec 2021 13:23:08 +0000 Subject: [PATCH 21/27] initialise timeout --- .../include/ros_ethercat_model/hardware_interface.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp b/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp index cfb98a1..32f50bd 100644 --- a/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp +++ b/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp @@ -91,7 +91,8 @@ class ActuatorState flags_(0), pwm_(0), clutch_slip_(0), - command_type_(COMMAND_TYPE_PWM) + command_type_(COMMAND_TYPE_PWM), + last_command_time_(ros::Time::now()) { } From 1d0932640f6b119296b2fb886d2037c3f7e91dc0 Mon Sep 17 00:00:00 2001 From: dg-shadow Date: Mon, 18 Jul 2022 14:13:12 +0100 Subject: [PATCH 22/27] Removed unnecessary warn --- ros_ethercat_hardware/src/ethercat_hardware.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros_ethercat_hardware/src/ethercat_hardware.cpp b/ros_ethercat_hardware/src/ethercat_hardware.cpp index 69651bb..7d304b1 100644 --- a/ros_ethercat_hardware/src/ethercat_hardware.cpp +++ b/ros_ethercat_hardware/src/ethercat_hardware.cpp @@ -873,7 +873,7 @@ EthercatHardware::configSlave(EtherCAT_SlaveHandler *sh) if (matching_class_name.size() != 0) { - ROS_WARN("Using device '%s' with product code %d", + ROS_INFO("Using device '%s' with product code %d", device_loader_.getClassDescription(matching_class_name).c_str(), product_code); try From 196d57718af4d015f548a1c7f3cbb6747cf23b9f Mon Sep 17 00:00:00 2001 From: dg-shadow Date: Wed, 20 Jul 2022 11:24:56 +0100 Subject: [PATCH 23/27] increase size of odo4 --- .../include/ros_ethercat_model/hardware_interface.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp b/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp index 32f50bd..4add2d4 100644 --- a/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp +++ b/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp @@ -67,7 +67,7 @@ typedef struct __attribute__((__packed__)) ActuatorOdometry volatile uint32_t odo_1; volatile uint32_t odo_2; volatile uint32_t odo_3; - volatile uint32_t odo_4; + volatile uint64_t odo_4; } ActuatorOdometry; From 7d524d07a15602ab2b6234f2cd61388be214c1c1 Mon Sep 17 00:00:00 2001 From: asd Date: Tue, 6 Sep 2022 13:48:36 +0100 Subject: [PATCH 24/27] fix slave count --- ros_ethercat_hardware/src/ethercat_hardware.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/ros_ethercat_hardware/src/ethercat_hardware.cpp b/ros_ethercat_hardware/src/ethercat_hardware.cpp index 69651bb..4c23f07 100644 --- a/ros_ethercat_hardware/src/ethercat_hardware.cpp +++ b/ros_ethercat_hardware/src/ethercat_hardware.cpp @@ -555,7 +555,7 @@ void EthercatHardwareDiagnosticsPublisher::publishDiagnostics() // status_.add("Motors halted", diagnostics_.motors_halted_ ? "true" : "false"); status_.addf("EtherCAT devices (expected)", "%d", num_ethercat_devices_); - status_.addf("EtherCAT devices (current)", "%d", diagnostics_.device_count_); + status_.addf("EtherCAT devices (current)", "%d", slaves_.size()); ethernet_interface_info_.publishDiagnostics(status_); // status_.addf("Reset state", "%d", reset_state_); @@ -563,7 +563,8 @@ void EthercatHardwareDiagnosticsPublisher::publishDiagnostics() status_.addf("Max PD Retries", "%d", max_pd_retries_); // Produce warning if number of devices changed after device initialization - if (num_ethercat_devices_ != diagnostics_.device_count_) + + if (num_ethercat_devices_ != slaves_.size()) { status_.mergeSummary(status_.WARN, "Number of EtherCAT devices changed"); } @@ -1031,6 +1032,7 @@ void EthercatHardware::loadNonEthercatDevices() void EthercatHardware::collectDiagnostics() { ROS_WARN_STREAM("collect diagnostics"); + if (NULL == oob_com_) return; @@ -1051,12 +1053,13 @@ void EthercatHardware::collectDiagnostics() oob_com_->txandrx(&frame); // Worry about locking for single value? - diagnostics_.device_count_ = status.get_adp(); + //diagnostics_.device_count_ = status.get_adp(); } for (unsigned i = 0; i < slaves_.size(); ++i) { + boost::shared_ptr d(slaves_[i]); d->collectDiagnostics(oob_com_); } From e1dca0d405f440ac234ad885b5a2b5e2d44b933f Mon Sep 17 00:00:00 2001 From: asd Date: Tue, 6 Sep 2022 13:51:02 +0100 Subject: [PATCH 25/27] back to old size --- .../include/ros_ethercat_model/hardware_interface.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp b/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp index 4add2d4..32f50bd 100644 --- a/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp +++ b/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp @@ -67,7 +67,7 @@ typedef struct __attribute__((__packed__)) ActuatorOdometry volatile uint32_t odo_1; volatile uint32_t odo_2; volatile uint32_t odo_3; - volatile uint64_t odo_4; + volatile uint32_t odo_4; } ActuatorOdometry; From 170d85cf53276e8c0547eaf8b294a07ef612c8b4 Mon Sep 17 00:00:00 2001 From: dg Date: Wed, 22 Nov 2023 15:12:01 +0000 Subject: [PATCH 26/27] added fields --- .../include/ros_ethercat_model/joint.hpp | 51 +++++++++++++++++++ .../ros_ethercat_model/robot_state.hpp | 2 +- 2 files changed, 52 insertions(+), 1 deletion(-) diff --git a/ros_ethercat_model/include/ros_ethercat_model/joint.hpp b/ros_ethercat_model/include/ros_ethercat_model/joint.hpp index 5cb0be4..ab3d32a 100644 --- a/ros_ethercat_model/include/ros_ethercat_model/joint.hpp +++ b/ros_ethercat_model/include/ros_ethercat_model/joint.hpp @@ -164,6 +164,52 @@ class JointState /// The position the joint should move to in radians or meters (write-to variable) double commanded_position_; + // Duplicates of command fields, specifically for reporting in finger states. + // This is due to other command fields being used/modified outside of DEX code. + double state_commanded_effort_; + double state_commanded_position_; + double state_commanded_velocity_; + double state_commanded_feedforward_effort_; + double state_commanded_position_effort_; + + // Set functions for above commanded state variables. + void setCommandedEffort(double effort) + { + state_commanded_effort_ = effort; + state_commanded_position_ = 0; + state_commanded_velocity_ = 0; + state_commanded_feedforward_effort_ = 0; + state_commanded_position_effort_ = 0; + }; + + void setCommandedPosition(double position, double effort) + { + state_commanded_effort_ = effort; + state_commanded_position_ = position; + state_commanded_velocity_ = 0; + state_commanded_feedforward_effort_ = 0; + state_commanded_position_effort_ = effort; + }; + + void setCommandedVelocity(double position, double effort, double velocity) + { + state_commanded_effort_ = effort; + state_commanded_position_ = position; + state_commanded_velocity_ = velocity; + state_commanded_feedforward_effort_ = 0; + state_commanded_position_effort_ = 0; + }; + + void setCommandedForcePosition(double position, double effort, double feed_forward, double pid_output) + { + state_commanded_effort_ = effort; + state_commanded_position_ = position; + state_commanded_velocity_ = 0; + state_commanded_feedforward_effort_ = feed_forward; + state_commanded_position_effort_ = pid_output; + }; + + /// The velocity the joint should move with in radians/sec or meters/sec (write-to variable) double commanded_velocity_; @@ -185,6 +231,11 @@ class JointState commanded_position_(0.0), commanded_velocity_(0.0), commanded_effort_(0.0), + state_commanded_effort_(0.0), + state_commanded_position_(0.0), + state_commanded_velocity_(0.0), + state_commanded_feedforward_effort_(0.0), + state_commanded_position_effort_(0.0), calibrated_(false), reference_position_(0.0) { diff --git a/ros_ethercat_model/include/ros_ethercat_model/robot_state.hpp b/ros_ethercat_model/include/ros_ethercat_model/robot_state.hpp index c47cfbb..a0e0211 100644 --- a/ros_ethercat_model/include/ros_ethercat_model/robot_state.hpp +++ b/ros_ethercat_model/include/ros_ethercat_model/robot_state.hpp @@ -140,7 +140,7 @@ class RobotState : public hardware_interface::HardwareInterface } catch (const std::runtime_error &ex) { - ROS_FATAL_STREAM("ros_ethercat_model failed to parse the URDF xml into a robot model\n" << ex.what()); + ROS_WARN_STREAM("ros_ethercat_model failed to parse the URDF xml into a robot model\n" << ex.what()); } } From c106cc1b361ec754d3de5f345357708be5fba9bc Mon Sep 17 00:00:00 2001 From: dg-shadow Date: Tue, 28 Nov 2023 17:55:54 +0000 Subject: [PATCH 27/27] make field sets consistent --- ros_ethercat_model/include/ros_ethercat_model/joint.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ros_ethercat_model/include/ros_ethercat_model/joint.hpp b/ros_ethercat_model/include/ros_ethercat_model/joint.hpp index ab3d32a..1e3b65a 100644 --- a/ros_ethercat_model/include/ros_ethercat_model/joint.hpp +++ b/ros_ethercat_model/include/ros_ethercat_model/joint.hpp @@ -178,7 +178,7 @@ class JointState state_commanded_effort_ = effort; state_commanded_position_ = 0; state_commanded_velocity_ = 0; - state_commanded_feedforward_effort_ = 0; + state_commanded_feedforward_effort_ = effort; state_commanded_position_effort_ = 0; }; @@ -197,7 +197,7 @@ class JointState state_commanded_position_ = position; state_commanded_velocity_ = velocity; state_commanded_feedforward_effort_ = 0; - state_commanded_position_effort_ = 0; + state_commanded_position_effort_ = effort; }; void setCommandedForcePosition(double position, double effort, double feed_forward, double pid_output)