search

ros-controls / ros_controllers

Generic robotic controllers to accompany ros_control
http://wiki.ros.org/ros_control
BSD 3-Clause "New" or "Revised" License
562 stars 524 forks source link

Commanded position stuck at initial values #401

Closed TomShkurti closed 5 years ago

TomShkurti commented 5 years ago

Created a custom hardware interface for our robot, which exposes a JointStateInterface and PositionJointInterface for each joint. Then we are running a joint_state_controller/JointStateController and position_controllers/JointTrajectoryController. The robot position is reported just fine by the joint state controller and can be published as a topic by robot_state_publisher, but the trajectory controller ignores the robot position and always keeps the commanded position at whatever it was initialized to- in our case, 0. It will still execute trajectories which are given to it and change the commanded position that way, but starting from 0 and not the position of the real robot.

We know that the controller is at least in the path of the problem because when we manually alter the commanded value in the HWI, the value is then changed back to 0 if the controller is running but not when it has been disabled.

The HWI interfaces:

`hardware_interface::JointStateHandle jsh[7]; hardware_interface::JointHandle jh[7];

for (int i = 0; i < 7; i++){
    //Always register joint handles in the high-level order.
        jsh[i] = hardware_interface::JointStateHandle(
        "PSM" + psm + "_" + dvrk_joints::joint_names[i],
        &pos[i],
        &vel[i],
        &eff[i]
    );
        jnt_state_interface.registerHandle(jsh[i]);
}

registerInterface(&jnt_state_interface);

for (int i = 0; i < 7; i++){
    jh[i] = hardware_interface::JointHandle(jnt_state_interface.getHandle(
        "PSM" + psm + "_" + dvrk_joints::joint_names[i]
    ), &cmd[i])
}

for (int i = 0; i < 7; i++){
    jnt_pos_interface.registerHandle(jh[i]);
}
registerInterface(&jnt_pos_interface);`

and the controller configuration:

`joint_state_controller: type: joint_state_controller/JointStateController publish_rate: 40

joint_traj_controller: type: position_controllers/JointTrajectoryController

These should always reflect the joint names given in cwru_davinci_geometry_models/model

joints:

bmagyar commented 5 years ago

Do you think you could reproduce the issue without the robot? Have a look at how we set up simplistic RobotHW in diff_drive_controll/test/diffbot.

On Sun, 13 Jan 2019, 21:04 TomShkurti <notifications@github.com wrote:

Created a custom hardware interface for our robot, which exposes a JointStateInterface and PositionJointInterface for each joint. Then we are running a joint_state_controller/JointStateController and position_controllers/JointTrajectoryController. The robot position is reported just fine by the joint state controller and can be published as a topic by robot_state_publisher, but the trajectory controller ignores the robot position and always keeps the commanded position at whatever it was initialized to- in our case, 0. It will still execute trajectories which are given to it and change the commanded position that way, but starting from 0 and not the position of the real robot.

We know that the controller is at least in the path of the problem because when we manually alter the commanded value in the HWI, the value is then changed back to 0 if the controller is running but not when it has been disabled.

The HWI interfaces:

`hardware_interface::JointStateHandle jsh[7]; hardware_interface::JointHandle jh[7];

for (int i = 0; i < 7; i++){ //Always register joint handles in the high-level order. jsh[i] = hardwareinterface::JointStateHandle( "PSM" + psm + "" + dvrk_joints::joint_names[i], &pos[i], &vel[i], &eff[i] ); jnt_state_interface.registerHandle(jsh[i]); }

registerInterface(&jnt_state_interface);

for (int i = 0; i < 7; i++){ jh[i] = hardware_interface::JointHandle(jnt_stateinterface.getHandle( "PSM" + psm + "" + dvrk_joints::joint_names[i] ), &cmd[i]) }

for (int i = 0; i < 7; i++){ jnt_pos_interface.registerHandle(jh[i]); } registerInterface(&jnt_pos_interface);`

and the controller configuration:

`joint_state_controller: type: joint_state_controller/JointStateController publish_rate: 40

joint_traj_controller: type: position_controllers/JointTrajectoryController

These should always reflect the joint names given in

cwru_davinci_geometry_models/model joints:

  • PSM$(arg psm_num)_outer_yaw
  • PSM$(arg psm_num)_outer_pitch
  • PSM$(arg psm_num)_outer_insertion
  • PSM$(arg psm_num)_outer_roll
  • PSM$(arg psm_num)_outer_wrist_pitch
  • PSM$(arg psm_num)_outer_wrist_yaw
  • PSM$(arg psm_num)_jaw gains: PSM$(arg psm_num)_outer_yaw: {p: 1, i: 0, d: 0} PSM$(arg psm_num)_outer_pitch: {p: 1, i: 0, d: 0} PSM$(arg psm_num)_outer_insertion: {p: 1, i: 0, d: 0} PSM$(arg psm_num)_outer_roll: {p: 1, i: 0, d: 0} PSM$(arg psm_num)_outer_wrist_pitch: {p: 1, i: 0, d: 0} PSM$(arg psm_num)_outer_wrist_yaw: {p: 1, i: 0, d: 0} PSM$(arg psm_num)_jaw: {p: 1, i: 0, d: 0} state_publish_rate: 40 action_monitor_rate: 40 allow_partial_joints_goal: true constraints: goal_time: 10.0 # Override default stopped_velocity_tolerance: 0.02 # Override default`

— You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub https://github.com/ros-controls/ros_controllers/issues/401, or mute the thread https://github.com/notifications/unsubscribe-auth/ADXH4R5qT9zf0ySmDpafTOaPt881PaKjks5vC596gaJpZM4Z9YBK .

TomShkurti commented 5 years ago

Discovered the issue was due to a problem on our end within the custom HWI. The robot alternates between a free-floating mode with sensors disabled and a rigid mode, and the controllers were supposed to be stopped and restarted when moving from the former to the latter to update them. This was not being done properly and so the controllers remained stuck at their initial values and never getting the sensor updates.