Issue with Force/Torque Control on UR5 Robot

[federicoq1997]

I'm trying to control a UR5 robot using force/torque control in the TDW simulator. I have noticed unexpected behavior when trying to vary the amplitude of the oscillation of the first joint. I was hoping that by modifying the value of tau and feeding it to the "add_joint_forces" method, I would be able to achieve a wider oscillation, but that is not happening.

from tdw.controller import Controller
from tdw.tdw_utils import TDWUtils
from tdw.add_ons.robot import Robot
from tdw.add_ons.third_person_camera import ThirdPersonCamera
from tdw.add_ons.image_capture import ImageCapture
from tdw.backend.paths import EXAMPLE_CONTROLLER_OUTPUT_PATH
import numpy as np

def getInfoJoints(robot):
            shoulder = robot.static.joint_ids_by_name["shoulder_link"]

            angles=[
                robot.dynamic.joints[shoulder].angles[0],
            ]
            return np.deg2rad(angles)

if __name__ == "__main__":
    c = Controller()
    robot_id = c.get_unique_id()
    robot = Robot(name="ur5",
                position={"x": 1, "y": 0, "z": -2},
                rotation={"x": 0, "y": 0, "z": 0},
                robot_id=robot_id)
    # Add a camera and enable image capture.
    camera = ThirdPersonCamera(avatar_id="a",
                            position={"x": -0.1, "y": 1.7, "z": 0.1},
                            look_at=robot_id)
    path = EXAMPLE_CONTROLLER_OUTPUT_PATH.joinpath("add_joint_forces")
    print(f"Images will be saved to: {path}")
    capture = ImageCapture(avatar_ids=["a"], path=path)
    c.add_ons.extend([robot, camera, capture])
    c.communicate(TDWUtils.create_empty_room(12, 12))

    dt = 0.01

    c.communicate([{"$type": "set_time_step", "time_step": dt},
                   {"$type": "create_robot_obi_colliders", "id": robot_id},
                ])

    total_time = 20

    t = 0
    q_des = 1.5707
    while t < total_time:

        q = getInfoJoints(robot)
        tau = np.cos(t)
        robot.add_joint_forces(forces={robot.static.joint_ids_by_name["shoulder_link"]:  tau})
        c.communicate([{"$type": "step_physics", "frames": 1}])
        t += dt
        print(f"Time: {t}, q:{q}, tau:{tau}")
    c.communicate({"$type": "terminate"})

I have conducted a test using the TDW simulator and a UR5 robot. In the provided code example, I am attempting to control the oscillation of the robot's first joint using the "add_joint_forces" method. The value of tau is modified in the main loop, but even if I change the value of tau to

tau = 2*np.cos(t)

the oscillation does not become wider as I expect. The same happens if I use a different formula, such as

tau = 0.2*(q_des-q[0])

I would like to understand if I am making any mistakes or if there is something I am overlooking in my approach to force/torque control on the UR5 robot.

Additionally, I would like to inquire if there is a way to obtain the velocity of each individual joint. I have searched the documentation but have not found a specific method for obtaining this information.

• Steps Tried:

  • Executed the provided sample code using the TDW simulator and a UR5 robot.
  • Modified the value of tau and fed it to the "add_joint_forces" method to vary the amplitude of the oscillation of the first joint.
  • Verified that changing the modified value of tau does not affect the amplitude of the oscillation as expected.

• Development Environment:

  • Operating System: Windows
  • TDW Simulator Version: 1.11.6

Thank you for assisting in understanding and resolving this issue. I look forward to your help.

[alters-mit]

I would like to understand if I am making any mistakes or if there is something I am overlooking in my approach to force/torque control on the UR5 robot.

You are not making mistakes. The default values of the UR5 robot may not be correct. Try sending set_robot_joint_drive for each joint you want to adjust.

Additionally, I would like to inquire if there is a way to obtain the velocity of each individual joint. I have searched the documentation but have not found a specific method for obtaining this information.

See: send_robot_joint_velocities

[federicoq1997]

Hi, I have conducted some tests with robot control. I performed several experiments to determine the correct values for each joint, based on both the actual robot documentation and other sources. Specifically, I set the force limits (force_limits) to a value 150 times larger than the nominal values specified by Universal Robots. I obtained this information from the following link: link.

from tdw.controller import Controller
from tdw.tdw_utils import TDWUtils
from tdw.add_ons.robot import Robot
from tdw.add_ons.third_person_camera import ThirdPersonCamera
from tdw.add_ons.image_capture import ImageCapture
from tdw.backend.paths import EXAMPLE_CONTROLLER_OUTPUT_PATH
import numpy as np

def getInfoJoints(robot):
            shoulder = robot.static.joint_ids_by_name["shoulder_link"]
            upper_arm_link = robot.static.joint_ids_by_name["upper_arm_link"]
            forearm_link = robot.static.joint_ids_by_name["forearm_link"]
            wrist_1_link = robot.static.joint_ids_by_name["wrist_1_link"]
            wrist_2_link = robot.static.joint_ids_by_name["wrist_2_link"]
            wrist_3_link = robot.static.joint_ids_by_name["wrist_3_link"]

            angles=[
                robot.dynamic.joints[shoulder].angles[0],
                robot.dynamic.joints[upper_arm_link].angles[0],
                robot.dynamic.joints[forearm_link].angles[0],
                robot.dynamic.joints[wrist_1_link].angles[0],
                robot.dynamic.joints[wrist_2_link].angles[0],
                robot.dynamic.joints[wrist_3_link].angles[0]
            ]
            return np.deg2rad(angles)

if __name__ == "__main__":
    c = Controller()
    robot_id = c.get_unique_id()
    robot = Robot(name="ur5",
                position={"x": 1, "y": 0, "z": -2},
                rotation={"x": 0, "y": 0, "z": 0},
                robot_id=robot_id)
    # Add a camera and enable image capture.
    camera = ThirdPersonCamera(avatar_id="a",
                            position={"x": -0.1, "y": 1.7, "z": 0.1},
                            look_at=robot_id)
    path = EXAMPLE_CONTROLLER_OUTPUT_PATH.joinpath("add_joint_forces")
    print(f"Images will be saved to: {path}")
    capture = ImageCapture(avatar_ids=["a"], path=path)
    c.add_ons.extend([robot, camera, capture])
    c.communicate(TDWUtils.create_empty_room(12, 12))

    dt = 0.01

    c.communicate([{"$type": "set_time_step", "time_step": dt},
                ])

    while robot.joints_are_moving():
        c.communicate([])

    # Strike a cool pose.0,-110,90,20,90,0
    robot.set_joint_targets(targets={robot.static.joint_ids_by_name["shoulder_link"]: 0,
                                    robot.static.joint_ids_by_name["upper_arm_link"]: -110,
                                    robot.static.joint_ids_by_name["forearm_link"]: 90,
                                    robot.static.joint_ids_by_name["wrist_1_link"]: 20,
                                    robot.static.joint_ids_by_name["wrist_2_link"]: 90,
                                    robot.static.joint_ids_by_name["wrist_3_link"]: 0,
                                    })

    # Wait for the joints to stop moving.
    while robot.joints_are_moving():
        c.communicate([])

    total_time = 60
    joints_info = {
        'shoulder_link':{'limits':(-360,360),'force_limit':150*150,'damping':180,'stiffness':1000},
        'upper_arm_link':{'limits':(-360,360),'force_limit':150*150,'damping':180,'stiffness':1000},
        'forearm_link':{'limits':(-360,360),'force_limit':150*150,'damping':180,'stiffness':1000},
        'wrist_1_link':{'limits':(-360,360),'force_limit':28*150,'damping':180,'stiffness':100},
        'wrist_2_link':{'limits':(-360,360),'force_limit':28*150,'damping':180,'stiffness':100},
        'wrist_3_link':{'limits':(-360,360),'force_limit':28*150,'damping':180,'stiffness':100}
    }
    commands = []
    for joint_name, joint_details in joints_info.items():
          joint_id = robot.static.joint_ids_by_name[joint_name]
          commands.append({"$type": "set_robot_joint_drive", "joint_id": joint_id, "axis": "x", 
                           "lower_limit": joint_details['limits'][0], "upper_limit": joint_details['limits'][1], 
                           "force_limit": joint_details['force_limit'], 
                           "stiffness": joint_details['stiffness'], 
                           "damping": joint_details['damping'], 
                           "id": robot_id})
    c.communicate(commands)

    t = 0
    q_des = np.deg2rad([270, -110, 90, 20, 90, 0])
    while t < total_time:

        q = getInfoJoints(robot) #is rad
        tau = 1.5*(q_des-q)
        robot.add_joint_forces(forces={robot.static.joint_ids_by_name["shoulder_link"]:  tau[0]}) 
        robot.add_joint_forces(forces={robot.static.joint_ids_by_name["upper_arm_link"]:  tau[1]})
        robot.add_joint_forces(forces={robot.static.joint_ids_by_name["forearm_link"]:  tau[2]})
        robot.add_joint_forces(forces={robot.static.joint_ids_by_name["wrist_1_link"]:  tau[3]})
        robot.add_joint_forces(forces={robot.static.joint_ids_by_name["wrist_2_link"]:  tau[4]})
        robot.add_joint_forces(forces={robot.static.joint_ids_by_name["wrist_3_link"]:  tau[5]}) 

        c.communicate([{"$type": "step_physics", "frames": 1}])
        t += dt
        print(f"Time: {t}, q:{np.rad2deg(q)}")
 c.communicate({"$type": "terminate"})

During the tests, using values 150 times larger than they should be, I noticed that the movement seems correct, but the first joint never reaches 270° or any angle greater than 210°, even though I also tried modifying the limits to [0,360]. I have verified the proper configuration of the robot and the control, but I haven't found anything that can explain this behavior.

Furthermore, I have observed another anomalous behavior when attempting to move the "wrist". The robot seems to go haywire and doesn't follow the commands correctly. This behavior is inconsistent with expectations, and I am unable to understand the cause of this problem.

I would appreciate assistance in resolving these issues. What could be the causes preventing the first joint from reaching 270°, despite the correctly set limits? Also, why should I set such large values for force_limits? What could be causing the anomalous behavior of the "wrist"?

In addition, I noticed that if I try to reach the [90,-80,0,20,90,0] configuration, the robot as soon as it reaches it falls to the ground as if gravity was there, whereas until a moment before it was not present

Thank you in advance for your help. I look forward to your response.

[alters-mit]

but I haven't found anything that can explain this behavior.

Your math is off, but I don't know why. I can reach 270 degrees by replacing this:

robot.add_joint_forces(forces={robot.static.joint_ids_by_name["shoulder_link"]: tau[0]})

...with this:

robot.set_joint_targets(targets={robot.static.joint_ids_by_name["shoulder_link"]: 270})

If I make that change, the robot moves very quickly but correctly, which tells me that you are using incorrect forces (and possibly force limits).

Furthermore, I have observed another anomalous behavior when attempting to move the "wrist".

Try setting all limits (min and max) to 0 for each joint, which will make movement unlimited.

In addition, I noticed that if I try to reach the [90,-80,0,20,90,0] configuration, the robot as soon as it reaches it falls to the ground as if gravity was there, whereas until a moment before it was not present

I don't know what you mean by this. Can you provide an example?