Trouble getting torque control to do anything (ROS Python API)

[Kukanani]

I'm trying to apply torque commands to the last joint of our 7DOF Jaco2 using the ROS command line but cannot get it to work. After launching the driver, I run the following python script:

import rospy
from kinova_msgs.msg import *
from kinova_msgs.srv import *

rospy.init_node("test")

ctl = rospy.ServiceProxy('/j2s7s300_driver/in/set_torque_control_parameters', SetTorqueControlParameters)
on = rospy.ServiceProxy('/j2s7s300_driver/in/set_torque_control_mode', SetTorqueControlMode)
torque = rospy.Publisher('/j2s7s300_driver/in/joint_torque', JointTorque, queue_size=1)

joints = {"joint1": 0.0,
          "joint2": 0.0,
          "joint3": 0.0,
          "joint4": 0.0,
          "joint5": 0.0,
          "joint6": 0.0,
          "joint7": 0.0}
ctl()
on(state=1)

rate = rospy.Rate(100)
while(True):
    resp = torque.publish(JointTorque(**joints))
    rate.sleep()

I don't get any errors from the driver node, but the robot stays rigidly in place. From my reading of the documentation, these are the only commands needed to apply a torque command. Am I missing something?

Some other info:

• Our robot has a Robotiq 2-finger gripper.
• I can control the robot just fine using moveit.
• I'm on the kinova-ros-beta branch
• My robot_parameters.yaml (the default):

# Set this parameter to use a specific arm on your system    --> 
# serial_number: PJ00000001030703130

# Joint speed limit for joints 1, 2, 3
jointSpeedLimitParameter1: 10

# Joint speed limit for joints 4, 5, 6
jointSpeedLimitParameter2: 20

# payload: [COM COMx COMy COMz]
#payload: [0, 0, 0, 0]

connection_type: USB #Ethernet 

# Ethernet connection parameters
ethernet: {
  local_machine_IP: 192.168.100.100,
  subnet_mask: 255.255.255.0,
  local_cmd_port: 25000,
  local_broadcast_port: 25025
}

#Torque control parameters
#Do not change these parameters unless you want to change torque control behavior
torque_parameters: {
      publish_torque_with_gravity_compensation: false,
      use_estimated_COM_parameters: false,
    # if torque min/max sepecified, all min/max values need to be specified
    #  torque_min: [80, 80, 80, 80, 80, 80, 80],
    #  torque_max: [90, 90, 90, 90, 90, 90, 90],
    # Decides velocity threshold at which robot switches torque to position control (between 0 and 1)
    # safety_factor: 1,
    # COM parameters
    # order [m1,m2,...,m7,x1,x2,...,x7,y1,y2,...y7,z1,z2,...z7]
    # com_parameters: [0,0,0,0,0,0,0, 0,0,0,0,0,0,0, 0,0,0,0,0,0,0, 0,0,0,0,0,0,0]
}

[nsaif-kinova]

Hello Kukani,
You should specify the payload for your robotiq gripper in this field payload: [0, 0, 0, 0] please refer to this issue #170
Also please don't forget to calibrate your Zero torque.
You can test switching your robot to torque mode in Torque Console (you should add the payload before you switch to torque).
Sincerely,

[sjhansen3]

1. Why are there 7 masses? Is there more documentation on the com_parameters somewhere?
2. Is there a way to force the robot to send zero torque to all joints forever? We are teaching the robot and prefer there to be no gravity compensation or intelligent torque of any kind. The only reason we wish to power the robot is to read the joint positions through the encoders.

[sjhansen3]

I am able to get the robot to respond to torque control commands very sporadically - sometimes it connects and sometimes it does not - and I don't quite understand the cause. Is the KG3 Gripper added as a mass by default?

I sent the following command:

rosservice call /m1n6s300_driver/in/set_torque_control_mode "state: 0"
And received the following error:
ERROR: service [/m1n6s300_driver/in/set_torque_control_mode] responded with an error:

There was nothing written after error. This error seems to not matter either because the robot will switch into torque control sometimes when the error is set, and sometimes when it is not.

Here is the log output from the terminal with kinova bringup:

[ INFO] [1540233927.812906679]: Switching to position control
[ INFO] [1540233929.064850023]: Switching to position control
[ INFO] [1540233930.196339746]: Switching to torque control
[ INFO] [1540233931.882093643]: Switching to torque control
[ INFO] [1540233933.114459618]: Switching to torque control
[ INFO] [1540233934.264728532]: Switching to position control
[ INFO] [1540233938.467065293]: Switching to position control
[ INFO] [1540233939.788942966]: Switching to torque control

terminate called after throwing an instance of 'kinova::KinovaCommException'

 what():  KinovaCommException: Could not get the angular command (return code: 1015)

After a quite a few failed attempts to turn on force control, the robot disconnected.

After restarting the robot the torque control was able to be set a few times, and then stopped responding to set requests, after 3-4 failed torque mode changes it started switching again at which point I got another kinova comm exception error code 1015.

[martine1406]

1. This applies for a 7 dof. For a 6 dof robot, you would need to provide the parameters this way:
   [m1,m2,m3,m4,m5,m6,x1,x2,x3,x4,x5,x6,y1,y2,y3,y4,y5,y6,z1,z2,z3,z4,z5,z6] . Your best source of information would be the API documentation (function SetGravityManualInputParam and SetGravityOptimalZParam depending on which gravity compensation mode you want to use).

2. Set all the parameters to zero and use the optimal mode, with a safety factor of 1. For switching to torque mode, you have two choices in this case: either keep all the parameters to zero and switch from the all-joints-straight-up position or switch and then set all the parameters to zero to deactivate the gravity compensation. Are you sending your own gravity compensation?

Next questions:
Is the KG3 Gripper added as a mass by default? yes, if it is connected
rosservice call /m1n6s300_driver/in/set_torque_control_mode "state: 0" state: 0 is the position mode. Perhaps related issue: #129
1015 error means that the robot has lost its connection with the computer, as you noticed
**Do you call /m1n6s300_driver/in/set_torque_control_mode repeatedly? **

The robot will switch back to position mode for the following reasons:

• a joint gets too close from its angle limits
• if you have not set the safety factor to 1, the arm has reached a velocity that is too high
• the hand is considered too close from the base
• the measured torques are too far away from the computed gravity torques (if you deactivating gravity compensation, this could perhaps trigger the switch to position mode)

I have the impression that your case might be rather complex. If you want, we could organize a video call to evaluate the situation. You can reach me via the support e-mail: [email protected].

[felixmaisonneuve]

This thread has being inactive for over 2 years, I am closing this issue. If there is still a problem, please create a new issue.

Thank You