joint angles in simulation

[lr101095]

I was comparing the joint angles and joint limits in the simulation and the real robot. I dont think the correct joint angles have been integrated in the simulation. I understand that there may be some variance in the real robot, but i just wanted to make sure i wasn't observing something incorrect.

I am trying to run a program, that was written for the real robot, in simulation. It works as expected with the real robot. But there are several discrepancies that i am coming across when running the simulation. One concern is that the joint limits do no match those of the real robot. Because of this, the robot in simulation isn't in the correct pose.

For example, in simulation, joint 6 (wrist) has a range of -3.14 to 3.14.
The real sawyer has a range of -4.714 to 4.709 in the same joint.

I'm not sure if that has anything to do the simulation failing to run the program as expected, but I feel it may because the lack in "flexibility" in the robot in simulation isn't allowing the required pose to be achieved. In the program i'm trying to run, a joint angle of 3.373 is required in the joint 6. Consequently, due to the range provided in the simulation, the required pose in not achieved.

it is worth noting that i've only observed this difference in range in joint 6, and i haven't, as of yet, checked to see if there are discrepancies in the other joints.

If anyone has any advice regarding how to resolve this, or if there is an upcoming update that resolves this issue, please let me know.

[Ian McMahon]

Hi lr,

I'm sorry to hear that this is causing your code trouble. The joint range of j6 is in fact the main difference between the real robot and the simulated robot. This is largely due to a difficulty I encountered with the ros_controls_plugin to calculate the shortest angular delta distance being reported by Gazebo. The angular distances would jump between timesteps into the range -pi to pi if they were beyond that range. This is the function I struggled with in ros_controls_plugin:

https://github.com/ros-simulation/gazebo_ros_pkgs/blob/kinetic-devel/gazebo_ros_control/src/default_robot_hw_sim.cpp#L338-L342

As a workaround to prevent Gazebo from reporting joint values that jump between timesteps, I limited the range of j6 to -pi to pi. If you prefer to use the original range, just modify this line in sawyer_description package's xacro file to give you the full joint range on J6:

https://github.com/RethinkRobotics/sawyer_robot/blob/master/sawyer_description/urdf/sawyer_base.urdf.xacro#L402

I'm open to recommendations on better solutions for this issue if anyone has any thoughts.

Hope this helps!

~ Ian

[lr101095]

Hi Ian,

I changed the upper and lower limits for joint 6, however now it doesn't seem to make use of the full range.
If i use joint_position_keyboard.py, and i try to rotate joint 6, it will only rotate between -4.714 and -1.569.

I have the following where you suggested i make the change:

    <limit effort="9.0" lower="-4.714" upper="4.714" velocity="4.545"/>

I haven't changed anything, apart from the lower and upper limit. Any idea why it restricted itself those angles?

Regards
Luke

[Ian McMahon]

Hi Luke,

I think you're encountering the same issue I experienced when designing the simulator - any reported joint angle beyond -pi to pi is  clamped by the ros_controls  angles::shortest_angular_distance_with_limits function. This seems to be a fundamental limitation in the angles library: https://github.com/ros/angles/blob/master/angles/include/angles/angles.h#L134

I found today that ros_controls  has an open pull request to address this same issue, but only in the position controller:

https://github.com/ros-controls/ros_controllers/pull/264/files/cd849483408795922d2cb4383fde0e78ed763ff8

    if (joint_urdf_->limits->lower >= -M_PI &&
        joint_urdf_->limits->upper <=  M_PI) {
      // this path only works when the joint travel is
      // within [-pi,pi]
      angles::shortest_angular_distance_with_limits(
        current_position,
        command_position,
        joint_urdf_->limits->lower,
        joint_urdf_->limits->upper,
        error);
    } else {
      // apply limits and compute error without trying to unwrap
      double clamped_command = command_position;
      if (clamped_command > joint_urdf_->limits->upper)
        clamped_command = joint_urdf_->limits->upper;
      else if (clamped_command < joint_urdf_->limits->lower)
        clamped_command = joint_urdf_->limits->lower;
      error = clamped_command - current_position;
    }

I will apply this patch in the gazebo_ros_controls/default_robot_hw_sim.cpp and report back if it fixes the issue.

Best,

~ Ian

[Ian McMahon]

Upon digging into this further, it appears that the fundamental issue is actually with the default Physics engine for Gazebo: ODE. It is only capable of reading values from -pi to pi:

http://opende.sourceforge.net/docs/group__joints.html#ga23cbd4983ea2e1b80b404a277ab21120

I commented on an issue inside of the Gazebo repository regarding this issue. Please feel free to add any additional details you may have here:

https://bitbucket.org/osrf/gazebo/issues/724/universal-joint-getangle-wraps-at-pi-pi

If things change upstream, I'll update the post here.

~ Ian