qb_move_hardware_interface.cpp
Go to the documentation of this file.
1 /***
2  * Software License Agreement: BSD 3-Clause License
3  *
4  * Copyright (c) 2016-2018, qbrobotics®
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
8  * following conditions are met:
9  *
10  * * Redistributions of source code must retain the above copyright notice, this list of conditions and the
11  * following disclaimer.
12  *
13  * * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
14  * following disclaimer in the documentation and/or other materials provided with the distribution.
15  *
16  * * Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote
17  * products derived from this software without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
21  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
22  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
23  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
24  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
25  * USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26  */
27 
29 
30 using namespace qb_move_hardware_interface;
31 
33  : qbDeviceHW(std::make_shared<qb_move_transmission_interface::qbMoveTransmission>(), {"motor_1_joint", "motor_2_joint", "shaft_joint"}, {"motor_1_joint", "motor_2_joint", "shaft_joint", "stiffness_preset_virtual_joint"}) {
34 
35 }
36 
38 
39 }
40 
41 std::vector<std::string> qbMoveHW::getJoints() {
43  return {joints_.names.at(2), joints_.names.at(3)};
44  }
45  return {joints_.names.at(0), joints_.names.at(1)};
46 }
47 
49  std::smatch match_row, match_value;
50  std::string device_info(getInfo());
51  if (std::regex_search(device_info, match_row, std::regex("Max stiffness: [[:digit:]]+"))) {
52  std::string max_stiffness(match_row[0]);
53  std::regex_search(max_stiffness, match_value, std::regex("[[:digit:]]+"));
54  return std::stoi(match_value[0]);
55  }
56  return 3000; // default limit of stiffness value
57 }
58 
59 bool qbMoveHW::init(ros::NodeHandle &root_nh, ros::NodeHandle &robot_hw_nh) {
60  if (!qb_device_hardware_interface::qbDeviceHW::init(root_nh, robot_hw_nh)) {
61  return false;
62  }
63  // for qbmoves is important that the following assertions hold
65  ROS_ASSERT(device_.position_limits.size() == 4);
67 
68  // if the device interface initialization has succeed the device info have been retrieved
70  std::static_pointer_cast<qb_move_transmission_interface::qbMoveTransmission>(transmission_.getTransmission())->setPresetFactor(getMaxStiffness());
71  command_with_position_and_preset_ = std::static_pointer_cast<qb_move_transmission_interface::qbMoveTransmission>(transmission_.getTransmission())->getCommandWithPoistionAndPreset();
72  preset_percent_to_radians_ = std::static_pointer_cast<qb_move_transmission_interface::qbMoveTransmission>(transmission_.getTransmission())->getPresetPercentToRadians();
73  position_ticks_to_radians_ = std::static_pointer_cast<qb_move_transmission_interface::qbMoveTransmission>(transmission_.getTransmission())->getPositionFactor().front();
76 
77  use_interactive_markers_ = root_nh.param<bool>("use_interactive_markers", false) && !root_nh.param<bool>("use_waypoints", false) && command_with_position_and_preset_;
80  }
81 
82  return true;
83 }
84 
85 void qbMoveHW::read(const ros::Time &time, const ros::Duration &period) {
86  // read actuator state from the hardware (convert to proper measurement units)
88 
91  }
92 }
93 
95  // the shaft limits [radians] depend on fixed motor limits [radians] and variable stiffness preset [0,1]
96  joints_.limits.at(2).max_position = std::min(joints_.limits.at(2).max_position, max_motor_limits_ - std::abs(joints_.commands.at(3)*preset_percent_to_radians_));
97  joints_.limits.at(2).min_position = std::max(joints_.limits.at(2).min_position, min_motor_limits_ + std::abs(joints_.commands.at(3)*preset_percent_to_radians_));
98 }
99 
100 void qbMoveHW::write(const ros::Time &time, const ros::Duration &period) {
101  // the variable stiffness decreases the shaft position limits (all the other limits are fixed)
103 
104  // send actuator command to the hardware (saturate and convert to proper measurement units)
106 }
107 
std::vector< std::string > getJoints() override
Return the shaft position and stiffness preset joints whether command_with_position_and_preset_ is tr...
qb_device_hardware_interface::qbDeviceHWResources joints_
qbMoveHW()
Initialize the qb_device_hardware_interface::qbDeviceHW with the specific transmission interface and ...
int getMaxStiffness()
Parse the maximum value of stiffness of the device from the getInfo string.
void write(const ros::Time &time, const ros::Duration &period) override
Update the shaft joint limits and call the base method.
void initMarkers(ros::NodeHandle &root_nh, const std::string &device_name, const std::vector< std::string > &joint_names)
Initialize the interactive_markers::InteractiveMarkerServer and the marker displayed in rviz to contr...
void setMarkerState(const std::vector< double > &joint_positions)
Update the stored joint positions with the given values.
void read(const ros::Time &time, const ros::Duration &period) override
Call the base method and nothing more.
bool init(ros::NodeHandle &root_nh, ros::NodeHandle &robot_hw_nh) override
qb_move_interactive_interface::qbMoveInteractive interactive_interface_
bool init(ros::NodeHandle &root_nh, ros::NodeHandle &robot_hw_nh) override
Call the base method and nothing more.
qb_device_hardware_interface::qbDeviceResources device_
bool param(const std::string &param_name, T &param_val, const T &default_val) const
void write(const ros::Time &time, const ros::Duration &period) override
The qbrobotics qbmove HardWare interface implements the specific structures to manage the communicati...
qb_device_transmission_interface::qbDeviceTransmissionResources transmission_
PLUGINLIB_EXPORT_CLASS(transmission_interface::BiDirectionalEffortJointInterfaceProvider, transmission_interface::RequisiteProvider)
#define ROS_ASSERT(cond)
void updateShaftPositionLimits()
Update the shaft joint limits since they depend on the fixed motors limits and on the variable stiffn...
std::vector< joint_limits_interface::JointLimits > limits
The qbrobotics qbmove Transmission interface implements the specific transmission_interface::Transmis...
void read(const ros::Time &time, const ros::Duration &period) override


qb_move_hardware_interface
Author(s): qbrobotics®
autogenerated on Thu Oct 10 2019 03:30:57