The accuracy of torque control mode

[YummyOu]

I would like to use torque control mode to send the torque to control the robot. But the accuracy between desired and actual torque looks quite big.

The process to set the torque control mode by 1. robot moves to a candle-like position, 2. ZeroTorque, 3.SetTorqueControlParameters, 4.Switch to torque control from position control.

When I published the torque cmd for joint-6 (with red line on the right-bottom graph), the result looks like the following figures.

I use Ubuntu16.04, kinetic, jaco2.

Is that a normal result? or what's wrong with my setting even program?

[felixmaisonneuve]

Hi @YummyOu,

This is the first time I see someone trying to use torque commands to control the arm. Usually people will use joint velocities or joint positions. I feel like torque can vary a lot based on the actuator age and wear. Torque is also dependant of external force applied on the arm (gravity). I think using torque would result in very inconsistant movements and velocities.

In the last graph, the difference between computed and actual force in that last graph does look a bit off. I don't think the arm has been optimized for this kind of request, so I wouldn't except much precision.

I don't think there is anything wrong with your arm, but if you want I could try it on another arm to see if I get a similar output. For that, a code sample would be greatly appreciated.

Like I mentionned, I think the arm was not meant to be used that way, so I would not except much from it in that scenario. I don't think there is much you can do about it.

Regards, Felix

[YummyOu]

Hi @felixmaisonneuve,

This is the first time I see someone trying to use torque commands to control the arm. Usually people will use joint velocities or joint positions.

I would like to do some impedance control and admittance control on Kinova, so try to test the torque control mode first.

I feel like torque can vary a lot based on the actuator age and wear. Torque is also dependant of external force applied on the arm (gravity).

We bought the Jaco2 in 2017, and the robot now with default 2-finger grasper. How could I offset or calibrate the external force(gravity)?

if you want I could try it on another arm to see if I get a similar output

Yes, sure. That would be nice if you could test on the other arms:) I just publish a sine signal to the robot.

import sys

import actionlib
import rospy
import kinova_msgs.msg
import geometry_msgs.msg
import std_msgs.msg
import math
import _thread
from kinova_msgs.srv import *
import argparse
from sensor_msgs.msg import JointState

from robot_control_modules import *

class Robot():

    def __init__(self): 

        # init arg
        self.JOINT_NUM = 6  
        self.prefix = 'j2n6s300_'

        # init robot state
        self.init_robot_state() 

        # subscriber to get feedback    
        self._jointstate_sub = rospy.Subscriber("/j2n6s300_driver/out/joint_state", JointState, self.curJointStateCallback)

        rospy.sleep(1)

    def init_robot_state(self): 
        self.curJoint = [0]*self.JOINT_NUM
        self.curVel = [0]*self.JOINT_NUM
        self.curTau = [0]*self.JOINT_NUM

    def curJointStateCallback(self, feedback):      
        self.curJoint = feedback.position
        self.curVel = feedback.velocity
        self.curTau = feedback.effort

class RobotControl():

    def __init__(self): 

        self.robot = Robot()

        # publish joint torque commands
        self._torque_pub = rospy.Publisher('/j2n6s300_driver/in/joint_torque', kinova_msgs.msg.JointTorque, queue_size=1)

    def setZeroTorque_service(self):

        input("Set zero torque for sensor")

        service_address = '/j2n6s300_driver/in/set_zero_torques'
        rospy.wait_for_service(service_address)
        try:
            zeroTorques = rospy.ServiceProxy(service_address, ZeroTorques)
            zeroTorques()  
            print("Done!!")            
        except rospy.ServiceException as e:
            print("zeroTorques Service call failed: ")
            repr(e)
            # return None   

    def setTorqueParameters_service(self):

        input("Set torque control parameters")

        service_address = '/j2n6s300_driver/in/set_torque_control_parameters'
        rospy.wait_for_service(service_address)
        try:
            setTorqueParameters = rospy.ServiceProxy(service_address, SetTorqueControlParameters)
            setTorqueParameters()   
            print("Done!!")        
        except rospy.ServiceException as e:
            print("setTorqueParameters Service call failed: ")
            repr(e)
            sys.exit(0) 

    def switchTorquemode_service(self):

        input("Set robot as torque mode")

        service_address = '/j2n6s300_driver/in/set_torque_control_mode'
        rospy.wait_for_service(service_address)
        try:
            switchTorquemode = rospy.ServiceProxy(service_address, SetTorqueControlMode)
            switchTorquemode(1)        
            print("Done!!")   
        except rospy.ServiceException as e:
            print("setTorqueParameters Service call failed")
            repr(e)
            # sys.exit(0)       

    def switchPositionmode_service(self):

        service_address = '/j2n6s300_driver/in/set_torque_control_mode'
        rospy.wait_for_service(service_address)
        try:
            switchPositionmode = rospy.ServiceProxy(service_address, SetTorqueControlMode)
            switchPositionmode(0)        
            print("Switch to position mode!!")   
        except rospy.ServiceException as e:
            print("switchPositionmode Service call failed")
            repr(e)

    def robot_setup(self):

        # print the torque before setup
        print('{:<33}'.format("torque_before_setup:"), ', '.join('{:>7.4f}'.format(k) for k in self.robot.curTau[0:6]))

        ######## robot calibration ########

        # move robot to candle like position
        input("Moving robot to candle like position")
        result = joint_position_client([180, 180, 180, 180, 0, 90, 0], self.robot.prefix)
        # print(result)

        print('{:<33}'.format("torque_after_candle_position:"), ', '.join('{:>7.4f}'.format(k) for k in self.robot.curTau[0:6]))

        # use service to set zero torque
        self.setZeroTorque_service()
        print('{:<33}'.format("torque_after_setZeroTorque:"), ', '.join('{:>7.4f}'.format(k) for k in self.robot.curTau[0:6]))

        # use service to set torque control parameters  
        self.setTorqueParameters_service()
        print('{:<33}'.format("torque_after_setTorqueParameters:"), ', '.join('{:>7.4f}'.format(k) for k in self.robot.curTau[0:6]))

        # use service to switch to torque control
        self.switchTorquemode_service()
        print('{:<33}'.format("torque_after_switchTorquemode:"), ', '.join('{:>7.4f}'.format(k) for k in self.robot.curTau[0:6]))

    def robot_movinig(self):

        ######## init moving param for torue ########
        t = 0
        tau = [0,0,0,0,0,0,0]
        last_tau = 0
        run_flag = True

        ## sin input ##
        A_torque = [0.5, 0.5, 0.5, 0.5, 0.5, 0.5]
        n = 1
        T = 10*n

        rospy.sleep(1)

        print('{:<33}'.format("torque_before_moving:"), ', '.join('{:>7.4f}'.format(k) for k in tau[0:6]))

        rospy.sleep(1)
        print('--------------------- start ------------------------')
        start = rospy.Time.now().to_sec()

        rate = rospy.Rate(50)
        while t < rospy.Time(T).to_sec() and not rospy.is_shutdown():
            t = rospy.Time.now().to_sec() - start

            tau[5] = A_torque[joint_index]*math.sin(2*math.pi*n*t/T)
            ## sine input ##
            # if run_flag:
            #   tau[joint_index] = A_torque[joint_index]*math.sin(2*math.pi*n*t/T)
            #   last_tau = tau[joint_index]
            # else:
            #   tau[joint_index] = last_tau
            #   run_flag = False

            # publish torque command
            self._torque_pub.publish(kinova_msgs.msg.JointTorque(*tau))

            rate.sleep()

        print('---------------------  end  ------------------------')
        print('{:<33}'.format("torque_after_moving:"), ', '.join('{:>7.4f}'.format(k) for k in self.robot.curTau[0:6]))

        self.switchPositionmode_service()

if __name__ == '__main__':

    rospy.init_node('torque_control_node')

    robot_control = RobotControl()

    rospy.sleep(1) 

    robot_control.robot_setup()

    input("Start to send the torque command! ")
    robot_control.robot_movinig()

[felixmaisonneuve]

If you want to control the arm using admitance, what you need to do is : 1.Put your arm in candle-like position (every joint in a vertical position) 2.Open Development Center (JacoSDK). In the "Monitoring" section, every actuator should have a value close to 180° in the Command and Position columns. Every actuator should also have a Torque value between -10 Nm and 10 Nm. 3.Open "Advanced Settings". In the section "Torque zero", select "apply to all". The "Monitoring" section should now show Torque values close to 0 for each actuator.

4. Open TorqueConsole.
5. Test your connection with the "Test Connection" button. It should change to "Good Connection"
6. Change the "Safety Factor" to 1
7. Click "Switch Torque Control" button.

After that, you should be able to use admitance and move the robot "by hand".

In that same TorqueConsole, you can calibrate and modify the gravity vector.

Thank you for your code sample. I will try to test it on an arm this week so we can compare our data

Regards, Felix

[YummyOu]

I think I have already followed these steps in my code. So let's try.

Thank you so much!

[felixmaisonneuve]

I just want to make sure of one thing.

What do you want to do?

Do you want to use admittance (manually moving the arm)?

[felixmaisonneuve]

Hi @YummyOu,

So I tested your python script. I am not sure exactly what you are trying to do.

The arm is not moving so torque values stay constant at around 0. In your case, the arm seems to move when you send torque commands?

Also, just before calling robot_control.robot_movinig(), your arm is put in admittance mode. Isn't that what you want to do? At that point (before robot_control.robot_movinig()) you can move the arm manually.

Best, Felix

[YummyOu]

Hi @felixmaisonneuve,

Thank you for your testing.

As you see, the arm didn't move. When I tested, each time showed a different result. But I think this amplitude is big enough to make my arm move anytime. Could you try a bigger value like A_torque = 0.7 in robot_control.robot_movinig() to let it move? I would like to see the performance during moving.

I am not sure exactly what you are trying to do.

I want to implement precision control by using torque commands. e.g., I wash a glass by using force control, so I need to use an accurate control method.

I actually don't need to move the arm manually. I want to control it as I wish.

I think I could do as follow:

1. Robot moves to a candle-like position,
2. ZeroTorque,
3. SetTorqueControlParameters
4. Move to the starting position of the trajectory
5. Switch to torque control from position control.

Have you any idea about how could I do?

Btw, do you use rqt_plot to plot the figure? How could you set this style, which looks great!

[felixmaisonneuve]

Hi @YummyOu,

I am still not sure moving the arm using torque commands is the best way to acheive what you want. I think it would be best to move the arm using regular pose control (moving in XYZ axis) or angular commands (moving each joint separately). Then you could do a verification of the torque values and stop the movement if the arm starts to apply to much force. In the same idea, you could also do a verification on the position/velocity of the arm. For example, when the arm hits a wall, it will stop moving and its velocity will drop close to 0. You could stop the movement when you notice the arm is slowing down (meaning it is moving against a wall).

The arm is working at 100Hz, which should be enough to stop any movement before it breaks the glass.

Would something like that be possible in your case?

To answer your second question, yes this is a plot generated by rqt, and for all I know I did not change the style, so I assume this is the default color scheme.

[YummyOu]

Hi @felixmaisonneuve,

Thank you so much!

I could understand what you mean. The position/velocity control is enough for many cases. But we want to research torque command to control the robot. I mean in the science area for research. So we need to use torque command.

[felixmaisonneuve]

Hi @YummyOu,

I played a bit with your script and did some testing on my side, and I don't think you will be able to accurately move the arm using only torque commands.

Here is some of the tests I did. Doing the modification ou suggested (increasing A_torque), I was able to move the joint.

Doing so, I realised you are using a rate of 50Hz, but the robot works at a rate 100hz and it is not a good idea to change this rate. I changed it to 100Hz, but it didn't seem to change anything. I also tried publishing constant values (using rostopic pub -r 100 /j2n6s300_driver/in/joint_torque kinova_msgs/JointTorque "{joint1: 0.0, jnt2: 0.0, joint3: 0.0, joint4: 0.0, joint5: 0.0, joint6: 1.0, joint7: 0.0}" with values of 1.0 and -1.0 for joint6). Here is the result.

After that, I monitored the joint velocity.

In this final test, I wanted to know if the joint velocity would be constant when sending a constant torque. The velocity curve in this test is similar to the velocity curve in the previous test (they are 2 different runs), but I think this is pure luck and there is no real correlation between torque and velocity. Constant torque gives variable velocity I am don't think you will able to "predict" the movement accurately.

My conclusions are that you won't be able to use torque commands to move the arm. I think this inconsistant behaviour depends on the implementation of the torque control mode, which was not designed for such a precise use case. This mode main focus was to be able to move the arm "by hand" in a somewhat smooth and instinctive way. It is therefore not very precise.

I don't think there is much we can do about that.

What you could do is monitor the position or the velocity while moving the arm (with torque commands) and adjust the torque in real time based on that input. For me, the 6th joint always started moving when torque command reached about ~0.5. It also stopped at about ~0.25, because of the momentum I assume. This might vary based on the speed of the joint.

That's about all I can do to help you... I will close this issue, but if you still have questions feel free to ask them and I will respond. If need be, we can re-open it at any time.

Regards, Felix

[YummyOu]

Thanks a lot for your testing.

It looks that it is not a good idea to use torque command to control Kinova. But there are torque input interfaces, I think it would somehow make sense.

I realised you are using a rate of 50Hz, but the robot works at a rate 100hz and it is not a good idea to change this rate. I changed it to 100Hz, but it didn't seem to change anything.

I think I used 50hz is because I have tested the published rate, which is maximal 50hz. But you are right, I should use 100hz better.

You could close the issue:)

[yuweiDu]

Hi! @YummyOu I am currently trying to do impedance control with j2n6s300 and see this issue. Have you finally accomplished your task by impedance control or admittance control? Could you please give me some suggestions? Thank you verry much!

[YummyOu]

Hi @yuweiDu, The torque mode is pretty inaccurate, so I still used velocity command to control the robot. I realized the admittance control in the x,y,z direction finally. A force sensor was installed on the end effector of kinova.

The equation is: