search

Kinovarobotics / kinova-ros

ROS packages for Jaco2 and Mico robotic arms
BSD 3-Clause "New" or "Revised" License
376 stars 319 forks source link

Error with gravity_compensated_mode.py j2s7s300 #129

Closed Magica closed 3 years ago

Magica commented 6 years ago

Dear all,

When I run the following node: rosrun kinova_demo gravity_compensated_mode.py j2s7s300 , I get the following output and robot does not enter to the gravity compensated mode. I use ubuntu 14.04 and indigo ROS.

Moving robot to candle like position, and setting zero torques, press return to start, n to skip Setting torques to zero, press return torque before setting zero Torque - 0.0290161762387, -0.193171977997, 0.126339733601, -0.0807036012411, 0.005625622347, -0.000657149590552, -0.00621682638302 torque after setting zero Torque - 0.0100783780217, -0.012953213416, 0.00504312058911, -0.0119093703106, -0.00484128342941, -0.0123831778765, 0.00961117632687 Starting gravity compensation mode Service call failed: service [/j2s7s300_driver/in/set_torque_control_parameters] responded with an error: Done!

Additionally in the roslaunch kinova_bringup kinova_robot.launch kinova_robotType:=j2s7s300 use_urdf:=false I get the following: [ WARN] [1521646310.439726361]: void kinova::KinovaAnglesActionServer::actionCallback(const ArmJointAnglesGoalConstPtr&): LINE 137, setSucceeded [ WARN] [1521646313.226455249]: Torques for all joints set to zero [ INFO] [1521646314.720062000]: Setting torque safety factor to 1.000000

martine1406 commented 6 years ago

Hi

Are you able to move the robot by hand after getting "Starting gravity compensation mode", or the robot stays stiff? And when you list the available services after launching kinova_bringup kinova_robot.launch kinova_robotType:=j2s7s300 use_urdf:=false, do you see /j2s7s300_driver/in/set_torque_control_parameters?

I tested on my end and I did not get the "Service call failed: service [/j2s7s300_driver/in/set_torque_control_parameters] responded with an error:"

I do get:

[ WARN] [1521646310.439726361]: void kinova::KinovaAnglesActionServer::actionCallback(const ArmJointAnglesGoalConstPtr&): LINE 137, setSucceeded [ WARN] [1521646313.226455249]: Torques for all joints set to zero [ INFO] [1521646314.720062000]: Setting torque safety factor to 1.000000

Nothing to worry about here. It is not a true warning, it is just information to the user.

Let me know if you still have this issue or if you succeeded to run kinova_demo gravity_compensated_mode.py j2s7s300 without trouble after all. Thanks

Martine

Magica commented 6 years ago

When I run the roslaunch kinova_bringup kinova_robot.launch kinova_robotType:=j2s7s300 use_urdf:=false I get the following:

SUMMARY

PARAMETERS

NODES / j2s7s300_driver (kinova_driver/kinova_arm_driver) j2s7s300_tf_updater (kinova_driver/kinova_tf_updater)

auto-starting new master process[master]: started with pid [29961] ROS_MASTER_URI=http://localhost:11311

setting /run_id to 47cec1c0-3cc8-11e8-9ccc-f8b156fe659e process[rosout-1]: started with pid [29974] started core service [/rosout] process[j2s7s300_driver-2]: started with pid [29986] process[j2s7s300_tf_updater-3]: started with pid [29992] [ INFO] [1523369153.371604193]: kinova_robotType is j2s7s300. [ INFO] [1523369153.373734099]: kinova_robotType is j2s7s300. [ INFO] [1523369153.375796769]: Initializing Kinova USB API (header version: 50300, library version: 5.2.0) [ INFO] [1523369153.539222706]: Found 1 device(s), using device at index 0 (model: Spherical 7DOF Serv, serial number: PJ00900006432381-0 , code version: 393481, code revision: 6) [ INFO] [1523369153.612223955]: Initializing fingers...this will take a few seconds and the fingers should open completely [ INFO] [1523369153.776969557]: The arm is ready to use. j2s7s300_joint_1 j2s7s300_joint_2 j2s7s300_joint_3 j2s7s300_joint_4 j2s7s300_joint_5 j2s7s300_joint_6 j2s7s300_joint_7

martine1406 commented 6 years ago

ok that seems normal... anything in particular I should notice?

Magica commented 6 years ago

For me everything looks normal. The rosservice list also shows that it runs the /j2s7s300_driver/in/set_torque_control_parameters. I still get the same error when I try to activate the gravity_compensated_mode node.

rosservice list output: /j2s7s300_driver/get_loggers /j2s7s300_driver/in/add_pose_to_Cartesian_trajectory /j2s7s300_driver/in/clear_trajectories /j2s7s300_driver/in/home_arm /j2s7s300_driver/in/run_COM_parameters_estimation /j2s7s300_driver/in/set_end_effector_offset /j2s7s300_driver/in/set_force_control_params /j2s7s300_driver/in/set_null_space_mode_state /j2s7s300_driver/in/set_torque_control_mode /j2s7s300_driver/in/set_torque_control_parameters /j2s7s300_driver/in/set_zero_torques /j2s7s300_driver/in/start /j2s7s300_driver/in/start_force_control /j2s7s300_driver/in/stop /j2s7s300_driver/in/stop_force_control /j2s7s300_driver/set_logger_level /j2s7s300_tf_updater/get_loggers /j2s7s300_tf_updater/set_logger_level /rosout/get_loggers /rosout/set_logger_level

martine1406 commented 6 years ago

Are you able to move the robot by hand after getting "Starting gravity compensation mode", or the robot stays stiff?

Magica commented 6 years ago

The robot is stiff. I will try reinstalling the packages and see if it works.

Magica commented 6 years ago

It still does not work when I reinstall the kinova ros packages. I should mention that I have also Kinect package (iai_kinect2) installed in the computer. before the iai_kinect2 package, the gravity compensated mode was working.

martine1406 commented 6 years ago

I have the Kinect package too. Are you able to use the Torque Console and switch the arm to torque mode?
Do you have any more details on the error: "Service call failed: service [/j2s7s300_driver/in/set_torque_control_parameters] responded with an error:" is there anything else appearing in the terminal afterwards? Thank you

Magica commented 6 years ago

Nothing after the: Starting gravity compensation mode Service call failed: service [/j2s7s300_driver/in/set_torque_control_parameters] responded with an error: Done! The node is stopped.

On windows with the API we are able to use the torque mode.

martine1406 commented 6 years ago

@Magica

I am unable to reproduce your issue. I guess you will have to dig in in the KinovaArm::setTorqueControlParametersService and check what goes wrong on your side. When calling this same service (/j2s7s300_driver/in/set_torque_control_parameters) from the terminal via rosservice call, are you successful? or do you get an error?

pirobot commented 6 years ago

I have the exact same behavior on our kinova. I execute the gravity compensation demo and get this:

Moving robot to candle like position, and setting zero torques, press return to start, n to skip Setting torques to zero, press return torque before setting zero Torque - 0.0017992479261, 0.106726542115, 0.0136077804491, -0.00788880418986, 0.0134067609906, 0.00998535007238, 0.0 torque after setting zero Torque - -0.0239478256553, 0.0824932903051, -0.0285632442683, 0.00368456495926, 0.00461620278656, 0.00578505499288, 0.0 Starting gravity compensation mode Service call failed: service [/m1n6s200_driver/in/set_torque_control_mode] responded with an error: Done!

The robot is stiff after that. This error began two days ago and is consistent. The driver returns only: [ INFO] [1536627401.624773153]: Setting torque safety factor to 1.000000 [ INFO] [1536627401.711949628]: Switching to torque control when I execute the demo, no error messages.

martine1406 commented 6 years ago

Hi @pirobot

Are you able to switch your robot to torque mode using the Development Center? What type of robot do you have? From the driver's name, it seems like a mico robot, but I just want to confirm. What if you call directly the service with rosservice call /m1n6s200_driver/in/set_torque_control_mode, does it also respond with an error?

Thank you Martine

martine1406 commented 6 years ago

Also, have you reprogrammed your robot recently? Is there anything that changed (installed a new package on your computer?) You say the issue started in the last two days...

roberto-martinmartin commented 6 years ago

Sorry, same person as pirobot, but from the right account now.

Thanks!

martine1406 commented 6 years ago

Hi roberto

martine1406 commented 6 years ago

Hi roberto

Me again. As it was the case for Magica, I cannot reproduce your issue. I think the best strategy would be to add some print messages in kinova_arm.cpp in setTorqueControlParametersService so you can figure out what is the exception you catch in python. The reason you get an empty error message is most likely the exception is raised in C++ but cannot be read in python (exceptions are not handled cross-language). Would you be able to modify this function on your end and give us more details?

Magica commented 6 years ago

Dear all,

In my case I made a new catkin_ws and download the ros package of koniva and everything works. I was not able to figure out why in the one workspace wasn't working while on the other it worked.

Best regards, Magica

On Tue, Sep 11, 2018, 22:32 martine1406 notifications@github.com wrote:

Hi roberto

Me again. As it was the case for Magica, I cannot reproduce your issue. I think the best strategy would be to add some print messages in kinova_arm.cpp in setTorqueControlParametersService https://github.com/Kinovarobotics/kinova-ros/blob/master/kinova_driver/src/kinova_arm.cpp#L261 so you can figure out what is the exception you catch in python. The reason you get an empty error message is most likely the exception is raised in C++ but cannot be read in python (exceptions are not handled cross-language). Would you be able to modify this function on your end and give us more details?

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub https://github.com/Kinovarobotics/kinova-ros/issues/129#issuecomment-420415490, or mute the thread https://github.com/notifications/unsubscribe-auth/AFrY_Sug9TczaIbExrYS4j2IBaIa8zDTks5uaB34gaJpZM4SyL7X .

roberto-martinmartin commented 6 years ago

Hi I tried the Torque console and I cannot turn torque mode on. I don't think it is a problem of the ROS service then. One thing I was doing, but I do not expect to cause this fatal behavior, is to change the direction of the gravity vector programatically because our arm is 90 tilted. However, I expect this value to be resetted when powered off. Also, I tried changing it with the Torque Command Gui: nothing changed.

Just to clarify: I press "Switch torque control" in the GUI, the arm is still stiff, the terminal where I started the GUI does not show anything.

martine1406 commented 6 years ago

Hi Roberto

By default, the robot will assume it is in a vertical position, and the gravity vector is 0, 0, -9.81. If your robot is mounted horizontally, you need to change the gravity vector as you say. You can do it in the Torque Console, and you will have to send it back every time you reboot the robot. You can also change it using the API function SetGravityVector.

What you can try is the following

Note: it is true the robot might have more trouble switching to torque mode in an horizontal configuration than in a vertical configuration. It is still possible to do it, but it was less heavily tested. Is it possible to test if it is still possible to switch to torque mode in a vertical configuration?

Note2: My guess is that the zero torque calibration we screwed up if you used the gravity compensation demo as is on a robot mounted horizontally. i.e. you sent the robot in a candle-like position, which is a good zero torque position for a robot mounted vertically, but which is not at all appropriate for a robot mounted horizontally. If you think this is what happened, then please go back to a vertical position, do the zero torque calibration again, and try to switch the robot to torque mode in a vertical configuration. Then mount the robot horizontally and follow the steps I have mentioned above with the changing of the gravity vector and the force monitoring. Eventually, if you need it, we can help you find a good zero torque calibration position for a robot mounted horizontally.

Thank you Martine

roberto-martinmartin commented 6 years ago

Just for me to understand before I can do the experiments you suggested (I can't access the robot for some hours). If the torques are not correctly zero-ed and/or the gravity vector is not correctly set up, the robot won't switch to torque control mode and it won't send any error message to the torque console?

I was expecting that the robot always switches to torque mode when you ask for it, even if the torque sensors have not been zeroed.

Elaborating on our procedure: First we set the gravity vector to the right direction (in our case + or -y, I don't remember now). We had to create our own service for that since the function is not externally accessible via ROS. Then we zero the sensors using a modified version of the gravity compensation python script in the kinava demo package. To zero the torque sensors we command the robot so that the arm points upwards (this is the modification) and the first joint has as few load as possible. We zero the torque values. Then we switch to gravity compensation. The last step fails (output in my first message as pirobot). Calling directly the rosservice also fails.

@Magica : thanks for your input but at this point I think I should first get the torque mode running with the GUI to rule out possible hardware issues.

roberto-martinmartin commented 6 years ago

Hi Martine, there are some unclear things on what you sent: 1) "Checking the forces in the Development center" means really the linear force in Cartesian space (Fx,Fy,Fz)? I guess so 2) You didn't mention any specific configuration of the arm 3) Should I zero the torque sensors in the joints first?

Without zeroing, changing the gravity decreases the forces but not to less than 2 N.

Oh! Actually now I could switch to torque mode with the Torque Console! So I had to bring it to "my candle configuration (pointing upwards when the arm is mounted horizontally), changed the gravity vector and reset the torque sensors. Then I can switch to torque mode / gravity compensation, although when I push it once in a while switches back alone to position control and I need to click again on the switch.

OK, again for me to understand: switching to torque mode (gravity compensation) is only possible when the sensed torques are close to zero? why? Anyway, this does not explain why I couldn't use the gravity compensation demo with the modify pose to point upwards, since the demo sends the robot to the candle position, zeros the sensors and then launches the grav comp.

Ideas? (maybe now the Magica's tip is useful, I can try cleaning+recompiling the catkin repo and see) Thanks

martine1406 commented 6 years ago

Hi Roberto

Let me answer your questions and comment:

Just for me to understand before I can do the experiments you suggested (I can't access the robot for some hours). If the torques are not correctly zero-ed and/or the gravity vector is not correctly set up, the robot won't switch to torque control mode and it won't send any error message to the torque console? yes that's it.

I was expecting that the robot always switches to torque mode when you ask for it, even if the torque sensors have not been zeroed. no that would be potentially dangerous. The robot could start accelerating very fast if the torque sensors are not well calibrated.

Elaborating on our procedure: First we set the gravity vector to the right direction (in our case + or -y, I don't remember now). We had to create our own service for that since the function is not externally accessible via ROS. Then we zero the sensors using a modified version of the gravity compensation python script in the kinava demo package. To zero the torque sensors we command the robot so that the arm points upwards (this is the modification) and the first joint has as few load as possible. We zero the torque values. Then we switch to gravity compensation. The last step fails (output in my first message as pirobot). Calling directly the rosservice also fails. that's clear thank you. But I still do not know why it fails.

"Checking the forces in the Development center" means really the linear force in Cartesian space (Fx,Fy,Fz)? I guess so yes, that's it

You didn't mention any specific configuration of the arm you can choose any configuration. But maybe start with your custom zero torque position (if the robot is mounted horizontally) or the traditional zero torque position (if the robot is mounted vertically).

Should I zero the torque sensors in the joints first? it's a good idea, but it is not mandatory, unless you see that when you put your arm in the zero torque position, the torque readings are far from zero.

Oh! Actually now I could switch to torque mode with the Torque Console! that's good news.

OK, again for me to understand: switching to torque mode (gravity compensation) is only possible when the sensed torques are close to zero? why? because it could be dangerous otherwise (see my earlier comment)

Anyway, this does not explain why I couldn't use the gravity compensation demo with the modify pose to point upwards, since the demo sends the robot to the candle position, zeros the sensors and then launches the grav comp. indeed, this is still a mystery.

Ideas? yes, you can try what Magica recommended. Or else, if it still does not work, you can try adding some prints in setTorqueControlParametersService as I recommended to give us more info on the error you are experiencing.

Thank you and good luck Martine

mr-mikmik commented 6 years ago

Hi all,

I got the same problem in my JACO2 (j2n6s300) as I am unable to set the torque control. I am running Ubuntu 16.04 and my ROS distribution is kinetic. The robot has installed the latest firmware.

I started as Magica failing to set the gravity compensation, and I followed the thread. The problem as Roberto it is not coming from ROS, as I have tried the setting it through Development Center - Torque Console (checking low forces and so) and the robot remains stiff. The robot is mounted as the default configuration (gravity as -z).

Any ideas?

Thanks!!

Mik

nsaif-kinova commented 6 years ago

Hello mr-mikmik, Please check the solution provided by Martine https://github.com/Kinovarobotics/kinova-ros/issues/129#issuecomment-420425261

It is very important to calibrate your torque sensors before you try to switch the arm to Torque. After calibrating the torque sensors, send the robot to home position and you should be able to switch to torque from the torque console. Sincerely,

mr-mikmik commented 6 years ago

Hi Nasaif,

Thanks for your answer. I wrote the post because I have already checked that without no luck. The robot remains stiff as hell. After calibrating the sensors, and verifying the measures, there is no way to set the torque control mode. Just to make sure:

The problem comes when I call the set_torque_control_mode service, as it fails. The message I get is: _ERROR: service [/j2n6s300_driver/in/set_torque_controlmode] responded with an error:

Despite this, I got in the kinova_robot.launch an info message saying Switching to torque control, but the robot seems to pay no attention.

Moreover, the Torque Console seems not working at all. The only action that seem to have effect is checking the connection state, which returns Good Connection. There is some errors in the terminal though: _QMetaObject::connectSlotsByName: No matching signal for on_VibrationControllerActive_clicked() QMetaObject::connectSlotsByName: No matching signal for on_VibrationControllerNotActive_clicked() QMetaObject::connectSlotsByName: No matching signal for on_Home_pressed() QMetaObject::connectSlotsByName: No matching signal for on_SetAngular_pressed() QMetaObject::connectSlotsByName: No matching signal for on_SetCartesian_pressed() QMetaObject::connectSlotsByName: No matching signal for on_SendActuatorGainDamping_pressed() QMetaObject::connectSlotsByName: No matching signal for on_groupBox_3_toggled(bool) QMetaObject::connectSlotsByName: No matching signal for on_VibrationControllerActive_pressed() QMetaObject::connectSlotsByName: No matching signal for on_VibrationControllerNotActivepressed()

I am quite confused.

Thanks,

Mik

nsaif-kinova commented 6 years ago

I am sorry you are still having a problem, but if you are getting a perfect 0.0 this is most probably because your robot is not in service mode. Can you please send me your robot serial number? You can send it to support@kinova.ca and we will help you to solve this problem. Sincerely,

roberto-martinmartin commented 6 years ago

I will just give a current report on our side. We got gravity compensation to work more or less stable. The process includes setting up the gravity vector (-y in our case), going to a different candle position where the robot points upwards, zeroing the torque sensor values and then setting the torque mode. This last step always returns an empty error message, but sometimes works, sometimes don't. If it doesn't work I repeat the zeroing and the setting torque mode on several times and after N attempts, it switches. I will try to dig into the error when I have time. Thank you for the support.

A final question: you mentioned you could indicate the best candle position for our robot configuration. As I said, in our case gravity points in -y direction. What would be a good candle configuration? I have problems to zero the first joint. Bye

martine1406 commented 6 years ago

Hi @roberto-martinmartin

There is no ideal position, but usually I put the arm in a kind of L position as you do. e.g. joint 1: 90 joint 2: 270 joint 3: 180 joint 4: 0 joint 5: 180 joint 6: 180

You could also use a downward L shape.

It could also make sense to zero the torques with an arm completely straight in the horizontal direction, with those angles, as you would get only cross torque on all the joints: e.g. joint 1: 180 joint 2: 180 joint 3: 180 joint 4: 180 joint 5: 180 joint 6: 180

I am not sure how much this would affect/improve your calibration. As joint 1 experiences a lot of cross-torque in a horizontal mounting configuration, and as the torque sensors inside the robot are sensitive to cross-torque up to a certain point, I doubt you will be able to reach a perfect zero. But I would try to find the best zero-torque position that is the closest possible from the workspace you will most frequently use. e.g. if you will use the arm to reach mainly objects located in an horizontal plane in front of the robot, I would probably try to use the last calibration position (arm completely straight in the horizontal direction)

I hope this helps Martine

felixmaisonneuve commented 3 years ago

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