I am using ‘/pos_joint_traj_controller/follow_joint_trajectory’ action interface to control the simulated UR in Gazebo. I have an external motion planner which commands the robot to move in cartesian space. However, when the commanded trajectory involves any joint crossing 180 degrees from higher value to lower value, the joint rotates full 360 degrees before settling down to the new value.
In addition to this, such behavior is only there for the first instance of motion. If I repeat the same motion again, the robot moves as expected. Please see attached gif for more details. I am mainly curious that why the robot responds to the exact same joint position, velocity and acceleration commands differently in two instances.
I am using /pos_joint_traj_controller/follow_joint_trajectory action along with the trajectory_msgs/JointTrajectory message to move the robot using joint position, velocity and acceleration commands.
For the scope of this discussion, let’s stick to UR5e robot model.
The issue I am facing -
Let’s say the robot is at joint configuration [-19.4459 -89.7390 73.0240 -163.2845 -70.5541 -0.0003] at the beginning of the motion.
I use tools provided by Robotis System Toolbox from MathWorks to generate a set of joint angles, velocities and acceleration to follow a pure cartesian motion i.e. the end-effector should move only in Z direction.
At the end of this motion, based on my calculation the robot joint configuration should be [-19.4459 -85.6467 113.9533 151.6939 -70.5541 -0.0005].
Now if I interpolate this trajectory, the wrist_1_joint should move from -163 deg to +151 deg while crossing -180 to +180. Ideally, this should be a motion of 47 degs and my velocity and acceleration is calculated based on the 47 deg of travel.
However, when the robot starts moving, it goes to -180 and then completed the entire revolution till +151 instead of crossing -180 to +180.
My question -
What should be the ideal range of joint angles while using the trajectory_msgs)/JointTrajectoryPoint Message? -pi to pi or 0 to 2pi?
Even if I use the 0 to 2pi range, the problem still persists. Like if I want to travel from +20 to +340, the robot completes the motion by traveling 320 deg, but ideally, this should be the 40 deg travel. Can you please suggest any workaround for this?
Interesting observation, the same trajectory via the same trajectory_msgs)/JointTrajectoryPoint Message works as expected with URSim.
As stated before, I would assume that the flip is already encoded inside the joint trajectory. Could you please post the trajectory that is actually being sent to the controller? (The output of rostopic echo /pos_joint_traj_controller/follow_joint_trajectory/goal)
Thank you for your response and apologies for digressing from the topic and not providing what you have asked for. However, I believe that I have some additional details on this.
So while looking at the range of joint angles on the URSim Move dashboard, it seems that the valid range of motion for each joint is -360 to 360 i.e. 720 deg total. With this range, there is an overlap in the range of physical motion of the joint and in theory, we can achieve a continuous motion.
However, the only catch in this is for the robot there is a one to one correspondence between various joint angles. For example, for any joint -160deg and 200deg are the same physical positions. But if you first command the robot to move to -160degs and then command the robot to move +200degs, it actually completes the entire revolution and ends up at the same joint position. This phenomenon applies to all possible combinations of joint angle movement.
Also the robot state is a reflection of the goal we have assigned to. For example, if we first assing -160deg as a goal and then read /joint_states, it says that the joint is at -160degs. But once we assign a goal to +200degs and read /joint_states once the robot motion is complete, it says the joint is at +200degs.
Hence, the only solution that I think of right now is :
Read angles from the robot.
Covert them into the preferred range.
Calculate Velocity and acceleration commands.
Calculate the joint command by integrating the velocity command and the initial position acquired from the robot in step 1.
Send the joint, velocity, and acceleration commands to the robot.
I have attached the /pos_joint_traj_controller/follow_joint_trajectory/goal message for -160deg and +200deg command for your reference. Please note that I am using the pure angle control for the demonstration and hence the velocity and acceleration field is zero.
It will be great if you can confirm if my understanding is correct and also share your thoughts on the workaround I have proposed. pure_angle_200.txt.pdf (20.7 KB) pure_angle_minus_160.txt.pdf (24.6 KB)
