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.