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>
155 pids_.resize(joint_handles.size());
156 for (
unsigned int i = 0; i <
pids_.size(); ++i)
165 ROS_WARN_STREAM(
"Failed to initialize PID gains from ROS parameter server.");
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)
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]);