48 : GenericHWInterface(nh, urdf_model), name_(
"sim_hw_interface")
52 std::size_t error = 0;
62 void SimHWInterface::init()
65 GenericHWInterface::init();
68 joint_position_prev_.resize(num_joints_, 0.0);
81 enforceLimits(elapsed_time);
89 for (std::size_t joint_id = 0; joint_id < num_joints_; ++joint_id)
91 switch (sim_control_mode_)
94 positionControlSimulation(elapsed_time, joint_id);
108 joint_velocity_[joint_id] = joint_velocity_command_[joint_id];
109 joint_position_[joint_id] += joint_velocity_command_[joint_id] * elapsed_time.
toSec();
123 pos_jnt_sat_interface_.enforceLimits(period);
126 void SimHWInterface::positionControlSimulation(
ros::Duration& elapsed_time,
const std::size_t joint_id)
128 const double max_delta_pos = joint_velocity_limits_[joint_id] * elapsed_time.
toSec();
131 p_error_ = joint_position_command_[joint_id] - joint_position_[joint_id];
133 const double delta_pos = std::max(std::min(p_error_, max_delta_pos), -max_delta_pos);
134 joint_position_[joint_id] += delta_pos;
140 if (elapsed_time.
toSec() > 0)
142 joint_velocity_[joint_id] = (joint_position_[joint_id] - joint_position_prev_[joint_id]) / elapsed_time.
toSec();
145 joint_velocity_[joint_id] = 0;
148 joint_position_prev_[joint_id] = joint_position_[joint_id];