Simulate Robotiq 3-finger gripper

[mattcorsaro1]

I'm trying to use an accurate model of the Robotiq 3-finger gripper to grasp objects in Drake. I've made some modifications to the official URDF from the Robotiq ROS-I repo, as described below, and I'm now using Drake's examples/allegro_hand subpackage as a template. My code is available in the robotiq_3f_WIP branch of my fork.

The hand model looks correct when viewed with the geometry inspector:

bazel-bin/manipulation/util/geometry_inspector manipulation/models/robotiq_3f_description/urdf/robotiq-3f-gripper_articulated.urdf

The constant load demo is also able to load the hand, and the fingers fall reasonably:

bazel-bin/examples/robotiq_3f/run_robotiq_3f_constant_load_demo

However, when I add a controller in the single object demo, I've encountered a couple of errors:

bazel-bin/examples/robotiq_3f/joint_control/robotiq_3f_single_object_simulation

The current error I'm getting is:

abort: Failure at multibody/plant/tamsi_solver.cc:217 in SolveQuadraticForTheSmallestPositiveRoot(): condition 'Delta > 0' failed.
Aborted (core dumped)

In the visualizer, the fingers appear to be in an unrealistic initial configuration. Maybe it's unable to find a path to the desired states. Any ideas?

On a slightly stale (a couple of days old, I think) version of master, I was initially getting this error:

terminate called after throwing an instance of 'std::runtime_error'
  what():  MultibodyPlant's discrete update solver failed to converge at simulation time =   0.001 with discrete update period =   0.000. This usually means that the plant's discrete update period is too large to resolve the system's dynamics for the given simulation conditions. This is often the case during abrupt collisions or during complex and fast changing contact configurations. Another common cause is the use of high gains in the simulation of closed loop systems. These might cause numerical instabilities given our discrete solver uses an explicit treatment of actuation inputs. Possible solutions include:
  1. reduce the discrete update period set at construction,
  2. decrease the high gains in your controller whenever possible,
  3. switch to a continuous model (discrete update period is zero),      though this might affect the simulation run time.
Aborted (core dumped)

I reduced my controller's gains in examples/robotiq_3f/robotiq_3f_common.cc, increased FLAGS_max_time_step in examples/robotiq_3f/joint_control/robotiq_3f_single_object_simulation.cc, tried several initial hand poses, and reduced each joint's velocity and effort limits and added damping, but received the same error. I was able to get the SolveQuadraticForTheSmallestPositiveRoot(): condition 'Delta > 0' failed error by removing several collision elements from my SDF (all but finger_*_link_3_collision).

If someone has a chance to take a quick look, it would be greatly appreciated. Thank you!

-Matt

[mattcorsaro1]

The documentation in my branch is far from complete; sorry. Here's how I modified the original robotiq-3f-gripper_articulated.urdf:

[Edit: added details] I started by modifying the ROS-Industrial robotiq-3f-gripper_articulated.urdf.

In my branch, I use relative paths to mesh files, which have been converted to .obj. I also replaced the collision meshes with boxes, though it's still a bit rough; I left plenty of space between the links to avoid self-collisions.

I also updated the joint limits based on some example data I recorded of my physical hand closing. I recalculated the visual meshes' inertial properties using MeshLab and this tutorial because the previous values didn't satisfy the triangle inequality and Drake's geometry_inspector wouldn't load the model without valid values. (I think MeshLab assumes a density of 1, so these values may not be accurate. Any more accurate methods you could recommend?)

I still use the previous estimated mass values and the large velocity and effort limits. In examples/robotiq_3f, I'm still using the velocity_thresh and controller gains from examples/allegro_hand that examples/robotiq_3f was based on.

[sherm1]

Any thoughts on this, @amcastro-tri ?

[sammy-tri]

Assigning to dynamics and @amcastro-tri since the error in question is coming out of that code and it seems like backing up from the failing assertion is a reasonable place to start. Feel free to remove labels/assignments or reassign if this doesn't seem appropriate.

[RussTedrake]

Looks like you are setting up the desired initial positions of the fingers in the controller, but not actually setting the actual initial positions of the fingers in the MBP? So the fingers might be starting in collision, which could lead to very stiff differential equations and this error? (it could also happen if your PID gains are too high).

N.B. -- when we've simulated the robotiq3 before, we've actually added the loop joints so that it gets the "underactuated" property of the hand more correct than that default urdf. We should add that version once #11831 lands.

[amcastro-tri]

ok @mattcorsaro1. I found several bugs.

1) Probably the most important; this is way to complex of a system to start with! debugging the physics would not need adding controllers nor any sort of LCM publishers and not even the mug. I for instance grabbed your example and removed everything but the model of the gripper with its PID controller. 2) Your fingertips are setup in a condition beyond the joint limits. This is usually real bad for the solver. 3) I didn't modify them, but your intertias area clear off by several orders of magnitude. Take finger_middle_link_3. If I compute the inertia of the box collision geometry analytically as an estimation of the ineritias, I get that these should be in the order of 5e-6. However all of your inertias are three orders of magnitude smaller. That'd explain the sudden accelerations you observe in sim. Maybe here you forgot to scale using the volume? MeshLab uses a density of one (thus mass = 1 * volume). If you scale up to increase precision you'd need to adjust accordingly.

Hopefully this help.s

[amcastro-tri]

Update. As a rough test, I multiplied all inertias by 1e3 (thus matching my order of magnitude estimation using boxes). For this case the simulation is stable. I was able to run with a time step as high as h=4e-4 (It becomes unstable at 5e-4, so I would recommend using something like 2e-4 or smaller.).

Another tip. It is useful to watch what happens at each time step. I accomplish that by using a ridiculously small --target_realtime_rate. For instance, I'd do:

bazel run examples/robotiq_3f/joint_control:robotiq_3f_single_object_simulation -- --max_time_step=1e-4 --simulation_time=1e-3 --target_realtime_rate=1e-4

so that I can see each time step with a one second pause in between. This is useful for cases like these for which you get very large accelerations (that's when I realized that either: a) gains were wrong or b) the model inertias were wrong.)

[mattcorsaro1]

Awesome, thank you both for looking into this for me.

@RussTedrake, any idea where the existing model is? It would be a useful reference.

@amcastro-tri, fantastic, thanks for the detailed response!

2. I set the initial finger positions in a new mugless demo before initializing the simulator. Will that be enough?
3. I got better estimates of the mass values from Robotiq's support team, added them to the URDF, and fixed the inertial values. The demo works now! Thanks for the debugging tips.

Feel free to close this, or leave it open until the underactuated model is added.

[RussTedrake]

How about these: https://github.com/openhumanoids/oh-distro/tree/master/software/models/common_components/robotiq_hand_description (note, the four-bar used some drake tags that were supported in matlab but are getting redone in the c++ code)

[amcastro-tri]

Glad to hear you made it work!, @mattcorsaro1

I love in your new demo you are using RevolutJoint::set_angle()'s API. That's the agnostic way to set the state without having to worry about indexes, specially useful for big models.

I see you have a hard-coded distal_joint_max, is that the joint limit? because in that case you could query the joint with RevolutJoint::position_lower/upper_limit().

I'd also recommend you use:

• AddMultibodyPlantSceneGraph(), which greatly simplifies things because it adds a MultibodyPlant and a SceneGraph to your diagram and wires them up for you. The docs contain examples and a global search in Drake will take you to more.
• plant.WeldFrames(), instead of plant.AddJoint<multibody::WeldJoint>(). It simplifies arguments.
  • Instead of hard-coded indexers for initial_joint_config, you could do initial_joint_config[finger_middle_joint_3.position_start()] = distal_joint_max, etc.

Don't forget to remove the code block:

builder.Connect(
      plant.get_geometry_poses_output_port(),
      scene_graph.get_source_pose_port(plant.get_source_id().value()));
  builder.Connect(scene_graph.get_query_output_port(),
                  plant.get_geometry_query_input_port());

once you start using AddMultibodyPlantSceneGraph().

happy codding!

[mattcorsaro1]

Awesome, thanks for the comments! I've gone through and made the changes you recommended.

Without gravity, my bazel-bin/examples/robotiq_3f/joint_control/run_grasping_mug script correctly sends the fingers to the closed state I defined.

When I add gravity with the --add_gravity flag, the fingers fall at the palmfinger{1,2}_joints, past the limits I specified in the URDF. Any idea why they might fall and how to enforce the joint limits? Thanks!

[amcastro-tri]

humm... not sure without running it myself. But MultibodyPlant uses compliant joint limits, not hard constraints. Therefore it is natural you'd observe some limit violation. However the violation should go to zero as max_time_step², do you observe that? If not, there's another bug somewhere.

[mattcorsaro1]

The gravity issue isn't blocking at the moment.

I think there's a problem with is_finger_stuck_ used in the allegro_hand example that I copied. A finger is considered stuck if any of its torque commands are in the opposite direction of a joint's velocity. This doesn't seem to be an issue with allegro_hand's mug_twisting example, but in my robotiq_3f example, when the fingers overshoot the goal and have to change direction, is_finger_stuck_ is true and the motion is considered complete.

I'll tune my PID controller, but is there a better way to determine if a finger is stuck @amcastro-tri ? I'm trying to emulate the gripper's underactuated properties in the controller, i.e. close a finger until it makes contact, then possibly continue to close some joints based on which link is in contact. Is there a cleaner way to check to see which link is in collision from the allegro's twisting_mug example? The context is created in allegro_single_object_simulation.

Thank you!

[amcastro-tri]

AFAIK that solution was engineered for the Allegro example and I doubt it is general enough you can apply it to your case. It is also a toy controller only for the example so I also doubt you'd learn any good control strategy from that example. That is, I'd only take that example as a good learning snippet to play with APIs. For your particular problem, you'd need to engineer a proper controller.

What problem are you trying to solve exactly? it'd be difficult for me to give you answer without a complete problem statement.

[mattcorsaro1]

Sounds good. Are there any more robust examples you recommend taking a look at?

I'm trying to simulate grasps. Given an object on a table, move the gripper to a user-given pose, close the fingers, and attempt to move upwards. Determine if the grasp was successful based on whether the object is still on the table.

[amcastro-tri]

I'd imagine many solutions to the same problem. Are you position controlling your gripper? possibly with a torque limit? If so I'd imagine something like:

1. Track when joint velocities are below a threshold (attempting to detect steady state). This has two outcomes: a) nothing gripped, b) something gripped.
2. Cases (a) and (b) could be distinguished by either: i) if the gripper reached the final commanded state, then most likely you gripped nothing. ii) if the gripper is "far" from the commanded closed state, most likely something jammed the gripper (hopefully your object).
3. You can verify (2) by looking at the reaction forces, MultibodyPlant::get_reaction_forces_output_port().

I'd imagine combinations and/or variations of those are possible.

[mattcorsaro1]

My robotiq_3f example is available in my fork here. The underactuation is modeled as a hybrid system as described in Technical Report: Use of Hybrid Systems to model the RobotiQ Adaptive Gripper. I ended up using the contact results from MultibodyPlant::get_contact_results_output_port() to determine when each link made contact with the target object. I didn't tune the gains to match the real gripper's output torques, but the behavior seems reasonably correct.

[jwnimmer-tri]

@amcastro-tri what work remains w.r.t. this bug report, and to what component should it be assigned?

Is this a usability defect in that some of the above was too difficult to discover? Or is there an error message missing?

[amcastro-tri]

The problems we found here are:

1. State intialized beyond joint limits. Tracked by #15765 currently in "About to Start" in MBP Tech Debt.

2. Rotational inertias off by three orders of magnitude. This could be mitigated by #13033. There is some work in #16466, but as far as I know only for MuJoCo parsing.

   I believe this could be closed since the two issues above track the actual problems. This particular issue was indeed resolved.