Question My robot is moving uncontrolled in Unity

I connected my robot in unity to a ros node running moveit. When the trajectory gets returned to unity my robot executes it perfectly 50% of the time. The other 50 percent the robots articulation bodies are going crazy and do huge movements. My guess is this related to the Xdrive Target of the articulation body component. The value jumps for example from 190 to -170 and that causes the robots uncontrolled movements. Is there a way to fix this?

 

Is this rotation?
Do you by any chance convert quaternion to euler?

 

This only happens with free revolute joints

 

I use the code from the Object-Pose Estimation Tutorial:

Code (CSharp):

1. for (int poseIndex = 0; poseIndex < response.trajectories.Length; poseIndex++)
2.             {
3.                 // For every robot pose in trajectory plan
4.                 for (int jointConfigIndex = 0; jointConfigIndex < response.trajectories[poseIndex].joint_trajectory.points.Length; jointConfigIndex++)
5.                 {
6.                     var jointPositions = response.trajectories[poseIndex].joint_trajectory.points[jointConfigIndex].positions;
7.                     float[] result = jointPositions.Select(r => (float)r * Mathf.Rad2Deg).ToArray();
8.  
9.                     // Set the joint values for every joint
10.                     for (int joint = 0; joint < jointArticulationBodies.Length; joint++)
11.                     {
12.                         var joint1XDrive = jointArticulationBodies[joint].xDrive;
13.                         joint1XDrive.target = result[joint];
14.                         Debug.Log(joint1XDrive.target);
15.                         jointArticulationBodies[joint].xDrive = joint1XDrive;
16.                     }
17.                     // Wait for robot to achieve pose for all joint assignments
18.                     yield return new WaitForSeconds(jointAssignmentWait);
19.                 }
20.  
21.                 // Close the gripper if completed executing the trajectory for the Grasp pose
22.                 if (poseIndex == (int)Poses.Grasp)
23.                 {
24.                     StartCoroutine(IterateToGrip(true));
25.                     yield return new WaitForSeconds(jointAssignmentWait);
26.                 }
27.                 else if (poseIndex == (int)Poses.Place)
28.                 {
29.                     yield return new WaitForSeconds(poseAssignmentWait);
30.                     // Open the gripper to place the target cube
31.                     StartCoroutine(IterateToGrip(false));
32.                 }
33.                 // Wait for the robot to achieve the final pose from joint assignment
34.                 yield return new WaitForSeconds(poseAssignmentWait);
35.             }

float[] result = jointPositions.Select(r => (float)r * Mathf.Rad2Deg).ToArray(); in Line 7 does the conversion to Euler

 

Hello,
Are you having this behavior with your own robot?
Or is it happening with the Unity show case Niryo One robot?

 

It's happening with my own robot
I just learned that MoveIt outputs the joint target in -pi to +pi but the Unity articulation bodies ignore this. For example target is at 178 deg and then -179 deg the joint will almost make a whole rotation despite only a 3 degree rotation is needed

 

Hello, I am having the same problem. Did you find a fix for making the articulation bodies work as expected?