diff --git a/CHANGELOG.md b/CHANGELOG.md index 7b1b11ea..3efcef6a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -11,6 +11,7 @@ Loading and reset times are reported in the server debug log. All plugin stats c ### Fixed * Added missing call to render callbacks in viewer. While the callbacks were still being run for offscreen rendering, the viewer did not render additional geoms added by plugins. +* *mujoco_ros_control*: fixed sometimes using wrong joint id in default hardware interface (would only be correct, if the joints appear first and in the same order in the compiled MuJoCo model). ### Changed * Moved `mujoco_ros::Viewer::Clock` definition to `mujoco_ros::Clock` (into common_types.h). diff --git a/mujoco_ros_control/src/default_robot_hw_sim.cpp b/mujoco_ros_control/src/default_robot_hw_sim.cpp index 347a8140..ec50d398 100644 --- a/mujoco_ros_control/src/default_robot_hw_sim.cpp +++ b/mujoco_ros_control/src/default_robot_hw_sim.cpp @@ -269,15 +269,15 @@ void DefaultRobotHWSim::writeSim(ros::Time time, ros::Duration period) for (unsigned int j = 0; j < n_dof_; j++) { switch (joint_control_methods_[j]) { case EFFORT: { - const double effort = e_stop_active_ ? 0 : joint_effort_command_[j]; - d_ptr_->qfrc_applied[m_ptr_->jnt_dofadr[j]] = effort; + const double effort = e_stop_active_ ? 0 : joint_effort_command_[j]; + d_ptr_->qfrc_applied[m_ptr_->jnt_dofadr[mujoco_joint_ids_[j]]] = effort; break; } case POSITION: { - d_ptr_->qpos[m_ptr_->jnt_dofadr[j]] = joint_position_command_[j]; - d_ptr_->qvel[m_ptr_->jnt_dofadr[j]] = 0.; - d_ptr_->qfrc_applied[m_ptr_->jnt_dofadr[j]] = 0.; + d_ptr_->qpos[m_ptr_->jnt_dofadr[mujoco_joint_ids_[j]]] = joint_position_command_[j]; + d_ptr_->qvel[m_ptr_->jnt_dofadr[mujoco_joint_ids_[j]]] = 0.; + d_ptr_->qfrc_applied[m_ptr_->jnt_dofadr[mujoco_joint_ids_[j]]] = 0.; break; } @@ -299,13 +299,13 @@ void DefaultRobotHWSim::writeSim(ros::Time time, ros::Duration period) const double effort_limit = joint_effort_limits_[j]; const double effort = clamp(pid_controllers_[j].computeCommand(error, period), -effort_limit, effort_limit); - d_ptr_->qfrc_applied[m_ptr_->jnt_dofadr[j]] = effort; + d_ptr_->qfrc_applied[m_ptr_->jnt_dofadr[mujoco_joint_ids_[j]]] = effort; break; } case VELOCITY: { - d_ptr_->qvel[m_ptr_->jnt_dofadr[j]] = e_stop_active_ ? 0. : joint_velocity_command_[j]; - d_ptr_->qfrc_applied[m_ptr_->jnt_dofadr[j]] = 0.; + d_ptr_->qvel[m_ptr_->jnt_dofadr[mujoco_joint_ids_[j]]] = e_stop_active_ ? 0. : joint_velocity_command_[j]; + d_ptr_->qfrc_applied[m_ptr_->jnt_dofadr[mujoco_joint_ids_[j]]] = 0.; break; } @@ -318,7 +318,7 @@ void DefaultRobotHWSim::writeSim(ros::Time time, ros::Duration period) error = joint_velocity_command_[j] - joint_velocity_[j]; const double effort_limit = joint_effort_limits_[j]; const double effort = clamp(pid_controllers_[j].computeCommand(error, period), -effort_limit, effort_limit); - d_ptr_->qfrc_applied[m_ptr_->jnt_dofadr[j]] = effort; + d_ptr_->qfrc_applied[m_ptr_->jnt_dofadr[mujoco_joint_ids_[j]]] = effort; break; } }