# Question My robot is moving uncontrolled in Unity

Discussion in 'Robotics' started by agru, Jan 24, 2022.

1. ### agru

Joined:
Jul 6, 2020
Posts:
13
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?

2. ### Antypodish

Joined:
Apr 29, 2014
Posts:
10,480
Is this rotation?
Do you by any chance convert quaternion to euler?

3. ### agru

Joined:
Jul 6, 2020
Posts:
13
This only happens with free revolute joints

4. ### agru

Joined:
Jul 6, 2020
Posts:
13
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

5. ### medinafb01

Joined:
Feb 1, 2021
Posts:
14
Hello,
Are you having this behavior with your own robot?
Or is it happening with the Unity show case Niryo One robot?

6. ### agru

Joined:
Jul 6, 2020
Posts:
13
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

Last edited: Jan 27, 2022
7. ### lampri

Joined:
Mar 27, 2021
Posts:
1
Hello, I am having the same problem. Did you find a fix for making the articulation bodies work as expected?