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?

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?

Asked by LyMich on 2015-04-16 04:47:39 UTC

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.

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...

Asked by Felix on 2014-10-08 09:59:46 UTC

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.

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.

Asked by Felix on 2014-10-08 09:34:32 UTC

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...

Asked by Felix on 2014-10-08 09:41:43 UTC

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.

Asked by Jim Rothrock on 2014-10-08 17:20:27 UTC

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.

Comments

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/question/26924/why-are-my-joints-accelerating-on-their-own-wo-control-windup/?answer=26937#post-id-26937.

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.

Comments

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/gazebo_ros_control/blob/jade-devel/src/default_robot_hw_sim.cpp#L201-L223)

// 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 link.

It can be set by modifying the xyz attribute of the origin tag inside inertial tag.

<inertial>
    <origin
        rpy="0 0 0"
        xyz="0.20328210 -0.00230698 -0.00000001"
    />
    <mass value="0.63998399" />
    <inertia
        ixx="0.00026130"
        ixy="0.00059318"
        ixz="-0.000000001"
        iyy="0.01189494"
        iyz="0.000000001"
        izz="0.01212353"
    />
</inertial>

• Set the inertia attributes (inside inertial tag) to valid values, which should be specified with reference to the center of gravity. You can set a very rough approximation like what the original poster recommended above:

https://answers.gazebosim.org/question/7142/strange-behaviour-of-robot-model-when-using-jointsetvelocity-velocityjointinterface/?answer=7163#post-id-7163

...but it must be valid

• Validate both the inertial values AND the center of gravity by looking at Inertial visualization in Gazebo.

In Gazebo, click View -> Inertias.

There should be a translucent magenta box around each joint, roughly covering the majority of the joint, and being oriented accordingly.

If your link model is a weird skewed shape for instance, the inertia box should actually follow that shape - so I am not just talking about the joint transform being applied to the inertia to orient it, I am talking about the orientation encoded inside the two ix_ and iy_ vectors. The inertia itself is oriented to follow the geometry of the link like a "shrink-wrapped" convex hull averaged as a box.

If you want the exact center of gravity and inertia, I recommend importing your model into a CAD program, setting the material (like Aluminum 6061 or Steel) and then having the CAD program calculate the values.

For example, I use Autodesk Inventor. In this program, you can go to File -> iProperties -> Physical, choose accuracy, and it will calculate the Mass, the Center of Gravity, and three types of inertial values for you: Principal, Global, and Center of Gravity. I then take the inertial values from Center of Gravity. There might be a trial you can get just to calculate your stuff and move on.

Comments