fix: default HW sometimes using wrong mj joint ids

Author: DavidPL1

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).

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;
 			}
 		}