Looking at the commit which added this line, it looks like the line might be needed for certain use cases (something related to initial joint positions for example). I think a solution which would be good for everyone can be either to have two different functions or to add a flag to this function, so we support both the behavior which zeroes the twist, and the behavior which doesn't. What do you think?
How to know when this line could be useful and when not ?
For more context, you can take a look at pull request #1096, which introduced this line. It looks like the line was introduced at the same time as the function. i.e. the function has never existed without the line.
Yes you are right the function MoveFrame in Link.cc comes from this pull request.
But it feels strange to me that the speed of the link is set to zero in the MoveFrame function. Why setting the twist to zero if you want to move the link frame ?
You think it could be useful for the initialization ?
For me this line jam the simulator when calling SetPosition on a joint when there is multiple joint in cascade like I reproduce here
I see what you're saying, but I can see use cases for both zeroing the twist and not zeroing it, especially since we're talking about the world frame. Say you have a link moving on the X axis. Then you use this function to rotate it by 90 degrees. You might not want it to keep on moving in the direction it was before.
In any case, since it looks like this has been the behavior of the function since it was created, I'm afraid that simply removing this line has the potential for breaking user code. I'll stick to my original suggestion of either adding a flag to this function or creating a new function with the desired behavior, but we should hear the opinion of other reviewers too.
I think the behavior of zeroing linear and angular velocity doesn't match the description of MoveFrame. I'm not sure if I would expect the new velocity to be the same relative to the destination frame as it was the source frame, or the velocity in world frame to be unchanged.
I agree about making a new method. How about the new method (MoveRelativeToFrame?) changes the pose and leaves the velocity in world frame unchanged?
Since setting the velocity to zero is trivial, I think MoveFrame could be deprecated in favor of calling the new method and then setting the link velocity to zero.
I am not an expert in simulation so I trust you judgement to keep both behavior (with and without zeroing the twist) but I do not see how to handle it.
In fact, the MoveFrame function is only called in Joint::SetPositionMaximal in physics/Joint.cc
When do you want to activate or deactivate zeroing the twist ?
Because I see a single use case : calling Joint::SetPosition
@sloretz, @chapulina, What do you think of maintaining the joint velocities rather than the velocities in either the source or world frame? This is what DART appears to do. Or alternatively, what about zeroing the velocities but relative to the parent link instead of the world?
SetPosition produces different behaviors for DART and ODE. I created tests on this branch to demonstrate. They expect the joint velocity to be unaffected by SetPosition, so they fail for ODE and pass for DART. I also made the change proposed in this PR but by modifying SetPositionMaximal so the behavior of MoveFrame is unaffected. The tests pass for ODE with the change.
Is it possible that this function is never used by gazebo it self ?
And only used with ROS when using the PositionJointInterface in urdf ?
This could explain why we saw this bug now even if the bug is from a commit in 2014.
Since ROS users use indigo with gazebo 2 and just start to migrate to ROS kinetic with gazebo 7.
It looks like the function is used by ODEJoint::SetPosition and BulletJoint::SetPosition , which are public functions. So any downstream code could be using them, just like gazebo_ros does.
Yeah it looks like the Link::MoveFrame function has been introduced after Gazebo 2.2, which goes with Indigo.
I'm not sure what's the best path forward here. Leaving it like this makes the function unusable in some cases, but changing it might break user code. I haven't used the SetPosition function enough to have a strong opinion about what it should do (in fact, I'd say using SetPosition to control a robot in simulation is not good practice, since it overrides the physics engine). @scpeters , thoughts?
Currently the JointPosition interface in ROS is using SetPosition (in default_robot_hw_sim.cpp) .It is problematic since PositionInterface freeze with gazebo 7. In our case, we want to use this to control a steering angle. Perhaps @chapulina have a good point, if the JointPositionInterface overwrite the simulation it will not respect the physics.( The joint position will directly match the joint command instead of reaching the position based on joint parameter.)
I think I found the way to dodge the problem. I am also doing the 2 JointPositionInterface for steering. Using directly the Position Interface will result in the << SetPosition>> of the joint which will cause an overwrite of the physics since we are directly setting the state. Instead, in ros_gazebo_pkg, if you define
In the param server, the default_robot_hw_sim will use a POSITION_PID instead and it will do a close loop with the physics.
Sure it doesn't solve the freezing problem, but I think the JointPositionInterface must be used that way, but I can see cases ( maybe direct linkage mechanism) that you want to impose joint position.
I tried to use the POSITION_PID but for me it is unstable.
I have tested different PID and joint damping/friction but I did not find a working point...
Is this issue fixed in the last version of gazebo 7 (7.7)?
This pull request hasn't been merged into any Gazebo versions yet.
The fix works for me, and makes sense. I'm using Gazebo compiled from source now.
Would it be possible to discuss a resolution to this PR? Based on the comments in this thread, I'll throw my 2 cents into the mix:
The simulations I construct using Gazebo mostly require sensor streams and visualizations to be accurate, but do not require the physics to be so. Being able to use JointPositionController is extremely valuable, even if it is not physically correct.
My attempts to use effort control via the POSITION_PID method are failing for the yaw joint of my arm. In a different model, I got the arm to behave well, but only by having very bizzare PID values which are probably not realistic either.
I haven't quite figured out the argument for why the lines being removed are there in the first place. I understand the necessity of not changing legacy code. Maybe target the fix for an upcoming major release?
The zeroing of world velocity makes sense if you consider the use case of some kind of "reinitialization" of a link to a known state. However, in the use case of wanting to rotate a link that is moving, as in the ros_controllers example, it causes the link to freeze or fall slowly because its velocity is constantly being zeroed. I'll throw a vote in for the zeroVelocity flag that defaults to true, there is value in both use cases.
There would also have to be a preserveVelocity flag in Joint::SetPosition for the flag solution to be effective, since thats the primary place Link::MoveFrame is being called from. It's possible to implement a solution on the use case side by caching the velocity of every frame before the positions are updated and then reapplying them afterwards, but that seems like a much less optimal solution.
This is certainly a thorny issue, and both views on the matter are understandable.
On one hand, changing one component of an object's state shouldn't clobber another component of the state unless it's completely necessary in order to preserve sanity, so zeroing out the twist seems like a superfluous behavior in this function.
On the other hand, it could be argued that having the velocity component of an object's state persist between physics-defying teleportations of an object is not particularly sane (unless we're supporting use cases for Aperture Science portal devices).
Trying to mix together physics-obeying concepts of momentum conservation with physics-defying concepts of discrete teleportation seems like begging for trouble. Since there is no obviously correct behavior in this circumstance, I agree with @zachanderson that the nicest thing to do would be to allow the user to specify the behavior with a flag, and have the default match the legacy behavior. That would mean adding a boolean flag to MoveFrame and SetPosition.
The catch with that solution is that SetPosition is a virtual function which gets implemented by every single physics engine, potentially multiple times (up to once for each joint type in each physics engine), so this fix would not be as trivial to implement as it seems on the surface. Worse yet, right now the exact behavior of SetPosition varies between physics engines, where some will zero out the velocity while others do not. If we introduce a flag which tells the physics engine to zero out the world velocity of the joint's child link, this would be extremely problematic for physics engines which use generalized coordinates, like DART and Simbody. That would require them to perform an inverse differential kinematics computation to find a set of parent joint velocities that would achieve zero world velocity for the child link. In many cases, such a solution might not even exist mathematically.
Therefore, while it's not the most user-friendly option, I think the most sensible option is to make the user responsible for saving the world velocity before calling the function and then reverting to that velocity afterwards. We should also add documentation to MoveFrame which indicates that it will reset the velocity, and add documentation to SetPosition to indicate that its behavior varies between physics engines (specifically that ODE and Bullet resets it while DART and Simbody don't).
Then, in the ROS JointPosition interface, we can tweak its behavior so that it saves the velocity before calling SetPosition and then reverts to that velocity after calling the function. I believe that should opaquely solve the problem for the users who have reported being affected in this thread.
As it stands, it seems the behavior of this kinematic teleportation is undefined in regards to how the other kinematic properties should be handled. What if we add the flag and update the existing implementations of SetPosition to throw an exception (or indicate an error in some other way) when the type of teleportation requested is not how the particular physics engine implemented it? That would lead to a more well defined behavior overall, without requiring functional changes to existing code beyond some refactoring.
I completely agree with the motive of having better-defined behavior for the API between different physics engines, but I'm afraid that the current implementation of physics in Gazebo was not originally designed for the kind of multiple-engine support that it currently exercises. As a result, there isn't a clear path to standardizing the behavior in a way that wouldn't be harmful to existing users. Having rigorously-defined behavior is one of the major goals of an upcoming revision of Gazebo, but that won't be back-portable to resolve this outstanding issue, since it involves a considerable redesign of the codebase.
We could potentially provide something similar to what you're proposing. We could give SetPosition a preserve_world_velocity (notice that it's a more specific name) flag which defaults to false.
When preserve_world_velocity is false, the physics engine will do whatever its default legacy behavior is. For ODE and Bullet, it would set the world velocity of the child link to all zeros. For DART and Simbody, it would preserve the link velocity relative to the parent link.
When preserve_world_velocity is true, ODE and Bullet will skip over resetting the world velocity of the child link so its world velocity remains unaffected. DART and Simbody will perhaps print a warning message or throw an exception, depending on what everyone thinks is the most appropriate level of error reporting.
That would still leave the default behavior somewhat weakly-defined and inconsistent between physics engines, but that's unavoidable with the current design if we want to avoid breaking existing, working code.
I read through all the comments and try to find a workaround that could take both cases (zero velocity or not) carefully.
Based on the existing function name and documentation: SetPositionMaximal and SetPosition should only set the positions of
the child links of the joint, and leave the velocity as it is, although in some cases the preserved velocity doesn't make sense any more.
It should be the users' responsibility to zero the velocity, or set the velocity to some other values. We could provide such APIs, such as SetVelocityRelativeToFrame()
In this case, I would prefer the fix by Peter here 3dd9114
If we accept the above commit: (1) update the documentation of SetPosition such that users are clear that this set position only and doesn't affect twist / velocity at all
(2) MoveFrame will be unused in the existing gazebo repo.
However, this fix might break some users' code who depends on the default behavior of SetPosition to zero the velocity (although I guess most users probably are not aware that we
set the velocity to zero, unless they read the source code and they probably are not depending on this too much, but this is just a guess). Anyway, It is trivial to zero the velocity after calling a function.
Therefore, I tend to vote for this fix.
Another approach is proposed by Grey, where we add a new flag preserve_world_velocity, this might cause inconsistent behaviors for different physics engines, as covered above.
But this fix would guarantee the legacy behaviors.
Note that behavior is already inconsistent between different physics engines, so the fix that @zanderson and I are proposing would not have an impact on any existing behavior, and would allow the issue to be gracefully and opaquely resolved without any unintended consequences.
I think major behavioral changes should be avoided until Gazebo X, where we will have an opportunity to start from a fresh design and ensure that we have rigorous definitions for how the physics engines should behave.
For the record, I do agree in principle that SetPosition functions shouldn't zero out velocity, but since it's already a behavior which user code may easily be depending on, it could be harmful to change the default behavior while keeping the API the same. Users whose code suddenly starts to fail after an upgrade would be lost as to why it's suddenly failing.
Sounds good to me, let's go with the approach by adding a flag then
Has anyone started on implementing this solution yet? I've got a few lines of code written, but I don't want to be duplicating work if someone else has already made more progress. Additionally, should we add this flag to Joint::SetPositionMaximal as well or let it use the default behavior?
Yes, I have another PR here
Please feel free to review the PR
Thanks for your help and contribution!
Another PR has been created to reflect the discussions here.
Please review it and add your comments if any!