Compliance to enable kinesthetic teaching #630

paliasgh · 2023-05-24T20:06:01Z

paliasgh
May 24, 2023

Hello,

Sorry for bother you again! We have recently started working again with iCubWaterloo01 after a long time and have doubts regarding proper usage of the robot for our research.

Let's say we want our robot to receive some kinesthetic demonstrations, like in this or this video. We are looking for a complaint behaviour in robot's arms, with gravity compensation, in order for a teacher to move the arms easily.

Based on the discussion here and also demoForceImitation, I am setting the following for the first 5 joints in one arm of the robot in my code:

iimp->setImpedance(j,0.2,0.02);
imod->setControlMode(j, VOCAB_CM_TORQUE);  //maybe not needed?
iint->setInteractionMode(j,VOCAB_IM_COMPLIANT);

It does give the feeling when moving robot's arm same as running the demoForceImitation itself, but moving robot's arm is not easy at all as it feels very heavy (especially joint 0 and 1 are hard to move due to the weight of the arm).

I am not sure about the usage of wholebodydynamics and gravityCompensator explained here. Should running these two modules themselves automatically set the torques to compensate the gravity force of the arms? I found an rpc port exists, but enabling/disabling gravity compensation does not affect anything:

icub@icub26:~$ yarp rpc /gravityCompensator/rpc 
>>help
Responses:
   commands are: 

   commands are: 
help         to display this message
gravity_on   to enabl e the gravity compensation 
gravity_off  to disbale the gravity compensation 
external_on  to enable the external input torque 
external_off to disable the external input torque 

>>gravity_on
Response: "gravity compensation on"

The modules are running through this part of start up xml application (with no port to connect manual or by the app):

        <module>
                <name>wholeBodyDynamics</name>
                <parameters>--autoconnect --headV2.7 --imuPortName /imuFilter</parameters>
                <node>icub26</node>
                <dependencies>
                    <port timeout="60.0" request="is_ready" reply="[ok]">/icub/yarprobotinterface</port>
                </dependencies>
        </module>

        <module>
                <name>gravityCompensator</name>
                <parameters>--headV2.7</parameters>
                <node>icub26</node>
                <dependencies>
                    <port timeout="60">/wholeBodyDynamics/rpc:i</port>
                </dependencies>
        </module>

Lastly, I could not find what was mentioned here as useful for our propose (the actionsRenderingEngine). Could you please give some hints/advice/pointers to find more about this topic or highlight possible issues with the above-mentioned implementation? Thanks in advance for your help!

Answered by pattacini

May 27, 2023

Hi @paliasgh

Regarding the following lines you posted above

iimp->setImpedance(j,0.2,0.02);
imod->setControlMode(j, VOCAB_CM_TORQUE);  //maybe not needed?
iint->setInteractionMode(j,VOCAB_IM_COMPLIANT);

Please, consider the table I provided a long time ago, which states that the interaction mode doesn't go along with the torque control mode. Thus, either you enable the torque control mode or the position control mode; for the latter, you have two interaction modes available: stiff and compliant.

If you want to perform kinesthetic teaching, you should enable the torque control mode.

It does give the feeling when moving robot's arm same as running the demoForceImitation itself, but moving …

View full answer

pattacini · 2023-05-27T17:49:58Z

pattacini
May 27, 2023
Maintainer

Hi @paliasgh

Regarding the following lines you posted above

iimp->setImpedance(j,0.2,0.02);
imod->setControlMode(j, VOCAB_CM_TORQUE);  //maybe not needed?
iint->setInteractionMode(j,VOCAB_IM_COMPLIANT);

Please, consider the table I provided a long time ago, which states that the interaction mode doesn't go along with the torque control mode. Thus, either you enable the torque control mode or the position control mode; for the latter, you have two interaction modes available: stiff and compliant.

If you want to perform kinesthetic teaching, you should enable the torque control mode.

It does give the feeling when moving robot's arm same as running the demoForceImitation itself, but moving robot's arm is not easy at all as it feels very heavy (especially joint 0 and 1 are hard to move due to the weight of the arm).

Difficult to judge. It may depend on several factors, including the status of the robot.
Maybe, a video could help.

I am not sure about the usage of wholebodydynamics and gravityCompensator explained here. Should running these two modules themselves automatically set the torques to compensate the gravity force of the arms?

You don't have to interact with these two modules. When your code changes the control modes, then they will kick in under the hood.

The modules are running through this part of start up xml application (with no port to connect manual or by the app):

Correct 👍🏻

Lastly, I could not find what was mentioned here as useful for our propose (the actionsRenderingEngine). Could you please give some hints/advice/pointers to find more about this topic or highlight possible issues with the above-mentioned implementation? Thanks in advance for your help!

actionsRenderingEngine (a.k.a. ARE) is documented at https://robotology.github.io/robotology-documentation/doc/html/group__actionsRenderingEngine.html.

It's a quite old piece of SW that grew wildly as a monolithic general-purpose motor component.
We used it years ago to 🎥fine-tune reaching while relying on an admittance cartesian control or to 🎥 teach actions straight away.

For the second operation, there are suitable RPC commands that you'll find documented:

TEACH ACTION
IMITATE ACTION

ARE was at the core of our Interactive Object Learning demo. However, we no longer maintain iol, although it should be still effective, at least for what concerns the motor part.

0 replies

paliasgh · 2023-05-29T22:07:56Z

paliasgh
May 29, 2023
Author

IMG_3239.mov

Hi @pattacini , thank you very much. I now see your point and can understand the table.

I see that for Kinesthetic teaching VOCAB_CM_TORQUE is the only thing needed, assuming gravityCompensator is working, right? I could also see this here in actionsRenderingEngine source code.

actionsRenderingEngine is actually a very powerful tool! I was not aware of that before, so I coded a few functionalities which were already there. Thanks for pointing to the videos/documentations.

The video attached here is for after running only

imod->setControlMode(0, VOCAB_CM_TORQUE);

on joints 0 to 4 in the left arm of the robot. gravityCompensator is also on. This what I mean by saying "robot's arm is not easy at all as it feels very heavy". It does not seem to provide any torque to hold the arm against gravity. Is it possible that gravityCompensator is not working after the update? Moving robot's arm feels the same even without gravityCompensator.

P.S.

I just discovered that changing the control mode of any joint in robot's arm to torque control mode, either through setControlMode(i,VOCAB_CM_TORQUE) or yarpmotorgui, will make the joint unavailable due to hardware fault (code 7). This might be the reason for the issue reported above.

6 replies

paliasgh Jun 3, 2023
Author

Hi @pattacini , thank you so much!

Searching for ports with ft in their name gave the following list (assuming cartesian interfaces, wholeDodyDynamics, and gravityCompensator modules are all running):

icub@icub26:~$ yarp name list | grep -i ft
registration name /cartesianSolver/left_arm/in ip 10.0.0.26 port 10151 type tcp
registration name /cartesianSolver/left_arm/left_arm/command:o ip 10.0.0.26 port 10129 type tcp
registration name /cartesianSolver/left_arm/left_arm/rpc:o ip 10.0.0.26 port 10127 type tcp
registration name /cartesianSolver/left_arm/left_arm/stateExt:i ip 10.0.0.26 port 10131 type tcp
registration name /cartesianSolver/left_arm/out ip 10.0.0.26 port 10153 type tcp
registration name /cartesianSolver/left_arm/rpc ip 10.0.0.26 port 10114 type tcp
registration name /cartesianSolver/left_arm/torso/command:o ip 10.0.0.26 port 10119 type tcp
registration name /cartesianSolver/left_arm/torso/rpc:o ip 10.0.0.26 port 10118 type tcp
registration name /cartesianSolver/left_arm/torso/stateExt:i ip 10.0.0.26 port 10121 type tcp
registration name /gravityCompensator/left_arm/client/command:o ip 10.0.0.26 port 10230 type tcp
registration name /gravityCompensator/left_arm/client/rpc:o ip 10.0.0.26 port 10229 type tcp
registration name /gravityCompensator/left_arm/client/stateExt:i ip 10.0.0.26 port 10231 type tcp
registration name /gravityCompensator/left_arm/ctrl_offset:i ip 10.0.0.26 port 10246 type tcp
registration name /gravityCompensator/left_arm/exec_torques:o ip 10.0.0.26 port 10251 type tcp
registration name /gravityCompensator/left_arm/gravity_torques:o ip 10.0.0.26 port 10256 type tcp
registration name /gravityCompensator/left_leg/client/command:o ip 10.0.0.26 port 10236 type tcp
registration name /gravityCompensator/left_leg/client/rpc:o ip 10.0.0.26 port 10235 type tcp
registration name /gravityCompensator/left_leg/client/stateExt:i ip 10.0.0.26 port 10237 type tcp
registration name /gravityCompensator/left_leg/ctrl_offset:i ip 10.0.0.26 port 10248 type tcp
registration name /gravityCompensator/left_leg/exec_torques:o ip 10.0.0.26 port 10253 type tcp
registration name /gravityCompensator/left_leg/gravity_torques:o ip 10.0.0.26 port 10258 type tcp
registration name /iCub/left_hand/analog:o ip 10.0.0.26 port 10376 type tcp
registration name /iCub/left_hand/analog:o/rpc:i ip 10.0.0.26 port 10375 type tcp
registration name /icub/cartesianController/left_arm/cartesianSolver/left_arm/in ip 10.0.0.2 port 10035 type tcp
registration name /icub/cartesianController/left_arm/cartesianSolver/left_arm/out ip 10.0.0.2 port 10036 type tcp
registration name /icub/cartesianController/left_arm/cartesianSolver/left_arm/rpc ip 10.0.0.2 port 10037 type tcp
registration name /icub/cartesianController/left_arm/command:i ip 10.0.0.2 port 10038 type tcp
registration name /icub/cartesianController/left_arm/events:o ip 10.0.0.2 port 10040 type tcp
registration name /icub/cartesianController/left_arm/rpc:i ip 10.0.0.2 port 10041 type tcp
registration name /icub/cartesianController/left_arm/state:o ip 10.0.0.2 port 10039 type tcp
registration name /icub/joint_vsens/left_arm:i ip 10.0.0.2 port 10072 type tcp
registration name /icub/joint_vsens/left_leg:i ip 10.0.0.2 port 10074 type tcp
registration name /icub/left_arm/analog:o ip 10.0.0.2 port 10099 type tcp
registration name /icub/left_arm/analog:o/rpc:i ip 10.0.0.2 port 10066 type tcp
registration name /icub/left_arm/command:i ip 10.0.0.2 port 10008 type tcp
registration name /icub/left_arm/rpc:i ip 10.0.0.2 port 10007 type tcp
registration name /icub/left_arm/state:o ip 10.0.0.2 port 10009 type tcp
registration name /icub/left_arm/stateExt:o ip 10.0.0.2 port 10010 type tcp
registration name /icub/left_foot/analog:o ip 10.0.0.2 port 10104 type tcp
registration name /icub/left_foot/analog:o/rpc:i ip 10.0.0.2 port 10071 type tcp
registration name /icub/left_hand/analog:o ip 10.0.0.2 port 10084 type tcp
registration name /icub/left_hand/analog:o/rpc:i ip 10.0.0.2 port 10051 type tcp
registration name /icub/left_leg/analog:o ip 10.0.0.2 port 10102 type tcp
registration name /icub/left_leg/analog:o/rpc:i ip 10.0.0.2 port 10069 type tcp
registration name /icub/left_leg/command:i ip 10.0.0.2 port 10016 type tcp
registration name /icub/left_leg/rpc:i ip 10.0.0.2 port 10015 type tcp
registration name /icub/left_leg/state:o ip 10.0.0.2 port 10017 type tcp
registration name /icub/left_leg/stateExt:o ip 10.0.0.2 port 10018 type tcp
registration name /icub/skin/left_arm ip 10.0.0.2 port 10088 type tcp
registration name /icub/skin/left_arm/rpc:i ip 10.0.0.2 port 10055 type tcp
registration name /icub/skin/left_foot ip 10.0.0.2 port 10094 type tcp
registration name /icub/skin/left_foot/rpc:i ip 10.0.0.2 port 10061 type tcp
registration name /icub/skin/left_forearm ip 10.0.0.2 port 10087 type tcp
registration name /icub/skin/left_forearm/rpc:i ip 10.0.0.2 port 10054 type tcp
registration name /icub/skin/left_hand ip 10.0.0.2 port 10086 type tcp
registration name /icub/skin/left_hand/rpc:i ip 10.0.0.2 port 10053 type tcp
registration name /icub/skin/left_leg_lower ip 10.0.0.2 port 10093 type tcp
registration name /icub/skin/left_leg_lower/rpc:i ip 10.0.0.2 port 10060 type tcp
registration name /icub/skin/left_leg_upper ip 10.0.0.2 port 10092 type tcp
registration name /icub/skin/left_leg_upper/rpc:i ip 10.0.0.2 port 10059 type tcp
registration name /icubSim/cam/left/depthImage:o ip 10.0.0.26 port 10397 type tcp
registration name /icubSim/cam/left/rgbImage:o ip 10.0.0.26 port 10396 type tcp
registration name /icubSim/cam/left/rpc:i ip 10.0.0.26 port 10395 type tcp
registration name /icubSim/left_arm/command:i ip 10.0.0.26 port 10388 type tcp
registration name /icubSim/left_arm/rpc:i ip 10.0.0.26 port 10387 type tcp
registration name /icubSim/left_arm/state:o ip 10.0.0.26 port 10389 type tcp
registration name /icubSim/left_arm/stateExt:o ip 10.0.0.26 port 10390 type tcp
registration name /wholeBodyDynamics/left_arm/FT:i ip 10.0.0.26 port 10161 type tcp
registration name /wholeBodyDynamics/left_arm/Torques:o ip 10.0.0.26 port 10168 type tcp
registration name /wholeBodyDynamics/left_arm/cartesianEndEffectorWrench:o ip 10.0.0.26 port 10195 type tcp
registration name /wholeBodyDynamics/left_arm/client/command:o ip 10.0.0.26 port 10133 type tcp
registration name /wholeBodyDynamics/left_arm/client/rpc:o ip 10.0.0.26 port 10130 type tcp
registration name /wholeBodyDynamics/left_arm/client/stateExt:i ip 10.0.0.26 port 10135 type tcp
registration name /wholeBodyDynamics/left_arm/com:o ip 10.0.0.26 port 10204 type tcp
registration name /wholeBodyDynamics/left_arm/endEffectorWrench:o ip 10.0.0.26 port 10176 type tcp
registration name /wholeBodyDynamics/left_arm/ext_ft_sens:o ip 10.0.0.26 port 10215 type tcp
registration name /wholeBodyDynamics/left_foot/FT:i ip 10.0.0.26 port 10165 type tcp
registration name /wholeBodyDynamics/left_foot/cartesianEndEffectorWrench:o ip 10.0.0.26 port 10199 type tcp
registration name /wholeBodyDynamics/left_foot/endEffectorWrench:o ip 10.0.0.26 port 10183 type tcp
registration name /wholeBodyDynamics/left_leg/FT:i ip 10.0.0.26 port 10163 type tcp
registration name /wholeBodyDynamics/left_leg/Torques:o ip 10.0.0.26 port 10170 type tcp
registration name /wholeBodyDynamics/left_leg/cartesianEndEffectorWrench:o ip 10.0.0.26 port 10197 type tcp
registration name /wholeBodyDynamics/left_leg/client/command:o ip 10.0.0.26 port 10144 type tcp
registration name /wholeBodyDynamics/left_leg/client/rpc:o ip 10.0.0.26 port 10143 type tcp
registration name /wholeBodyDynamics/left_leg/client/stateExt:i ip 10.0.0.26 port 10145 type tcp
registration name /wholeBodyDynamics/left_leg/com:o ip 10.0.0.26 port 10206 type tcp
registration name /wholeBodyDynamics/left_leg/endEffectorWrench:o ip 10.0.0.26 port 10179 type tcp
registration name /wholeBodyDynamics/left_leg/ext_ft_sens:o ip 10.0.0.26 port 10217 type tcp
registration name /wholeBodyDynamics/left_leg/modelWrench:o ip 10.0.0.26 port 10191 type tcp
registration name /wholeBodyDynamics/left_leg/sensorWrench:o ip 10.0.0.26 port 10187 type tcp
registration name /wholeBodyDynamics/left_wrist/Torques:o ip 10.0.0.26 port 10172 type tcp
registration name /wholeBodyDynamics/right_arm/FT:i ip 10.0.0.26 port 10162 type tcp
registration name /wholeBodyDynamics/right_arm/ext_ft_sens:o ip 10.0.0.26 port 10216 type tcp
registration name /wholeBodyDynamics/right_foot/FT:i ip 10.0.0.26 port 10166 type tcp
registration name /wholeBodyDynamics/right_leg/FT:i ip 10.0.0.26 port 10164 type tcp
registration name /wholeBodyDynamics/right_leg/ext_ft_sens:o ip 10.0.0.26 port 10218 type tcp

I also tested if some relevant ports actually publish any output:

icub@icub26:~$ yarp read ... /wholeBodyDynamics/left_arm/FT:i
[INFO] |yarp.os.Port|/tmp/port/1| Port /tmp/port/1 active at tcp://10.0.0.26:10003/
[INFO] |yarp.os.impl.PortCoreInputUnit|/tmp/port/1| Receiving input from /wholeBodyDynamics/left_arm/FT:i to /tmp/port/1 using tcp

icub@icub26:~$ yarp read ... /wholeBodyDynamics/left_arm/Torques:o
[INFO] |yarp.os.Port|/tmp/port/1| Port /tmp/port/1 active at tcp://10.0.0.26:10003/
[INFO] |yarp.os.impl.PortCoreInputUnit|/tmp/port/1| Receiving input from /wholeBodyDynamics/left_arm/Torques:o to /tmp/port/1 using tcp
1 1.56717448240744028354 -0.943850984098839274061 0.151011971455497440164 -0.680200214314772400037 0.0569468937549454587432 -0.0241318946280751102373 0.393043435128048235239
1 1.56125732848709164458 -0.942025830216480564161 0.151010507049799097556 -0.711019595750900679221 0.0568831249057451041051 -0.0238640538652060384128 0.346560790373746152593

icub@icub26:~$ yarp read ... /icub/left_arm/analog:o
[INFO] |yarp.os.Port|/tmp/port/1| Port /tmp/port/1 active at tcp://10.0.0.26:10003/
[INFO] |yarp.os.impl.PortCoreInputUnit|/tmp/port/1| Receiving input from /icub/left_arm/analog:o to /tmp/port/1 using tcp
49.346923828125 -75.0732421875 -34.97314453125 -2.5634765625 -3.753662109375 0.3662109375
49.713134765625 -73.79150390625 -33.69140625 -2.471923828125 -3.753662109375 0.3662109375
50.1708984375 -74.981689453125 -34.881591796875 -2.5634765625 -3.753662109375 0.3662109375

(The first one gave no output, the other two were publishing data.)

pattacini Jun 5, 2023
Maintainer

Thanks for the test @paliasgh 👍🏻

ft was actually a too broad tag to check 😄
/icub/left_arm/analog:o should be the one publishing FT data for the left arm. It seems all ok. Did you check the other limbs too?
/wholeBodyDynamics/left_arm/FT:i is an input port (it ends with ":i"), but you could check whether it receives the correct inputs by doing yarp ping /wholeBodyDynamics/left_arm/FT:i, which prints the connected ports.

You may also consider attaching here a file containing the output of yarprobotinterface, or even better the whole yarplogger session.

paliasgh Jun 5, 2023
Author

Hi @pattacini , thanks for looking into the issue!

All the ports in the list below were publishing something:

icub@icub26:~$ yarp name list | grep -i analog
registration name /iCub/left_hand/analog:o ip 10.0.0.26 port 10376 type tcp
registration name /iCub/left_hand/analog:o/rpc:i ip 10.0.0.26 port 10375 type tcp
registration name /iCub/right_hand/analog:o ip 10.0.0.26 port 10378 type tcp
registration name /iCub/right_hand/analog:o/rpc:i ip 10.0.0.26 port 10377 type tcp
registration name /icub/left_arm/analog:o ip 10.0.0.2 port 10099 type tcp
registration name /icub/left_arm/analog:o/rpc:i ip 10.0.0.2 port 10066 type tcp
registration name /icub/left_foot/analog:o ip 10.0.0.2 port 10104 type tcp
registration name /icub/left_foot/analog:o/rpc:i ip 10.0.0.2 port 10071 type tcp
registration name /icub/left_hand/analog:o ip 10.0.0.2 port 10084 type tcp
registration name /icub/left_hand/analog:o/rpc:i ip 10.0.0.2 port 10051 type tcp
registration name /icub/left_leg/analog:o ip 10.0.0.2 port 10102 type tcp
registration name /icub/left_leg/analog:o/rpc:i ip 10.0.0.2 port 10069 type tcp
registration name /icub/right_arm/analog:o ip 10.0.0.2 port 10100 type tcp
registration name /icub/right_arm/analog:o/rpc:i ip 10.0.0.2 port 10067 type tcp
registration name /icub/right_foot/analog:o ip 10.0.0.2 port 10103 type tcp
registration name /icub/right_foot/analog:o/rpc:i ip 10.0.0.2 port 10070 type tcp
registration name /icub/right_hand/analog:o ip 10.0.0.2 port 10085 type tcp
registration name /icub/right_hand/analog:o/rpc:i ip 10.0.0.2 port 10052 type tcp
registration name /icub/right_leg/analog:o ip 10.0.0.2 port 10101 type tcp
registration name /icub/right_leg/analog:o/rpc:i ip 10.0.0.2 port 10068 type tcp

We got:

icub@icub26:~$ yarp ping /wholeBodyDynamics/left_arm/FT:i
This is "/wholeBodyDynamics/left_arm/FT:i" at "tcp://10.0.0.26:10161/"
There are no outgoing connections
There is an input connection from "/icub/left_arm/analog:o" to "/wholeBodyDynamics/left_arm/FT:i" using tcp
There is an input connection from "<ping>" to "/wholeBodyDynamics/left_arm/FT:i" using text_ack

icub@icub26:~$ yarp ping /wholeBodyDynamics/right_arm/FT:i
This is "/wholeBodyDynamics/right_arm/FT:i" at "tcp://10.0.0.26:10162/"
There are no outgoing connections
There is an input connection from "/icub/right_arm/analog:o" to "/wholeBodyDynamics/right_arm/FT:i" using tcp
There is an input connection from "<ping>" to "/wholeBodyDynamics/right_arm/FT:i" using text_ack

Here is the yarplogger output for running yarprobotinterface first and then wholeDodyDynamics and gravityCompensator. The log contains lots of errors due to this matter and also this issue that still exists (we are going to get back to this one next!).

yarprunlog_05_06_2023_18_25_49.log

paliasgh Jun 5, 2023
Author

There is also something unusual regarding yarprobotinterface. If I stop it through yarpmanager and then, start it again, the arms will not get calibrated:

IMG_3274.mov

In fact, the main joints stay in idle mode (I can switch them back to position control though) and some others give fault as you can see in the screenshot below.

This means, every time we want to restart yarprobotinterface, we have to manually shutdown the motors (press robot's backpack's button) and start them again. It was not like this before. The log file below shows the process of stopping yarprobotinterface and starting it again:

yarprunlog_05_06_2023_18_39_47.log

pattacini Jun 6, 2023
Maintainer

Hi @paliasgh

I deem that we would need at this point to open a proper support ticket at https://github.com/robotology/icub-tech-support/issues detailing the problems clearly.

Would you mind doing it? Copying and pasting should be sufficient.

In this regard, I realized that browsing yarplogger's logs tend to be more difficult than reading up standard console outputs.
Thus, when opening the new ticket, could you also provide the console printouts of, at least, yarprobotinterface, wholeBodyDynamics, and gravityCompensator? That would be extremely useful.

Sorry for bothering you with yet another log session!

paliasgh · 2023-11-30T22:06:26Z

paliasgh
Nov 30, 2023
Author

Hello again @pattacini,

Now that the issue discussed here is resolved, I wanted to bother again regrading this topic. Thanks in advance for you help!

When wholeBodyDynamics and gravityCompensator are running (log session attached), we can now switch joints 0-4 in each of the arms into Torque control mode (and back to Position control mode) with no problem. In this case, checking from the yarpmotorgui, there seems to be a variable torque applied to each joint.

However, moving any of the robot's arm still is not easy. I mean, it is super hard to move the arm of the robot as seen in the video you mentioned because some of the joints (mostly Joint 0) feel very heavy. In fact, setting the joints in Idle mode makes them easier to move compared to Torque control mode! I can feel something is happening there (maybe due to gravityCompensator working), but that does not compensate gravity or make the arm compliant at all. This is a video of our robot today:

IMG_4635.MOV

Just to remind you, what we are looking for is to simply let the robot go through some actions (receive kinesthetic demonstrations) pretty much like the video mentioned above.

yarprunlog_30_11_2023_17_04_00.log

21 replies

paliasgh Jul 24, 2024
Author

Hi @martinaxgloria, I hope everything is going great with you. I am so sorry for bothering you again, is there any news on this matter? Thanks again!

martinaxgloria Aug 5, 2024

Hi @paliasgh,

I'm sorry for not getting back to you sooner. In the past months, we worked on this issue trying to debug and then solve the problems we encountered on the FT mounted on the arms of our iCub.

After:

We found a workaround to properly visualize the right measurements on RVIZ, but unfortunately iDynTree, which is used by wbd, is not able to parse this parameter, so we didn't solve the issue yet.

we tried to find another solution, and we came to this one:

iCubV2_*: Fix measure of arm ft sensors icub-models-generator#265

For this reason, we kidly suggest to re-do the tests you did in the past but with icub-models on the last devel and iDynTree on the last master, using the same configuration files you used in the previous tests.

Looking forward to receiving your feedback,
Martina

paliasgh Aug 14, 2024
Author

Hello @martinaxgloria , thanks for your effort! As you said, I updated iDynTree to the last master and icub-models to the last devel (I saw the URDF file of our robot was updated). I ran the previous tests and it seems like the issue is almost resolved! As before, I ran yarprobotinterface --config icub_wbd.xml and then switched the first 5 joints of each arm to Torque control mode in yarpmotorgui. The left arm can be moved quite easily now. In fact, the shoulder joint is very light and handy, forearm is a bit harder to move, but is still okay. The only issue is that the right arm is not as easy to move as the left arm. The shoulder joint seems to not providing any (or enough) torque to compensate the gravity. Can there be any issue in the URDF regarding the left arm? Maybe it is not updated? Thanks again for all your help and support!

traversaro Aug 19, 2024
Maintainer

Hello @paliasgh @martinaxgloria !

Just a word of warning on a key difference between the old gravityCompensator module and the gravity compensator functionality included in the wholebodynamics-device.

In the gravityCompensator module, if the joint was in VOCAB_CM_TORQUE control mode, if the gravityCompensator was running the gravity compensation torque was send as . If you wanted to actually do torque control in this case (i.e. if you want to set a desired torque different from the gravity torque), you need to close the gravityCompensator, otherwise the setRefTorques call of gravityCompensator would conflict with the user's module calls to setRefTorques. So, a way of doing pure gravity compensation with the gravityCompensation module is to set the joint in torque control mode, and then activate the gravityCompensator module. For the related code, see https://github.com/robotology/icub-main/blob/2c799b7c27e473731c50b166944e4e22b5f100da/src/modules/gravityCompensator/gravityThread.cpp#L615-L617 .

In the gravity compensator functionality included in the wholebodynamics-device, the functionality is always running as part of the wholebodydynamics device that is running as part of the yarprobotinterface, so if the control mode is in VOCAB_CM_TORQUE mode, no gravity compensation is sent to the robot, as otherwise the setRefTorques of the gravity compensator would conflict with the one of any external module doing torque control. See https://github.com/robotology/whole-body-estimators/blob/80d45bf79c5e4be3c7f83e0bec8f2cdfef48caab/devices/wholeBodyDynamics/README.md?plain=1#L53 for the docs on this, and https://github.com/robotology/whole-body-estimators/blob/80d45bf79c5e4be3c7f83e0bec8f2cdfef48caab/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp#L2661-L2688 for the related code.

If you are just interesting in doing pure gravity compensation with any position correction, you can do that by:

Either setting the joint in compliant interaction mode, but setting the stiffness and the damping of the joint to zero, via setImpedance(jntIdx, 0.0, 0.0) https://www.yarp.it/latest//classyarp_1_1dev_1_1IImpedanceControl.html#a200e94123b685cc49cfc07afa34e4084
Or by writing your own gravity compensation module, that control the joint in torque mode and you send the gravity compensation you want (see for example https://github.com/vvv-school/tutorial_impedance-control/).

Deeply sorry if this difference was not clear before.

martinaxgloria Aug 19, 2024

Thank you very much @traversaro for the clarification! 🙏

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Robotology

Compliance to enable kinesthetic teaching #630

{{title}}

Replies: 3 comments 27 replies

{{title}}

{{editor}}'s edit

{{editor}}'s edit

{{title}}

{{editor}}'s edit

{{editor}}'s edit

{{title}}

{{title}}

{{editor}}'s edit

{{editor}}'s edit

{{title}}

{{title}}

{{editor}}'s edit

{{editor}}'s edit

{{title}}

{{title}}

{{title}}

{{title}}

{{editor}}'s edit

{{editor}}'s edit

{{title}}

{{title}}

{{editor}}'s edit

{{editor}}'s edit

{{title}}

Select a reply

Robotology

Compliance to enable kinesthetic teaching #630

paliasgh May 24, 2023

Replies: 3 comments · 27 replies

pattacini May 27, 2023 Maintainer

paliasgh May 29, 2023 Author

paliasgh Jun 3, 2023 Author

pattacini Jun 5, 2023 Maintainer

paliasgh Jun 5, 2023 Author

paliasgh Jun 5, 2023 Author

pattacini Jun 6, 2023 Maintainer

paliasgh Nov 30, 2023 Author

paliasgh Jul 24, 2024 Author

martinaxgloria Aug 5, 2024

paliasgh Aug 14, 2024 Author

traversaro Aug 19, 2024 Maintainer

martinaxgloria Aug 19, 2024

paliasgh
May 24, 2023

Replies: 3 comments 27 replies

pattacini
May 27, 2023
Maintainer

paliasgh
May 29, 2023
Author

paliasgh Jun 3, 2023
Author

pattacini Jun 5, 2023
Maintainer

paliasgh Jun 5, 2023
Author

paliasgh Jun 5, 2023
Author

pattacini Jun 6, 2023
Maintainer

paliasgh
Nov 30, 2023
Author

paliasgh Jul 24, 2024
Author

paliasgh Aug 14, 2024
Author

traversaro Aug 19, 2024
Maintainer