Not able to replicate the force control task with UR5 robot

[deepakraina99]

I have seen _Force_controlexample.py with the Kuka-IIWa robot, which is working perfectly fine. But then I tried to implement the same with the UR5 robot, then it is not able to do the force tracking. Can you please let me know, what I am doing wrong here. Is it an issue with M/D/K parameter tunning?

Code:

#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
The task is to track the force along z axis (vertical to the table) by employing admittance control, meanwhile tracking
a circle trajectory on the xy plane. And the end-effector's target position is visualized by a sphere.
Reference:
[1] SONG, Peng; YU, Yueqing; ZHANG, Xuping. A tutorial survey and comparison of impedance control on robotic manipulation
. Robotica, 2019, 37.5: 801-836.
"""

import numpy as np
from itertools import count

from pyrobolearn.simulators import Bullet
from pyrobolearn.worlds import BasicWorld
from pyrobolearn.robots import Body, sensors, UR10, UR5
from pyrobolearn.utils.transformation import *

from pyrobolearn.utils.plotting.end_effector_realtime_FT_plot import EeFtRealTimePlot

from threading import Thread

import matplotlib.pyplot as plt
import matplotlib
matplotlib.use('TkAgg')

# Real-time plot the End-effector force and torque
def plotting_thread(plot):
    if not isinstance(plot, EeFtRealTimePlot):
        raise TypeError("Expecting to plot type is CartesianRealTimePlot, not ""{}".format(plot))
    while True:
        plot.update()

# Manipulate the whole process
# The sphere is used to visualize the reference trajectory, So I creat the sphere trajectory as the reference
def manipulator_thread(world, robot, sphere, FT_sensor):
    """
    First step: is to arrive the initial position
    """
    for t in count():
        # move sphere
        # sphere.position = np.array([0.36, 0, 0.8])
        sphere.position = np.array([0.5, 0.1, 0.94])

        # get current end-effector position and velocity in the task/operational space
        x = robot.get_link_world_positions(link_id)
        dx = robot.get_link_world_linear_velocities(link_id)
        o = robot.get_link_world_orientations(link_id)
        do = robot.get_link_world_angular_velocities(link_id)

        # Get joint positions
        q = robot.get_joint_positions()

        # Get linear jacobian
        if robot.has_floating_base():
            J = robot.get_jacobian(link_id, q=q)[:, qIdx + 6]
        else:
            J = robot.get_jacobian(link_id, q=q)[:, qIdx]

        # Pseudo-inverse: \hat{J} = J^T (JJ^T + k^2 I)^{-1}
        Jp = robot.get_damped_least_squares_inverse(J, damping)

        dv = kp * (sphere.position - x) - kd * dx
        dw = kp * quaternion_error(sphere.orientation, o) - kd * do
        # evaluate damped-least-squares IK
        dq = Jp.dot(np.hstack((dv, dw)))

        # set joint positions
        q = q[qIdx] + dq * dt
        robot.set_joint_positions(q, joint_ids=joint_ids)
        if t > 1000:
            break
        # step in simulation
        world.step(sleep_dt=dt)
    """
        Second step: From the initial pose, Move vertically downward 
        until end-effector touches the desktop with a force of 10N 
    """
    for t in count():
        Fz_desired = 5  # the threshold of the contact force with table
        # move sphere
        sphere.position = np.array([0.5, 0.1, 0.94-0.0005*t])

        # get current end-effector position and velocity in the task/operational space
        x = robot.get_link_world_positions(link_id)
        dx = robot.get_link_world_linear_velocities(link_id)
        o = robot.get_link_world_orientations(link_id)
        do = robot.get_link_world_angular_velocities(link_id)

        # Get joint positions
        q = robot.get_joint_positions()

        # Get linear jacobian
        if robot.has_floating_base():
            J = robot.get_jacobian(link_id, q=q)[:, qIdx + 6]
        else:
            J = robot.get_jacobian(link_id, q=q)[:, qIdx]

        # Pseudo-inverse: \hat{J} = J^T (JJ^T + k^2 I)^{-1}
        Jp = robot.get_damped_least_squares_inverse(J, damping)

        dv = kp * (sphere.position - x) - kd * dx
        dw = kp * quaternion_error(sphere.orientation, o) - kd * do
        # evaluate damped-least-squares IK
        dq = Jp.dot(np.hstack((dv, dw)))

        # set joint positions
        # robot.set_joint_velocities(dq, joint_ids=joint_ids)
        q = q[qIdx] + dq * dt
        robot.set_joint_positions(q, joint_ids=joint_ids)

        print('force:', FT_sensor.sense()[2])
        if FT_sensor.sense() is not None:
            if FT_sensor.sense()[2] > Fz_desired:
                print('reached desired F')
                break
        # step in simulation
        world.step(sleep_dt=dt)
    sp_z = []
    num = []
    detx = np.array([0.0, 0.0, 0.0])
    """
    Third step to keep the target force along z axis(vertical to the table), 
    and complete circular motion trajectory on plane xy
    """
    circle_center = np.array([0.5, 0.1]) # the center of the trajectory
    for t in count():
        Fz_desired = 5  # desired force
        # move sphere
        if t == 0:
            z = robot.get_link_world_positions(link_id)[2]
            sphere.position = np.array([circle_center[0] - r * np.sin(w * t + np.pi / 2), circle_center[1] + r * np.cos(w * t + np.pi / 2), z])

        # get current end-effector position and velocity in the task/operational space
        x = robot.get_link_world_positions(link_id)
        dx = robot.get_link_world_linear_velocities(link_id)
        o = robot.get_link_world_orientations(link_id)
        do = robot.get_link_world_angular_velocities(link_id)

        # Get joint positions
        q = robot.get_joint_positions()

        # Get linear jacobian
        if robot.has_floating_base():
            J = robot.get_jacobian(link_id, q=q)[:, qIdx + 6]
        else:
            J = robot.get_jacobian(link_id, q=q)[:, qIdx]

        # Pseudo-inverse: \hat{J} = J^T (JJ^T + k^2 I)^{-1}
        Jp = robot.get_damped_least_squares_inverse(J, damping)
        # Apply the admittance control
        Fz_current = FT_sensor.sense()[2]  # record the current Fz
        print('Fz: ', Fz_current)
        Fz_error = Fz_current - Fz_desired  # record the current error

        # set the M\D\K parameters by heuristic method, these parameters may have a good result
        # M = 1
        # D = 9500
        # K = 500000
        M = 0.2
        D = 4250
        K = 100000
        # Refer the formula (33) in this article [1]
        # the formula is theta_x(k) = Fc(k)*Ts^2+Bd*Ts*theta_x(k-1)+Md*(2*theta_x(k-1)-theta_x(k-2))/(Md+Bd*Ts+Kd*Ts^2)
        numerator = Fz_error * np.square(dt) + D * dt * detx[1] + M * (2 * detx[1] - detx[2])
        denominator = M + D*dt + K*np.square(dt)
        detx_ = numerator / denominator
        print (detx_)
        detx[2] = detx[1]
        detx[1] = detx[0]
        detx[0] = detx_

        zzz = sphere.position[2] + detx[0]

        # circle_center the the centre of the circle trajectory on the table
        sphere.position = np.array([circle_center[0] - r * np.sin(w * t + np.pi / 2), circle_center[1] + r * np.cos(w * t + np.pi / 2), zzz])
        dv = kp * (sphere.position - x) - kd * dx  # compute the other direction tracking error term

        sp_z.append(sphere.position[2])
        num.append(t)

        dw = kp * quaternion_error(sphere.orientation, o) - kd * do
        # evaluate damped-least-squares IK
        dq = Jp.dot(np.hstack((dv, dw)))

        # set joint positions
        q = q[qIdx] + dq * dt
        robot.set_joint_positions(q, joint_ids=joint_ids)

        # print(Fz_error, dv[2])
        if t == 2000:
            break
        # step in simulation
        world.step(sleep_dt=dt)
    plt.plot(num, sp_z)  # plot the position on the z axis
    plt.xlabel("timesteps")
    plt.ylabel("vertical position")
    plt.title("The z axis position during the task")
    plt.show()

if __name__=='__main__':
    # Create simulator
    sim = Bullet()

    # create world
    world = BasicWorld(sim)

    # flag : 0 # PI control
    flag = 0
    # create robot
    robot = UR5(sim, position=[0,0,0.5])
    # robot = 'ur5'
    robot.print_info()
    world.load_robot(robot)
    world.load_table(position=np.array([1, 0., 0.]), orientation=np.array([0.0, 0.0, 0.0, 1.0]))
    # define useful variables for IK
    dt = 1. / 240
    link_id = robot.get_end_effector_ids(end_effector=0)
    print('link_id:',link_id)
    joint_ids = robot.joints  # actuated joint
    damping = 0.01  # for damped-least-squares IK
    wrt_link_id = -1  # robot.get_link_ids('iiwa_link_1')
    qIdx = robot.get_q_indices(joint_ids)
    print('qidx:', qIdx)

    # define gains
    kp = 500  # 5 if velocity control, 50 if position control
    kd = 5  # 2*np.sqrt(kp)

    # create sphere to follow
    sphere = world.load_visual_sphere(position=np.array([0.5, 0., 0.5]), radius=0.05, color=(1, 0, 0, 0.5))
    sphere = Body(sim, body_id=sphere)

    # set initial joint p
    # Positions (based on the position of the sphere at [0.5, 0, 1])
    robot.reset_joint_states(q=[0.0, -1.57, 1.57, -1.57, -1.57, -1.57])

    # define amplitude and angular velocity when moving the sphere
    w = 0.01
    r = 0.1

    # I set the reference orientation to a constant
    sphere.orientation = np.array([-0.49940256, 0.5001992, 0.50019888, 0.50019888])

    FT_sensor = sensors.JointForceTorqueSensor(sim, body_id=robot.id, joint_ids=5)
    # The plotting handle
    plot = EeFtRealTimePlot(robot, sensor=FT_sensor, forcex=True, forcey=True, forcez=True,
                            torquex=True, torquey=True, torquez=True, num_point=1000, ticks=24)
    # FT_ = np.zeros(6)

    plot_t = Thread(target=plotting_thread, args=[plot], name='plotting task')
    manipulator_t = Thread(target=manipulator_thread, args=(world, robot, sphere, FT_sensor), name='manipulator task')

    thread_pools = [plot_t, manipulator_t]
    for thread in thread_pools:
        thread.start()

    for thread in thread_pools:
        thread.join()

[deepakraina99]

I request someone to look into this issue. It's very urgent. Thanks

[TFLQW]

@deepakraina99 Hi, Sorry to respond too late, because of the recent matter. I have tested you code using UR3 (because I have changed the original version of UR5 and UR10 urdf model), I found the coordinate of end- effector of Kuka iiwa and UR series is different. I think you should change the urdf model or you can add the coodinate transformation to modify this mismatch.

[deepakraina99]

Thanks @TFLQW for your response. I think I have already changed the desired orientation (i.e sphere.orienation) as per UR5 robot. Actually I noted down the orientation at the initial position and kept that as desired throughout the motion of robot. I am attaching code for your reference:

# I set the reference orientation to a constant
sphere.orientation = np.array([-0.49940256, 0.5001992, 0.50019888, 0.50019888])

While in Kuka IIWA, this has been kept as

# I set the reference orientation to a constant
sphere.orientation = np.array([1, 0, 0, 0])

So I think orientation will not be an issue in this case. Please let me know if my thinking is right. Thanks!

[TFLQW]

@deepakraina99 In this force control example, I want to keep the force as a constant along the z axis of end-effector. So I choose the sphere.orientation = np.array([1, 0, 0, 0]), If you select sphere.orientation = np.array([-0.49940256, 0.5001992, 0.50019888, 0.50019888]) that mean the z axis of your robot end-effector is not vertical to the desk. The essential cause of the problem you are facing is the difference between the urdf model of iiwa and ur

[deepakraina99]

@TFLQW I have changed UR5 end-effector frame by making changes in the URDF file. You can check the same in the pictures below:

But when I am loading UR5 using the _examples/force_control/Force_controlexample.py, it is still not able to do the appropriate force tracking. I am attaching the video, code, and URDF file for your reference.

Code: code.zip

Can you please guide me regarding this? I shall be thankful to you.

[deepakraina99]

Thanks for your help. I am able to solve the above issue. The error is due to incorrect Joint ID in JointForceTorqueSensor. I have to change the Joint ID to 6 (earlier I was using 5) and it worked fine then.

FT_sensor = sensors.JointForceTorqueSensor(sim, body_id=robot.id, joint_ids=6)

Now the problem is that it is showing fluctuations especially on the one side of the circular trajectory. Although it is able to converge to the desired force value on one side of circle. What could be the reason for this? Attaching a video for your reference.

[TFLQW]

@deepakraina99 I am good to know that it has been solved, but I have not met this new problem, maybe you can modulate the M/D/K parameters or the stiffness of the table (in urdf model).

[deepakraina99]

@TFLQW Thanks for your suggestion. PyRoboLearn is very useful library. Also can you tell me where table urdf is located in package ? I could not find this.

[TFLQW]

@deepakraina99 You can find the table urdf model in pybullet package which address maybe : /lib/python2.7/site-packages/pybullet_data/table

[deepakraina99]

@TFLQW I have been able to solve the issue by changing the object (table). Thank you so much for your help with this.

[lrozo]

@TFLQW and @deepakraina99 thank you so much for the interaction and communication for this issue! It's great to know that @deepakraina99 managed to solve the problem. By the way @deepakraina99, would you like to share your code by integrating in Pyrobolearn? We welcome nice code like the one you are working on! As @TFLQW, you just need to create a PR in order for us to review your code and integrate it in this framework!

[deepakraina99]

@lrozo I would be happy to share my code and contribute to Pyrobolearn. Let me know the process for the same.

[TFLQW]

@lrozo Hi~, I think this version code of the force control have no problem, maybe need add an attention to help the user set the urdf model coordinate system to be the same as the example, or change the stiffness of the robot or the table.

[deepakraina99]

@lrozo I agree with @TFLQW. I am planning to add a few more capabilities to this and then we can create a new PR.

@TFLQW Can the same code work for a wavy surface (picture below)?

[TFLQW]

@deepakraina99 I guess it could work in some other complex surface if you can get appropriate parameters (M/D/K), this example code in the package is used to prove the force control function feasibility in this package, if you want to develop more complex tasks, you can add more useful function, it is very thankful to create the pull request to improve this package.

[deepakraina99]

@TFLQW I have noticed that robot is converging to desired force value but without making any contact with uneven surface. The video below will explain the issue. Any suggestions on this ? I have loaded the uneven surface using following code:

world.load_mesh(filename='examples/tele-ultrasound/wavy_table/wavy_table.stl', position=np.array([0.5, 0.05, 0.2]), orientation=(0, 0, 1, 1), mass=0., scale=(6., 6., 6.))

Attaching the model files for your reference. wavy_table.zip