55 template <
class HardwareInterface,
class State>
93 template <
class State>
102 joint_handles_ptr_ = &joint_handles;
109 if (!joint_handles_ptr_) {
return;}
112 for (
auto& jh : *joint_handles_ptr_)
114 jh.setCommand(jh.getPosition());
122 const State& desired_state,
126 const unsigned int n_joints = joint_handles_ptr_->size();
127 for (
unsigned int i = 0; i < n_joints; ++i) {(*joint_handles_ptr_)[i].setCommand(desired_state.position[i]);}
143 template <
class State>
152 joint_handles_ptr_ = &joint_handles;
155 pids_.resize(joint_handles.size());
156 for (
unsigned int i = 0; i < pids_.size(); ++i)
163 if (!pids_[i]->
init(joint_nh))
165 ROS_WARN_STREAM(
"Failed to initialize PID gains from ROS parameter server.");
171 velocity_ff_.resize(joint_handles.size());
172 for (
unsigned int i = 0; i < velocity_ff_.size(); ++i)
174 controller_nh.
param(std::string(
"velocity_ff/") + joint_handles[i].
getName(), velocity_ff_[i], 0.0);
182 if (!joint_handles_ptr_) {
return;}
185 for (
unsigned int i = 0; i < pids_.size(); ++i)
188 (*joint_handles_ptr_)[i].setCommand(0.0);
196 const State& desired_state,
197 const State& state_error)
199 const unsigned int n_joints = joint_handles_ptr_->size();
202 if (!joint_handles_ptr_)
204 assert(n_joints == state_error.position.size());
205 assert(n_joints == state_error.velocity.size());
208 for (
unsigned int i = 0; i < n_joints; ++i)
210 const double command = (desired_state.velocity[i] * velocity_ff_[i]) + pids_[i]->computeCommand(state_error.position[i], state_error.velocity[i], period);
211 (*joint_handles_ptr_)[i].setCommand(command);
216 typedef std::shared_ptr<control_toolbox::Pid>
PidPtr;
251 template <
class State>
282 template <
class State>
289 template <
class State>
298 joint_handles_ptr_ = &joint_handles;
308 const State& desired_state,
312 const unsigned int n_joints = joint_handles_ptr_->size();
313 for (
unsigned int i = 0; i < n_joints; ++i)
315 (*joint_handles_ptr_)[i].setCommand(desired_state.position[i], desired_state.velocity[i]);
326 template <
class State>
335 joint_handles_ptr_ = &joint_handles;
345 const State& desired_state,
349 const unsigned int n_joints = joint_handles_ptr_->size();
350 for (
unsigned int i = 0; i < n_joints; ++i)
352 (*joint_handles_ptr_)[i].setCommand(desired_state.position[i], desired_state.velocity[i], desired_state.acceleration[i]);
void stopping(const ros::Time &)
void updateCommand(const ros::Time &, const ros::Duration &, const State &desired_state, const State &)
HardwareInterfaceAdapter()
HardwareInterfaceAdapter()
bool init(std::vector< typename HardwareInterface::ResourceHandleType > &, ros::NodeHandle &)
std::vector< hardware_interface::PosVelAccJointHandle > * joint_handles_ptr_
void starting(const ros::Time &)
void stopping(const ros::Time &)
std::vector< hardware_interface::JointHandle > * joint_handles_ptr_
void stopping(const ros::Time &)
ROSCONSOLE_CONSOLE_IMPL_DECL std::string getName(void *handle)
bool init(std::vector< hardware_interface::JointHandle > &joint_handles, ros::NodeHandle &controller_nh)
bool init(std::vector< hardware_interface::JointHandle > &joint_handles, ros::NodeHandle &)
std::vector< hardware_interface::JointHandle > * joint_handles_ptr_
void stopping(const ros::Time &)
std::vector< hardware_interface::PosVelJointHandle > * joint_handles_ptr_
void starting(const ros::Time &)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
void updateCommand(const ros::Time &, const ros::Duration &period, const State &desired_state, const State &state_error)
ROSLIB_DECL std::string command(const std::string &cmd)
Helper base class template for closed loop HardwareInterfaceAdapter implementations.
void updateCommand(const ros::Time &, const ros::Duration &, const State &, const State &)
void updateCommand(const ros::Time &, const ros::Duration &, const State &desired_state, const State &)
std::vector< PidPtr > pids_
std::vector< double > velocity_ff_
HardwareInterfaceAdapter()
#define ROS_WARN_STREAM(args)
Helper class to simplify integrating the JointTrajectoryController with different hardware interfaces...
std::shared_ptr< control_toolbox::Pid > PidPtr
void starting(const ros::Time &)
bool init(std::vector< hardware_interface::PosVelAccJointHandle > &joint_handles, ros::NodeHandle &)
void starting(const ros::Time &)
void starting(const ros::Time &)
bool init(std::vector< hardware_interface::PosVelJointHandle > &joint_handles, ros::NodeHandle &)
void stopping(const ros::Time &)
void updateCommand(const ros::Time &, const ros::Duration &, const State &desired_state, const State &)
ClosedLoopHardwareInterfaceAdapter()