30 #ifndef JOINT_TRAJECTORY_CONTROLLER_HARDWARE_INTERFACE_ADAPTER_H 31 #define JOINT_TRAJECTORY_CONTROLLER_HARDWARE_INTERFACE_ADAPTER_H 37 #include <boost/shared_ptr.hpp> 56 template <
class HardwareInterface,
class State>
94 template <
class State>
103 joint_handles_ptr_ = &joint_handles;
110 if (!joint_handles_ptr_) {
return;}
113 for (
unsigned int i = 0; i < joint_handles_ptr_->size(); ++i)
115 (*joint_handles_ptr_)[i].setCommand((*joint_handles_ptr_)[i].getPosition());
123 const State& desired_state,
127 const unsigned int n_joints = joint_handles_ptr_->size();
128 for (
unsigned int i = 0; i < n_joints; ++i) {(*joint_handles_ptr_)[i].setCommand(desired_state.position[i]);}
144 template <
class State>
153 joint_handles_ptr_ = &joint_handles;
156 pids_.resize(joint_handles.size());
157 for (
unsigned int i = 0; i < pids_.size(); ++i)
164 if (!pids_[i]->
init(joint_nh))
166 ROS_WARN_STREAM(
"Failed to initialize PID gains from ROS parameter server.");
172 velocity_ff_.resize(joint_handles.size());
173 for (
unsigned int i = 0; i < velocity_ff_.size(); ++i)
175 controller_nh.
param(std::string(
"velocity_ff/") + joint_handles[i].
getName(), velocity_ff_[i], 0.0);
183 if (!joint_handles_ptr_) {
return;}
186 for (
unsigned int i = 0; i < pids_.size(); ++i)
189 (*joint_handles_ptr_)[i].setCommand(0.0);
197 const State& desired_state,
198 const State& state_error)
200 const unsigned int n_joints = joint_handles_ptr_->size();
203 if (!joint_handles_ptr_)
205 assert(n_joints == state_error.position.size());
206 assert(n_joints == state_error.velocity.size());
209 for (
unsigned int i = 0; i < n_joints; ++i)
211 const double command = (desired_state.velocity[i] * velocity_ff_[i]) + pids_[i]->computeCommand(state_error.position[i], state_error.velocity[i], period);
212 (*joint_handles_ptr_)[i].setCommand(command);
252 template <
class State>
283 template <
class State>
290 template <
class State>
299 joint_handles_ptr_ = &joint_handles;
309 const State& desired_state,
313 const unsigned int n_joints = joint_handles_ptr_->size();
314 for (
unsigned int i = 0; i < n_joints; ++i)
316 (*joint_handles_ptr_)[i].setCommand(desired_state.position[i], desired_state.velocity[i]);
327 template <
class State>
336 joint_handles_ptr_ = &joint_handles;
346 const State& desired_state,
350 const unsigned int n_joints = joint_handles_ptr_->size();
351 for (
unsigned int i = 0; i < n_joints; ++i)
353 (*joint_handles_ptr_)[i].setCommand(desired_state.position[i], desired_state.velocity[i], desired_state.acceleration[i]);
361 #endif // header guard boost::shared_ptr< control_toolbox::Pid > PidPtr
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 &)
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 &)
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.
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
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...
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()