Cannot switch to torque control mode

[TheHidj]

Hello, I am currently working with the Jaco Gen2 (twist) and I cannot control the robot using the torque control mode.

I followed all the steps presented in the advanced user guide : 1- ZeroTorqueCalibration in a Candle like position; 2- Gravity Param Estimation; 3- Removed safety Parameters (set to 1); 4- Set the Torque Inactivity Type to 1 (sending zero torque instead of switching to position control);

All the previous steps were done through the API, therefore I tired using the TorqueConsole.exe but it did not work either.

I removed the end-effector hand, can that be the cause of the problem ? If so, how can I bypass the problem ?

More info: When using GetAngularTorqueGravityEstimation() , the computed torque are all of -1.07374e+08 N.m. The control type is set actually set to TORQUE when using the GetTrajectoryTorqueMode() function to find the control type.

The read sent torque command (GetAngularTorqueCommand()) is always of -1.07374e+08 N.m for all torques even if the array used in SendAngularTorqueCommand() is different.

Thank you,

[felixmaisonneuve]

Hi @TheHidj,

The torque control mode can be a bit tricky on Jaco arms. Can you tell me what API commands you used?

I am not sure if removing the gripper could prevent the use of the torque control mode on the arm.

Your readings are also weird. Does the Torque readings look good in DevCenter? I will also mention that you cannot use DevCenter/TorqueConsole at the same time as your ROS driver is running.

Best, Felix

[TheHidj]

Here is the main !

I mixed here the torque sensors calibration in the first part, the gravity estimation torque parameters (that i got from the advanced user guide code) and finally the torque control. Even without the calibration and without changing the EstimationParam, the torque control was not working.

for (int i = 0; i < devicesCount; i++) { // home position MySwitchTrajectoryTorque(POSITION); MyMoveHome(); // candle like position TrajectoryPoint pointToSend; pointToSend.InitStruct();

              //We specify that this point will be an angular(joint by joint) position.
              pointToSend.Position.Type = ANGULAR_POSITION;

              //We get the actual angular command of the robot.
              MyGetAngularCommand(currentCommand);
              MyGetAngularPosition(currentAngle);

              pointToSend.Position.Actuators.Actuator1 = currentCommand.Actuators.Actuator1 + (180 - currentAngle.Actuators.Actuator1);
              pointToSend.Position.Actuators.Actuator2 = currentCommand.Actuators.Actuator2 + (180 - currentAngle.Actuators.Actuator2);
              pointToSend.Position.Actuators.Actuator3 = currentCommand.Actuators.Actuator3 + (180 - currentAngle.Actuators.Actuator3);
              pointToSend.Position.Actuators.Actuator4 = currentCommand.Actuators.Actuator4 + (0 - currentAngle.Actuators.Actuator4);
              pointToSend.Position.Actuators.Actuator5 = currentCommand.Actuators.Actuator5 + (0 - currentAngle.Actuators.Actuator5);
              pointToSend.Position.Actuators.Actuator6 = currentCommand.Actuators.Actuator6 + (180 - currentAngle.Actuators.Actuator6);

              MySendBasicTrajectory(pointToSend);
              Sleep(10000);
              //Set torque to zero
              Sleep(1000);      result = (*MySetTorqueZero)(16);
              Sleep(1000);      result = (*MySetTorqueZero)(17);
              Sleep(1000);      result = (*MySetTorqueZero)(18);
              Sleep(1000);      result = (*MySetTorqueZero)(19);
              Sleep(1000);      result = (*MySetTorqueZero)(20);
              Sleep(1000);      result = (*MySetTorqueZero)(21);
              Sleep(1000);      result = (*MySetTorqueZero)(22);
              Sleep(1000);      result = (*MySetTorqueZero)(25);

              // Set Optimal param 
              float OptimalzParam[OPTIMAL_Z_PARAM_SIZE]
              ifstream file("ParametersOptimal_Z.txt");
              if (file.is_open())
              {
                  for (int i = 0; i < OPTIMAL_Z_PARAM_SIZE; ++i)
                  {
                      file >> OptimalzParam[i];
                      OptimalzParam[i] = 0;
                      cout << i << " OptimalzParam Gravity : " << OptimalzParam[i] << endl;
                  }
              }
              cout << "*********************************" << endl
              MySetGravityOptimalParameter(OptimalzParam);
              // informs the robot on the new optimal gravity parameters
              MySetGravityType(OPTIMAL); //sets the gravity compensation mode to 
              // Set the safety factor to 0.6
              MySetTorqueSafetyFactor(1)
              // Set the vibration controller to 0.5
              MySetTorqueVibrationController(0.5);
              MySetTorqueInactivityType(1)
              //Set control type
             MySwitchTrajectoryTorque(TORQUE);
             MySetTorqueControlType(DIRECTTORQUE)
             // init command 
             float TorqueCommand[COMMAND_SIZE];
             float Result[COMMAND_SIZE];
             for (int i = 0; i < COMMAND_SIZE; i++)
             {
                 TorqueCommand[i] = 0;
             }
             // Send the torque commands for 2 seconds
             for (int i = 0; i < 400; i++)
             {
                 // Torque of 0.5Nm on joint 6
                 TorqueCommand[0] = 1;
                 // Send the torques
                 MySendAngularTorqueCommand(TorqueCommand);
                 // Sleep 10 ms
                 Sleep(10);
             }

[felixmaisonneuve]

Could you try to reboot the arm and run the gravity_compensated_mode.py example from the kinova_demo folder? Assuming you didn't change anything in the example or your ROS driver

Edit : You also need to close DevCenter and Torque Console before launching your ROS driver. Also, make sure no other program is running in the background that could connect to the arm's API

[Betancourt20]

Hi, I'm having the same problem as @TheHidj. First It is confused what the pdf advices about the calibration of the torque sensor I'm using the 6 DoF spherical and I do not know how to put the joint 5 at zero ? mechanical speaking I believe is impossible, or maybe I cannot see it. So I reset everything on the candle position. As @TheHidj did, my arm does not have the wrist. When I switch to Torque control

    this->comm_->getJointTorques(tqs_);
    this->comm_->setTorquesControlSafetyFactor(factor_);
    this->comm_->SetTorqueControlState(state_);

I switch without any problem but the arm moves alone smoothly, I do not know if the Gravity compensation is working well in torque control, which in theory should be (In position control, angular control etc, it works well). Then I try to send the torques obtained from the API

  float tau_f[] = {tqs_.Actuator1,tqs_.Actuator2,tqs_.Actuator3,tqs_.Actuator4,tqs_.Actuator5,tqs_.Actuator6};
  this->comm_->setJointTorques(tau_f);

and I got the error

 Setting torque safety factor to %f0.6
 Switching to torque control
 terminate called after throwing an instance of 'kinova::KinovaCommException'
  what():  KinovaCommException: Could not send joint torques (return code: 9999)

I looked into the files and the error correspond to

//If the robot answered a NACK to our command
#define ERROR_NACK_RECEIVED 9999

but I do not have any idea what does that mean. Do you have any hint that could help us please? Thanks in advance ;) Julio

[Betancourt20]

Little update ...

this->comm_->runCOMParameterEstimation(robot_type);
ROBOT_TYPE robot_type = SPHERICAL_6DOF_SERVICE;

and I got the optimal parameters for the gravity. Then, I called them with

float TorqueCommand[COMMAND_SIZE];
for (int i = 0; i<COMMAND_SIZE; i++)
{
 TorqueCommand[i] = 0.0; //initialization
}
GRAVITY_TYPE grav_type = OPTIMAL;
this->comm_->setRobotCOMParam(grav_type,params);
TorqueCommand[5] = -1.0f;
this->comm_->setJointTorques(TorqueCommand);

where for sure params are those one I obtained running the algorithm. Thus, I got the same error as before

 terminate called after throwing an instance of 'kinova::KinovaCommException'
  what():  KinovaCommException: Could not send joint torques (return code: 9999)

So, I do not know what could be possible? For sure, with this parameters the manipulator behaves better ;)

[Betancourt20]

[Update]

So, I tried with the torque control of the SDK and it worked. But It worked only when I was using the USB port and not with the Ethernet (all the tests I have been doing is with that). Then I changed for the USB com with my program and the torque control worked pretty well.

It is possible that I cannot use Torque control on my Jaco 2 spherical ???

[felixmaisonneuve]

Hi @Betancourt20,

I fixed exactly this a couple months ago. It was impossible to send Joint Torques via Ethernet for arms with the gripper removed from the arm or with a single finger configuration (the robotiq gripper on the EOD arms). I assume this is the arm you have. Otherwise you recieve the NACK message

The best/only option for you at the moment is to use USB if you want to use torque commands (since my fix is not included in the most recent arm's firmware). If you must absolutely use an Ethernet cable and torque commands, you could contact the support team and we might be able to work something out.

@TheHidj, are you also using Ethernet communication with arm without gripper or with a single finger?

[Betancourt20]

Hi @felixmaisonneuve , when switching to Torque mode, gravity is compensated by default. It is possible to switch to Torque mode without considering the aforementioned? if yes, could you give a hint please? Best regards, Julio

[TheHidj]

@felixmaisonneuve I contacted and had a meeting with Kinova Support. My robotic arm was a Gen 1 which doesn't support torque control, that was my bad. However, I am now working with a MICO (gen 2/6Dof), and wanted to switch to ethernet connection for better communication rate (500 Hz). Your fix was uploaded for what firmware ? Maybe I have the good one or I will contact Kinova to setup torque control with ethernet cable ! Thank you for your time.

@Betancourt20 you could set all parameters to zero, I think that will sent the gravity compensation to zero.

[Betancourt20]

Hi @TheHidj , thank you for the advice ;)

By any chance, do you know what is the meaning of the 16 parameters (in my case, Jaco gen2 spherical)? in the code they put

/** @brief Sets COM and COMxyz for all links
  * @arg command[42] - {m1,m2..m7,x1,x2,..x7,y1,y2,...,y7,z1,z2,...z7}
//! */

but I'm confuse, I have 6 links and I don't see the relation of the 16 parameters with the aforementioned.

any idea @felixmaisonneuve Best regards, Julio

[felixmaisonneuve]

Hi @Betancourt20,

Like mentionned, you could maybe try to set gravity parameters to 0. I have never tried either option so be very carefull. I do not think it will work, but that's the only idea I can think of that could remove gravity compensated torques.

The parameters are defined in the HTML documention that you can access from the installation folder of Development Center / Torque Console. In the documentation, search for "setGravity" and you will find all the functions that may be usefull for this : SetGravityManualInputParam, SetGravityOptimalZParam, SetGravityPayload, SetGravityType and SetGravityVector. Parameters definition is mentionned in the first two.

[GS-DUT]

Hi @felixmaisonneuve, I also cannot switch my Jaco2 to torque control mode. My problem is same as the describe in issues #358. And I followed your answer in that issues step in step. When I got to step 7(Click "Switch Torque Control" button), there is nothing print in the terminal where I launch kinova driver with roslaunch kinova_bringup kinova_robot.launch kinova_robotType:=j2s6s300. Is that current? Do you have any hint that could help us please? Thanks in advance.

[felixmaisonneuve]

You cannot uuse the Torque Console at the same time a the Kinova ROS node is running (roslaunch kinova_bringup ...). When sing the Torque Console, make sure there is no ROS node running. I recommand using the Torque Console because it is a fast and easy way to test the torque control mode. If you want to use ROS, I would recommand running the gravity_compensated_mode.py example in the kinova_demo folder. It does very similar steps to what I described in the other issue. Make sure DevCenter and the Torque Console are not running when using ROS

[Betancourt20]

Hi @felixmaisonneuve

Thank you so much for your support. In fact I succeed to use Torque Control without gravity ;) it works quite good. I am using Pinocchio as a library and for sure there are many things I believe can be improved in order to have a good Torque control mode. I sent the commands with and without gravity and the robot works well.

Again, many thanks for your support. Best regards

[felixmaisonneuve]

Great!

Keep in mind the robot was not designed for this kind of use, so you won't be able to achieve super precise work, but it should be good enough to be usable.

Best of luck in your work, Felix

[chocamielpops]

Hi @Betancourt20,

Did you manage to use Torque Control without the end-effector? Did you remove the 6th actuator, or only what's after the 6th actuator? I'm trying to use torque control on a Mico gen2, it works with the end-effector but won't switch if I remove it.

Thanks!

[Betancourt20]

Hi @chocamielpops

Yes yes the problem was not the end-effector ;) and I only removed that's after the 6th actuator. I have the Jaco gen 2 maybe for the Mico is different. But from my experience, the torque control you are gonna get it is not the optimal neither robust. I contacted Kinova support to try to collaborate with them and improve this controller but they said that they nope :( So, Velocity control or force control is the best option for this kind of robot. Maybe Jaco3 is more functional for research purposes.

kind regards, Julio

[chocamielpops]

Hi @Betancourt20 Thanks for your answer! It works now that I've put back the 6th actuator. Did setting gravity parameters to zero allowed you to use torque control without gravity compensation? Which function did you use specifically? Thank you

[Betancourt20]

Hi @chocamielpops , Sorry for the delay. Yes, I succeed to use torque control without gravity compensation but as @felixmaisonneuve advised, I believe it is better to leave it with that compensation and compute your own torque control (based on the library of your choice) without the gravity. I did that with Pinocchio repo. It works pretty well. Best regards