Velocity seems to be set to 0 after detaching

[sven-hoek]

The plugin seems to work pretty well but I was checking what happened, when the gripper opens while in motion. It seems that the gripped object's velocity is reset and falls straight down by gravity.
For example in situations, where the simulated robot is supposed to throw an object, it would be important, that the gripped object keeps its velocity.

[JenniferBuehler]

It's quite possible I haven't considered this. Detachment is handled here - ignore the USE_MODEL_ATTACH flag, I should remove that some time, it was for testing. I'm not explicitly resetting velocity, but it could be that detaching a joint from another joint with Joint::Detach() causes this. The fixedJoint is a fake joint which is added to connect the object to the gripper while the object is attached. It would of course be possible to store the velocity before detachment and restore it after, as a simple solution (if detaching is the cause of the issue at all).

I don't have time this week to check up on this but will try as soon as I can, if you haven't solved it until then.

[sven-hoek]

Okay, so I've had no experience with Gazebo plugins before and naively tried to set the object link's velocity to its previous, without seeing a change in behavior:

physics::LinkPtr objLink = boost::dynamic_pointer_cast<physics::Link>(obj->GetParent());
ignition::math::Vector3d linVel = objLink->WorldLinearVel();
ignition::math::Vector3d angVel = objLink->WorldAngularVel();

this->fixedJoint->Detach();

objLink->SetLinearVel(linVel);
objLink->SetAngularVel(angVel);

Then, when checking the velocities of the link before and after Detach(), they stay the same, even when I don't set them myself.
Did I set the velocity of the wrong object or have any other misconceptions? Any other suggestions where the velocity could be set/reset? I also tried working with the parent of my objLink, which was the Model with name Box (corresponding to the box I'm gripping).

Note, that I am using Gazebo9 with the plugin modifications from the open pull request in this repo, so the method names should be GetWorldLinearVel, GetWorldAngularVel for Gazebo versions below 8.

[JenniferBuehler]

I'm not sure about obj->getParent(), what is obj is in your case, is it the
same object as here? Maybe try obj->getLink()?
What is the velocity of the object before detachment? Does it have any?

In any case, you need to set the velocity to the link of your object (the root link of all the object parts). If it doesn't have an easy to recognize name in your SDF/URDF, then maybe give it one for testing, and make sure this is the link you are setting the velocity to.
See also this tutorial (section "Set Link Velocity Instantaneously").

Another thing to consider is the object properties, like weight. Once the object is detached, it will be subject to gravity again. So if it's very heavy and the speed isn't that high, it would fall to the ground pretty quickly, maybe the linear velocity won't be noticed as much then, as the gravity force pulling it causes it to change too much?

[JenniferBuehler]

BTW, if you are using the open pull request, I guess you reverted the faulty commits at the end, which were done automatically by verifying the suggestions I wrote in the discussion (which were meant to be like pseudo code only, but then they were commited). I guess so, otherwise it probably wouldn't be compiling...
I have so little time at hand that I still wasn't able to fix the PR myself. I will try to do this on the weekend, it's long overdue ;)

[sven-hoek]

is it the same object as here?

Yes, it's the same object/scope and yes, it does have velocity before. My object is just a box, so there is only one link, which is the parent of obj. Using GetLink() unfortunately didn't change anything. I checked, if it's really the right link.
I lowered the execution speed a lot by lowering the step size/limiting the real time update rate and also reduced the gravity a lot. At the point of releasing the object, it just stops abruptly.

I'm using an earlier commit of the PR, where everything version-related was enclosed by #if-macros. Thanks for your support and for your work! Hope, I can be of help by trying to resolve this. I'll check if the velocity restoration works when done in the update method of the plugin.

[sven-hoek]

Ok, so unfortunately it also doesn't seem to work when restoring the velocity after this line.

[JenniferBuehler]

Hi,
ok, that sounds like something is off. With your object, if you set its velocity like that before you do any attachment, does it react to it at all?
I'll have to look into the Gazebo set velocity code again to refresh my memory on how it works. First, can you check if the object's velocity can be set before you use the attachment with the plugin?
Also, you can try to set force and/or acceleration and see if that makes a difference, just to see if everything works fine with the object in general.

[sven-hoek]

I tried setting an arbitrary velocity at the end of GazeboGraspFix::Load() and it works without any problems. I then added an arbitrary force or velocity after the detachment process inside GazeboGraspFix::OnUpdate() and both also worked. This means at least, that the velocity won't be reset after this step.
So, even though the object has a velocity right before the detachment, they are too small to be seen or realistic. For example, I mostly got velocities like (0.002224 -0.000124 0.003645) but sometimes something like (-0.01131 0.124148 0.007077), which is still small but at least in y-direction not negligible small. Maybe just noise, though.
If I find the time, I will see, whether I find a better way to get the velocity. It seems weird, though, that the velocity is that low, which would make some sense if it were in relation to the gripper, whereas the name WorldLinearVel() suggests, that it's in relation to the world frame.

[JenniferBuehler]

Hmmm... maybe the velocity of links which are attached to a moving joint are not updated, though I would have thought so. Sorry I'm being so slow on testing this, I had too much work to get done, and tomorrow I'm going away for a week. I still have this on my radar, and eventually I will get to test this. Meanwhile, let me know if you find out more.

My first thought would be to check the links of the gripper while they are moving obviously fast, and see if they have a reasonable velocity. Just to confirm whether links attached to moving joints have a realistic world velocity.
If that's the case, then maybe try to assign the velocity which the gripper had at the moment of detachment. Though my guess would be that if the cube doesn't have a significant velocity while it is still attached, the gripping link probably won't be much different. But it would be worth a check.

Next step would be to look up in the Gazebo code how exactly the velocity of Links is updated. I can't remember exactly how it worked, but a look at the code will tell the truth ;)

One other random try would also be to try a different physics engine (e.g. gazebo -e bullet) to see if that's the same there.

[sven-hoek]

I appreciate every bit of work and help that you put into this and right now this is also not my main work, so I'm also kinda slow on this. But I just plotted the velocity of the link of the object and of the gripper (inside Gazebo).
2 things are really weird:

• The velocity of the gripper link is way smaller than the one of the object and this cannot be because of some lever action
• When moving the arm back and forth with the object attached, the velocity of the object is always negative and the velocity of the gripper is always positive, no matter which direction the robot moves. Like the absolute value was taken (negated on the object's velocity, though)

[sven-hoek]

So for another simple test I printed out the velocities retrieved by WorldLinearVel() of each link of the robot when GazeboGraspFix::OnUpdate() ist called and even when the robot is moving with considerable speed, they stay close to zero or at least not larger than when standing still. Maybe the world variable which is set by model->GetWorld() inside GazeboGraspFix::Load() is somehow not able to access the velocities?
Do you maybe have a hint where to look in the Gazebo code, or another possibility to get the link velocities?

[JenniferBuehler]

Hi, so finally I got a spare hour to check this ;)

I've just used my Jaco test setup to test this, and I printed the velocities. For me they actually make sense - only while the object is not moving, there is still a very small (insignificant) velocity. However I've so far only tested it on my current work computer with Indigo with the default install of Gazebo 2, which is ancient. I will test it with a newer Gazebo version and report back.

Meanwhile, can you check if the velocities still remain insignificant for you, if possible with the Jaco arm. I've updated the code and you can use the same test prints in this line and here by just setting it to #if 1 (my probably not so common way to make it easier to comment/uncomment blocks with only one type ;D). I will remove those test prints once we've fixed this issue here.

If you have also installed+compiled my grasp-execution-pkgs, then you can test easily on the Jaco like this:

roslaunch grasp_execution_jaco_tutorial jaco_on_table_gazebo_objects_controlled.launch load_grasp_fix:=true

and in another terminal, put all other commands to one line to save you some headaches ;) (you may need to adjust the sleep values if this doesn't go smooth for you):

roslaunch grasp_execution_jaco_tutorial spawn_and_reach_test_cube.launch && sleep 1.5 && rosrun grasp_execution_jaco_tutorial grasp_cube_test && sleep 1 && rosrun grasp_execution_jaco_tutorial set_arm_to_cube_test --home --no-fingers

You can release the cube with this:

rosrun grasp_execution_jaco_tutorial grasp_cube_test --ungrasp

although if the arm is still moving, it will stop to release the cube first, so you can't really throw it around with this test setup. However the velocities reported are exactly the same as before the detachment (even if small), so I think I wouldn't have the issue if the arm was moving.

[JenniferBuehler]

I just remembered my update will conflict with your settings because you're using the PR for melodic. I'm just processing that PR to finalise it actually, then you'll be able to use the new code on master directly.

[JenniferBuehler]

I have updated all my packages to support Melodic. You can now try again using all my master branches.

[JenniferBuehler]

I've just tested this again. The velocity values while the object is moving actually make sense (using the test setup with the Jaco mentioned above):

[Msg] Velocity for link link (collision name cube1::link::collision): 0.000127 0.000334 0.000235, absolute val 0.000427883
[Msg] Velocity for link link (collision name cube1::link::collision): 8.7e-05 0.000283 0.000201, absolute val 0.000358163
[Msg] Velocity for link link (collision name cube1::link::collision): 8e-05 -0.000419 -0.000735, absolute val 0.000849296
[Msg] Velocity for link link (collision name cube1::link::collision): 0.000143 -0.000386 -0.000534, absolute val 0.000674358
[Msg] Velocity for link link (collision name cube1::link::collision): 0.257508 0.367559 0.279731, absolute val 0.528828
[Msg] Velocity for link link (collision name cube1::link::collision): 0.289677 0.319544 0.284859, absolute val 0.51688
[Msg] Velocity for link link (collision name cube1::link::collision): 0.314252 0.273718 0.287556, absolute val 0.506325
[Msg] Velocity for link link (collision name cube1::link::collision): 0.329963 0.231457 0.289894, absolute val 0.496474
[Msg] Velocity for link link (collision name cube1::link::collision): 0.323467 0.037872 0.289916, absolute val 0.436023
[Msg] Velocity for link link (collision name cube1::link::collision): 0.313703 -0.123585 0.290164, absolute val 0.444835
[Msg] Velocity for link link (collision name cube1::link::collision): 0.315365 -0.171255 0.29009, absolute val 0.46145
[Msg] Velocity for link link (collision name cube1::link::collision): 0.319376 -0.182716 0.288207, absolute val 0.467386
[Msg] Velocity for link link (collision name cube1::link::collision): 0.324368 -0.188044 0.286177, absolute val 0.47167
[Msg] Velocity for link link (collision name cube1::link::collision): 0.329308 -0.189511 0.282151, absolute val 0.473252
[Msg] Velocity for link link (collision name cube1::link::collision): 0.327746 -0.18948 0.274998, absolute val 0.467915
[Msg] Velocity for link link (collision name cube1::link::collision): 0.329393 -0.188287 0.267562, absolute val 0.464264
[Msg] Velocity for link link (collision name cube1::link::collision): 0.331628 -0.186817 0.260241, absolute val 0.461089
[Msg] Velocity for link link (collision name cube1::link::collision): 0.242179 -0.202395 0.220052, absolute val 0.384756
[Msg] Velocity for link link (collision name cube1::link::collision): 0.214991 -0.204539 0.201868, absolute val 0.358899
[Msg] Velocity for link link (collision name cube1::link::collision): 0.208103 -0.201454 0.190138, absolute val 0.346472
[Msg] Velocity for link link (collision name cube1::link::collision): 0.206403 -0.196861 0.180527, absolute val 0.33756
[Msg] Velocity for link link (collision name cube1::link::collision): 0.20569 -0.191597 0.17143, absolute val 0.329251

And just when detaching, it seems to go down a little bit when the object isn't touching the gripper any more, but still BEFORE it is detached (as said, unfortunately detaching while the arm is moving doesn't work with my simple test setup, so the velocity is already low before detaching).
However, what's most important, is that pre/post detachment doesn't affect the velocity. It's exactly the same value, so I'm guessing that would be the same if the velocity was higher...

[Msg] Velocity for link link (collision name cube1::link::collision): 0.000155 -2.5e-05 -0.00053, absolute val 0.000552371
[Msg] Velocity for link link (collision name cube1::link::collision): -0.000159 -5.9e-05 0.000483, absolute val 0.000511539
[Msg] Velocity for link link (collision name cube1::link::collision): 0.000165 -4.4e-05 -0.000527, absolute val 0.000553946
[Msg] Velocity for link link (collision name cube1::link::collision): -0.000162 5e-05 0.000531, absolute val 0.000557578
[Msg] Velocity for link link (collision name cube1::link::collision): 0.000849 0.000197 -0.003541, absolute val 0.00364716
[Msg] GazeboGraspFix: Detaching cube1::link::collision from gripper JacoArm1.
[Msg] PRE-DETACH Velocity for link link (collision name cube1::link::collision): 1.7e-05 -6e-06 8e-06, absolute val 1.92592e-05
[Msg] POST-DETACH Velocity for link link (collision name cube1::link::collision): 1.7e-05 -6e-06 8e-06, absolute val 1.92592e-05

and right afterwards it starts to fall down:

[Msg] Velocity for link link (collision name cube1::link::collision): 0.01772 -0.008722 -0.113658, absolute val 0.115361
[Msg] Velocity for link link (collision name cube1::link::collision): -0.033747 -0.115394 -0.134026, absolute val 0.180049
[Msg] Velocity for link link (collision name cube1::link::collision): -0.03044 -0.101856 -1.0324, absolute val 1.03786

Can you confirm this is similar on your setup, using the new versions of all my packages? (all on master branches)

[sven-hoek]

Ok, I created a new workspace with your jaco packages and got similar, reasonable results:

[Msg] Velocity for link link (collision name cube1::link::collision): -4e-06 -1e-06 -0, absolute val 3.93345e-06
[Msg] Velocity for link link (collision name cube1::link::collision): 1.1e-05 -5e-06 0.000106, absolute val 0.00010672
[Msg] Velocity for link link (collision name cube1::link::collision): 0.264903 0.357312 0.281031, absolute val 0.52614
[Msg] Velocity for link link (collision name cube1::link::collision): 0.33405 0.20541 0.289578, absolute val 0.487481
[Msg] Velocity for link link (collision name cube1::link::collision): 0.322413 -0.186705 0.287224, absolute val 0.470432
[Msg] Velocity for link link (collision name cube1::link::collision): 0.32949 -0.188507 0.267178, absolute val 0.464201
[Msg] Velocity for link link (collision name cube1::link::collision): 0.209733 -0.202451 0.193545, absolute val 0.349906

After that I replaced the new version of the grasp plugin in my other workspace. While compiling I got the following warning, which I got rid of by pointing cmake to the right places. They also occured when using the Jaco packages but there I got no error while starting Gazebo/loading the plugin (see below). Just mentioning it in case it might have to do with the issue.

CMake Warning at /opt/ros/kinetic/share/catkin/cmake/catkin_package.cmake:166 (message):
  catkin_package() DEPENDS on 'gazebo' but neither 'gazebo_INCLUDE_DIRS' nor
  'gazebo_LIBRARIES' is defined.
Call Stack (most recent call first):
  /opt/ros/kinetic/share/catkin/cmake/catkin_package.cmake:102 (_catkin_package)
  CMakeLists.txt:26 (catkin_package)

So it compiles, but when I try to start Gazebo with our Franka robot in that workspace, gazebo crashes and I get the following info:

gzserver: symbol lookup error: /home/franka2/marcus_ws/devel/lib/libgazebo_grasp_fix.so: undefined symbol: _ZN6gazebo10GetPhysicsERKN5boost10shared_ptrINS_7physics5WorldEEE

Which seems to coincide with line 259 in GazeboGraspFix.cpp:

physics::PhysicsEnginePtr physics = GetPhysics(this->world);

The error occurs with or without setting the cmake variables before (re)compilation. Also after cleaning the workspace or adding our packages to the new jaco workspace, it still remained. But weirdly, it doesn't occur when starting gazebo with the jaco packages.
Any idea why this symbol lookup error could occur despite using the same Gazebo and plugin version?

[JenniferBuehler]

Hi,
Yes I should get rid of the warnings at some point, however they relate to the catkin_package() command and that shouldn't be related to the issue you're having.
It looks like the grasp fix plugin may have been compiled with the wrong version, though it's strange it happens only with that line then.
Have you tried to completely re-compile all packages including your Franka package, by removing the build and devel directories in your catkin workspace and compile again?
Let me know if the issue still comes up then.

[sven-hoek]

Yes, I did. Unfortunately it persists, I even tried it on another machine (I was using the older version of the plugin there before, too, though). What's really weird is that the error doesn't occur with the Jaco, even in the same workspace. After all, it should do the same call...

[JenniferBuehler]

Hm that's weird. Your Franka repositories aren't public by any chance, so I could try to reproduce it?

So it doesn't happen if you comment out GetPhysics(this->world) (so just leave it to be a nullptr even if at runtime that would segfault), it then compiles? No issues with the other new helper functions in gazebo_version_helpers?

[sven-hoek]

Yeah, they are: https://github.com/sven-hoek/franka_ros
Just run roslaunch franka_gazebo franka_gazebo. The plugin is included on the bottom of franka_description/robots/hand.xacro.

I compiled it again, leaving physics a nullptr. Somehow exactly the same error occurs as before when I launch Gazebo. I double checked whether I am editing/compiling the right file.

[JenniferBuehler]

That's then because it is having trouble with the gazebo::physics::Physics type itself. I'm just cloning the repo and will try as well.
Ah you're talking about the official Franka repo, cool! I love the Franka :)

[JenniferBuehler]

Ah silly me, I am currently finishing other work on Trusty. Here I've had trouble building libfranka so I didn't build all the franka_ros packages. franka_gazebo was OK but I can't run the launch file because of the dependencies I removed. Anyway the whole issue probably doesn't come up on Trusty anyway, so I'd rather test on Bionic once I reboot into it again.
Unfortunately I'll have to wait until I finish the work on my Trusty boot before I reboot.

Meanwhile, general thoughts. If you still keep having the error

gzserver: symbol lookup error: /home/franka2/marcus_ws/devel/lib/libgazebo_grasp_fix.so: undefined symbol: _ZN6gazebo10GetPhysicsERKN5boost10shared_ptrINS_7physics5WorldEEE

after you remove all the calls to GetPhysics() then something is truly weird. Maybe take out the call in GazeboGraspGripper too, to make sure that GetPhysics is never called. To be 100%, just comment the whole GetPhysics functions from .cpp/.h files in gazebo_version_helpers. If you still get the same error, then it's definitely something with your build.

gzserver throws the lookup error at runtime, when it loads the grasp fix plugin. That could mean that either libgazebo_grasp_fix.so isn't linked up properly with libgazebo_version_helpers.so (but I dont' think so otherwise you'd have the problem with the Jaco too), or libgazebo_version_helpers.so are not in the LD_LIBRARY_PATH, or something like that (and also then, it's weird that you don't get that with Jaco).

If you do sudo updatedb && locate libgazebo_grasp_fix.so, does it come up with only one library of libgazebo_grasp_fix.so?

[JenniferBuehler]

Also, what's the output of

nm ` locate libgazebo_version_helpers.so` | grep GetPhysics

[JenniferBuehler]

Hmmmmm I think I may just have found something which I noticed with

ldd ` locate libgazebo_grasp_fix.so` | grep gazebo

which doesn't include gazebo_version_helpers ! Though that still wouldn't explain why it works with the Jaco, how puzzling...

It was a minor change in the gazebo_grasp_plugin CMakeLists.txt which changed this.

Can you please pull gazebo-pkgs again and try again?

[sven-hoek]

Ok, now it runs again, thanks. With your diagnostic print I realized 2 unusual things:

1. The velocity drops to a very small value right before detaching:

[Msg] Velocity for link box_link1 (collision name box::box_link1::collision): -0.18197 0.135352 0.000549, absolute val 0.226789
[Msg] Velocity for link box_link1 (collision name box::box_link1::collision): -0.187825 0.039643 0.000692, absolute val 0.191964
[Msg] Velocity for link box_link1 (collision name box::box_link1::collision): 0.001451 0.002234 0.000948, absolute val 0.00282756
[Msg] GazeboGraspFix: Detaching box::box_link1::collision from gripper gripper.

2. Don't know if directly related to the issue, but currently I just pick a box up from the ground and let it go while moving to a more upright position. The absolute linvel as from your output is close to 0 when in the lower (pick up) position, goes up to ~0.3-0.35, but stays at ~0.25 when in the upright position. I realized that there is some jittering visible when zooming in.
   Unfortunately, Franka Emika didn't provide a proper model for simulation, therefore I tried to setup one myself (wanted to provide it for the Franka Community, too, but haven't posted it in the forum yet). So the inertia values are rather guesstimated and I wonder now, if the links of the collision model might be too close together to end up in a jittering control behaviour. I'm still getting to know ROS/Gazebo, so I don't know if this might be part of this problem.

[JenniferBuehler]

Yes I've noticed the same steep drop in velocity just before detaching (as posted above) with the Jaco, not sure why this happens. It's still when the object is attached, but the gripper would have opened and would not be touching the cube any more.

It may be a while before I get around to reboot into Bionic and test again, so hopefully I can help you with some suggestions and guess work in the meantime ;)

First I would try to reduce the release tolerance (parameter release_tolerance in xacro) and see if this still happens then. Can you try that?

Also, can you set this and this from 0 to 1 and post the output of that as well? Thanks :)

[sven-hoek]

It persists when I reduce the release tolerance:(

[Msg] Velocity for link box_link1 (collision name box::box_link1::collision): -0.138627 -0.174153 0.001265, absolute val 0.222594
[Msg] Velocity for link box_link1 (collision name box::box_link1::collision): -0.138036 -0.172628 0.001995, absolute val 0.221039
[Msg] Velocity for link box_link1 (collision name box::box_link1::collision): -0.040687 -0.013907 -0.000555, absolute val 0.0430015
[Msg] GazeboGraspFix: Detaching box::box_link1::collision from gripper gripper.
[Msg] PRE-DETACH Velocity for link box_link1 (collision name box::box_link1::collision): -0.040687 -0.013907 -0.000555, absolute val 0.0430015
[Msg] POST-DETACH Velocity for link box_link1 (collision name box::box_link1::collision): -0.040687 -0.013907 -0.000555, absolute val 0.0430015

[JenniferBuehler]

Hmmmm ok that's something that needs closer looking into, seems very strange. If you increase the release tolerance (even if it's so high it will never be released) and fully open the hand, does this steep drop still happen at some point?

I'll have to look into this more, unfortunately I will probably not have time for that this week. Meanwhile, I hope you can work with it despite the velocity reduction? Or maybe you end up finding the reason for that weird behaviour, that would be great too :)

[sven-hoek]

The drop seems to be smaller but it seems to persist:

[Msg] Velocity for link box_link1 (collision name box::box_link1::collision): -0.179151 0.134644 -0.006592, absolute val 0.224204
[Msg] Velocity for link box_link1 (collision name box::box_link1::collision): -0.058385 0.00709 -0.024909, absolute val 0.0638714

It happens right when opening the gripper (the messages stop afterwards but the object is not being detached).

Like I said, I'm also mostly working on a different project but trying to make a little progress on this issue from time to time. I'm trying to get this running for other users who want to do throwing experiments using reinforcement learning.
I checked, if the jittering was caused by the constant collisions of the adjacent links by disabling the collision model for the robot links except the gripper, but it didn't seem to change much. If I have the time, I will check it more deeply and think of other problems that might cause issues.

[JenniferBuehler]

Thanks for testing this!
It makes sense when this happens as the gripper open, as the conditions change. Though I would have thought the properties are not affected with self-collisions, and technically the object becomes part of the robot, but maybe it's not considered the same for self-collisions.
This will require some more detailed looking into. It looks like in 1-2 weeks I'll have some time freed up, so then I can look into this more.