Search Unity

  1. Welcome to the Unity Forums! Please take the time to read our Code of Conduct to familiarize yourself with the forum rules and how to post constructively.
  2. Dismiss Notice

Question Controlling Robot joint using Transform instead of articulation body (Xdrive)

Discussion in 'Scripting' started by Dgad7, May 19, 2023.

  1. Dgad7


    Feb 5, 2023

    I am trying to control the Niryo one URDF robot using just transforms and not using ARticulation bodies via xdrive as done in unity tutorials.

    To start with, the current joint positions are sent to ROS, which uses MoveIt to calculate the trajectory. The trajectory is then used to move the robot joints.

    Using xdrive the code would look like below
    Code (CSharp):
    1. for (var joint = 0; joint < m_JointArticulationBodies.Length; joint++){
    2.         var joint1XDrive = m_JointArticulationBodies[joint].xDrive;
    3. = result[joint];
    4.           m_JointArticulationBodies[joint].xDrive = joint1XDrive;
    5. }
    Using the transfor the code looks like below. where each join is created as variables like below and object connection is done in unity window.
    public Transform shoulder_link;

    Code (CSharp):
    1. var result = t.positions.Select(r => (float)r * Mathf.Rad2Deg).ToArray();
    4. shoulder_link.localRotation = Quaternion.Euler(0, -(float)result[0], 0);     // Option 1                  
    6. arm_link.localEulerAngles = new Vector3(((float)result[1]),0,-90);  // Option 2
    8. elbow_link.localRotation = Quaternion.Euler(0,90 -(float)result[2], 0);
    9. forearm_link.localRotation = Quaternion.Euler(90 +(float)result[3], 0,0);
    10. wrist_link.localRotation = Quaternion.Euler((float)result[4]-40, 90,-90);
    11. hand_link.localRotation = Quaternion.Euler((float)result[5]-50, 90,90);

    The problem is that the trajectory looks a bit different from the trajectory execution using xdrive but not too far away. Nevertheless the robot does not position itself correctly over the target and there are some more minor errors.

    Would appreciate some hints how to solve this.
    As documented in the link above, I have tried with localEulerAngles and localRotation properties of the transform. There are some more properties but not sure if they are applicable in this scenario.
    On the rhs, I have tried with both new Vector3 and Quarternion.Euler, but there is no difference.