Strange behaviour of robot model when using Joint::SetVelocity (VelocityJointInterface)

asked 2014-10-01 08:35:14 -0600

Felix
31 ●5 ●5 ●8

updated 2014-10-01 08:42:17 -0600

Dear all,

the problem stated in the title of this question is a real blocker for us at the moment.

What we try to do:
Our setup is: Trusty, Indigo, gazebo2_2.2.3-1~trusty_amd64

We try to simulate two 7-DoF arms:
- Schunk lwa4d
- Schunk lwa4p
Models can be found here for lwa4d and here for lwa4p_extended

We try to control those robots using velocity_controllers/JointVelocityControllers which uses the VelocityJointInterface hardware interface (specified in transmission). Velocity commands will finally be sent from a higher level node. For testing we simply use a terminal publisher or rqt.

To reproduce the behaviour described below, there exist convenience "bringup"-packages containing all launch- and config files here.

Simply run either
roslaunch schunk_lwa4d lwa4d_sim_urdf.launch or
roslaunch schunk_lwa4p_extended lwa4p_extended_sim_urdf.launch respectively.

The problem we are having
It seems that (for some) joints, the velocity commands cannot be executed by the simulated robot. I.e. the current joint velocity (as published in /joint_states/velocity differs from the velocity command being send from the (forward command) controller velocity_controllers/JointVelocityController (on topic /arm_X_joint_velocity_controller/command).

The following videos visualize the current behaviour.
In video1 it can be seen that arm_2_joint of the Schunk lwa4p_extended already starts to drop/drift right from the beginning of the simulation, i.e. whon no velocity command has yet been send and thus goal velocity is 0.0 for all joints.
In video2 it can be seen that also commanding velocities != 0.0 are not executed correctly.
However in video3 it can be seen that velocity commands actually can be executed without dropping/drifting. (Beside the strange behaviour of arm_6_joint! As this is also of the same motor type as used in the lwa4p_extended, I assume that the behaviour of that arm_6_joint has similar reasons.)

What we already tried to fix it
Of course, I found the following related threads on this list:

In this thread and this thread it is suggested to check SetMaxForce. However, as I am using the gazebo_ros_control plugin with DefaultRobotHWSim hardware interface, the value passed to Joint::SetMaxForce is received from the effort limit in the URDF. We use values from the technical data sheets of our motors for our model but of course also tested (arbitrarily) higher values - without success.
In this thread the problem was caused by a wrong inertia tag in the URDF. However, in our models, we use inertias derived from CAD data. We also tried to use "simplified" approximations for the inertias using these macros - again: no success.

We also played around with the meshes used for our models in order to guarantee they don't collide thus causing unexpected forces to the joints - again: no success.

It's really frustrating, as I don't have any more ideas what could be causing this strange behaviour.
But as it seems to actually be possible to command joints by sending velocity commands with SetVelocity (see lwa4d arm_2_joint), it doesn't seem to be a general problem with Joint::SetVelocity.

Could it be that it ... (more)

edit retag flag offensive close merge delete

add a comment

6 Answers

Sort by » oldest newest most voted

answered 2023-08-04 11:00:26 -0600

01binary
1

I just experienced the same issue and managed to solve it without awkward workarounds (like the ones suggested by other answers).

Posting the solution here in case it helps anyone:

1. Remove the pid_gains configuration specified for /gazebo_ros_control:

You should not have the following loaded as part of any launch files:

/gazebo_ros_control:
  pid_gains:
    base:
      p: 10
      i: 1
      d: 1

The reason is because there's a condition in gazebo controller integration (called gazebo_ros_control) that switches between "Force" mode and "Velocity" mode depending on whether the pid configuration is specified. If you don't have this config, the plugin will complain:

No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/base

This sounds like a problem that must be fixed, so of course you go and set the pid_gains. That will switch the trajectory controller into VELOCITY_PID mode that does not work in Gazebo (due to issues described by DongDong above).

(https://github.com/ros-controls/gazeb...)

// Initialize the PID controller. If no PID gain values are found, use joint->SetAngle() or
// joint->SetVelocity() to control the joint.
const ros::NodeHandle nh(model_nh, robot_namespace + "/gazebo_ros_control/pid_gains/" +
                          joint_names_[j]);
if (pid_controllers_[j].init(nh, true))
{
  switch (joint_control_methods_[j])
  {
    case POSITION:
      joint_control_methods_[j] = POSITION_PID;
      break;
    case VELOCITY:
      joint_control_methods_[j] = VELOCITY_PID;
      break;
  }
}
else
{
  // joint->SetMaxForce() must be called if joint->SetAngle() or joint->SetVelocity() are
  // going to be called. joint->SetMaxForce() must *not* be called if joint->SetForce() is
  // going to be called.
  joint->SetMaxForce(0, joint_effort_limits_[j]);
}

In the code above, you can see that not specifying pid configuration makes it drop into the else block and call SetMaxForce so that it can use SetVelocity later to directly set the joint velocity, instead of going through its broken velocity PID controller. Here's where it does that, just below in the same file:

case VELOCITY:
  sim_joints_[j]->SetVelocity(0, e_stop_active_ ? 0 : joint_velocity_command_[j]);
  break;

case VELOCITY_PID:
  double error;
  if (e_stop_active_)
    error = -joint_velocity_[j];
  else
    error = joint_velocity_command_[j] - joint_velocity_[j];
  const double effort_limit = joint_effort_limits_[j];
  const double effort = clamp(pid_controllers_[j].computeCommand(error, period),
                              -effort_limit, effort_limit);
  sim_joints_[j]->SetForce(0, effort);
  break;

If you don't have pid_gains it would cause the VELOCITY to be used, which sets velocity and works fine. If you have pid_gains it uses VELOCITY_PID and freaks out because the physics simulation doesn't have enough resolution for it to work.

Which brings us to...

2. Ensure the inertias specified for the links are absolutely correct!

Incorrect inertias can cause the simulation to completely blow up, or move in tiny amounts for no particular reason, which is very irritating to watch. I recommend the following:

Set the center of gravity for each link to something other than its origin. The origin of the coordinate frame is almost always not the same as the actual center of gravity for an object. It's very convenient to set the coordinate frame origin to the point the link rotates around, but the center of gravity tends to be near the center of the ...

(more)

edit flag offensive delete link

add a comment

answered 2021-05-05 15:48:00 -0600

Adam Gronewold
1 ●1 ●3

Hello Felix,

I hate to reinitiate this discussion as I am 7 years late to the part. Nevertheless, this problem still seems persistent and is my primary limiting factor at the moment. In contrast to some of the suggestions above, I have found that using a effortControllers/JointVelocityController does not fix this issue. The behavior is the same in both cases for me.

I see that the pull requests listed by SCPeters are no longer accessible. My assumption is that whatever they were initially opened for was completed, so the requests have been deleted. Either way, the problem has not been fixed. I have a lengthy explanation of my specific case here: https://answers.gazebosim.org/questio....

The main things that are limiting for me is that I get motion simply by turning the controllers on, without sending a command. I have found that changing my PID values for the joint impact this startup motion, but I certainly would not say I can "tune" my control because the system is clearly incredibly messy. I have settled on leaving < limit effort=2000 velocity=3.6> again, in line with a physical robot. I will be posting a spread sheet of what behavior I get by varying PID values on my question listed above later today. In the meantime, if you, or anyone, happens to see my comment and is knowledgeable about the direction I should move in, that would be appreciated.

edit flag offensive delete link

add a comment

answered 2014-10-06 04:29:42 -0600

Felix
31 ●5 ●5 ●8

updated 2014-10-06 04:32:05 -0600

Hi there,

I think, I found a way to make my model behave as desired, i.e. execute the commanded velocities (without "dropping" when velocities 0.0 are sent to the joints).

However, I'm not sure whether I should be happy about this or disappointed:
I'm now using a fake inertia in all our links. Therefore, I defined the following xacro macro and use it in all our links without caring about actual mass and distribution of mass of the corresponding link of the real hardware:

  <xacro:macro name="default_inertial">
    <inertial>
      <mass value="0.01" />
      <origin xyz="0 0 0" />
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
    </inertial>
  </xacro:macro>

This macro corresponds to a box-shaped body of mass 0.01 kg and a size of about 0.8 x 0.8 x 0.8 m³ (x-y-z). With this macro, the arm now moves the joints with the velocities commanded by the controller.

What makes me disappointed about this, is the fact that we put tremendous efforts in assessing the correct inertia values for our (non-primitive) links by assigning mass properties to the CAD data. We also used data from the technical specs sheets of the real hardware for specifying other properties. Even playing around with all the other properties that can be set in the URDF like, damping, effort limits, soft-limits, mechanicalReduction of transmission as well as all the gazebo-/sdf- tags (see also this thread and this thread) did not improve the behaviour.

Questions
Thus, I would like to know whether Gazebo is able to correctly model non-primitive inertias? Like inertias of asymmetric links that have values != 0.0 for the entries ixy, ìxz and iyz, i.e. values beyond the diagonal of the inertia matrix?

Is there any robot out there that uses a (URDF or SDF) model that actually corresponds to its real hardware properties? That I could use as a template? That uses a VelocityJointInterface as well and works well in simulation?

What other options do I have to further investigate why our model (with real inertias) is not able to execute the commanded velocities?

Am I using wrong units?

edit flag offensive delete link

Comments

Hi, I'm stuck in the same situation described in the open post. I've tried to interact with my simulated robot arm in a lot of ways, but I still can't find a solution to control it through velocity and not only by position. I also tried the solution suggested in the last answer (implementation of a fake inertia in my urdf model) but it didn't work either.

Are there some news in your work Felix? Did you find a better solution of the last one?

LyMich ( 2015-04-16 04:47:39 -0600 )edit

add a comment

answered 2014-10-07 13:15:54 -0600

Jim Rothrock

247 ●8 ●12 ●20 http://www.wunderkamme...

To help debug the problem, you could replace velocity_controllers/JointVelocityController with effort_controllers/JointVelocityController, then set each controller's PID parameters to appropriate values. This will cause DefaultRobotHWSim to use SetForce() instead of SetVelocity(). Links will still drop, however, because each PID controller will not apply any force to its joint until the joint has a non-zero velocity. The links will just drop slowly. The only way to prevent this with PID control is to use effort_controllers/JointPositionController, then set joint positions instead of velocities.

edit flag offensive delete link

Comments

The reason why we also need JointVelocityController is because we also would like to command the arm based on a given Cartesian twist that is then converted into joint velocities using (some kind of) inverse Jacobian...we still use JointPositionController and JointTrajectoryController in according other control modes.

And we wanted to play use the velocity_controllers (HW-Interface) to be 1:1 compatible with our hardware.

Felix ( 2014-10-08 09:34:32 -0600 )edit

Early experiments with using effort_controllers were also not very satisfying but this may have been due to wrong models...

We will probably try it again with our latest models but use the (rather hidden) feature offered by DefaultRobotHWSim to set /gazebo_ros_control/pid_gains/ parameters in order to use VELOCITY_PID control_method

This way we could keep the velocity_controllers (HW-Interface) and generate effort commands under the hood of the gazebo_ros_control plugin...

Felix ( 2014-10-08 09:41:43 -0600 )edit

When I wrote "set each controller's PID parameters to appropriate values," I was referring to /gazebo_ros_control/pid_gains. Sorry for the confusion. Also, I was incorrect in saying that you should replace velocity_controllers/JointVelocityController with effort_controllers/JointVelocityController. You should use velocity_controllers/JointVelocityController, but set PID parameters in /gazebo_ros_control/pid_gains.

Jim Rothrock ( 2014-10-08 17:20:27 -0600 )edit

add a comment

answered 2014-10-07 00:18:29 -0600

scpeters
2861 ●4 ●21 ●33

Sorry I haven't had a chance to test your models. I will comment on the behavior of Joint::SetVelocity as I'm one of the system integrators for Gazebo physics. I'm not sure it will be enough for the chocolate bar, however.

This is actually very timely, since I just submitted a pull request to change the way that Joint::SetVelocity works. In ODE, a torque-limited speed constraint is used with SetVelocity and SetMaxForce. In the other physics engines, however, SetVelocity is akin to resetting the initial conditions. It also behaves differently for long chains in the maximal coordinate solvers (ODE, Bullet) and minimal coordinate solvers (Simbody, DART). For ODE and Bullet (as of pull request 1218), SetVelocity will set the velocity of a single child link relative to the parent link, while the minimal coordinate solvers will set the velocity of all subsequent child links down the rest of the chain. I'm going to create an issue to document this difference until I have a chance to fix it.

edit flag offensive delete link

Comments

I assume that your PR is on top of latest gazebo4 from source...I will test our model against your PR branch and check whether it behaves differently...hopefully better....I'll report the outcome here...

Felix ( 2014-10-08 09:59:46 -0600 )edit

add a comment

answered 2014-11-13 09:07:32 -0600

DongDong
46 ●1 ●3 ●4

Hi Felix,

My advice on simulating a velocity controlled robot arm in gazebo is to use a joint position controller in gazebo. To do this, you have to program a interface which accepts the desired velocity from your trajectory planner and integrates the velocity to the desired position. Then you can send this position to the joint position controller. In the real hardware, you are not required to use this interface, you can just directly send the planning result to the robot. This works for our velocity controlled kuka LBR.

I have experienced some similar issue by tuning the joint velocity controller in the earlier version of gazebo without any success. Because of the physics simulation, i can never get the exact desired velocity to be set on the arm, which causes an position error between the planned and executed trajectory.

edit flag offensive delete link

add a comment

Strange behaviour of robot model when using Joint::SetVelocity (VelocityJointInterface)

6 Answers

Comments

Comments

Comments

Question Tools

Stats

Related questions

Strange behaviour of robot model when using Joint::SetVelocity (VelocityJointInterface) edit

6 Answers

Comments

Comments

Comments

Question Tools

Stats

Related questions

Strange behaviour of robot model when using Joint::SetVelocity (VelocityJointInterface)