1

The objective I am solving is the position control of a nut screwing station. The pipeline is loosely based on that notebook.

Function make_gripper_frames was changed accordingly to command screwing motion of a gripper. PseudoInverseController of iiwa remains same as the one in a notebook. The gripper is controlled by schunk_wsg_position_controller in the same way: starts opened, closed on pick_end, reopened on place_end.

The bolt is welded to world frame, the nut is connected by a screw_joint.

My expectation is to see the controller driving a nut in meshcat visualizer. Instead, I see the nut remaining unmoved, while the gripper rotates around it. The effect is shown on the gif below: failed_case

Next, I provide a counterexample, where the screw_joint behaves (somewhat) correctly: The controller trajectory were purposefully changed, such that the gripper collides with the nut on the downward motion of gripper. This contact creates the expected change in screw_joint configuration:

good_case

Question: What change is needed in the controller to enable it to influence the nut rotationaly, i.e. to screw it. That is, countrary to only pushing it downward, as it were demonstated above. :D

Approaches already tested:

  • had a hypothesis that the physics of interaction isn't right, so I've grid-search through ineria, mass, damping, screw_pitch params of the nut. (to no avail)
  • had a hypothesis that the issue is due to poor contact modeling, so tried to replace the complex nut collision geomentry in sdf with a basic one (the box). Also, have tried to change schunk_wsg collision model from box to kBoxPlusFingertipSpheres (to no avail)
  • had a hypothesis that the issue has to do with schunk_wsg controller. Looks, like it barely contacts, but does not "grip". Have tried to log wsg_force_measured output port of the manipulation_station. I have seen little correlation between wsg_force_measured values and the timestamp of experiment, whenever the gripper is opened, closed, in contact or not.

My last step, that was not tried yet, is to replace schunk_wsg_position_controller with schunk_wsg_plain_controller in the force mode, and to command a high grip force on the end-effector.

Any suggestions will be appreciated!

Dmitri K
  • 331
  • 1
  • 12
  • 1
    Your example is awesome, and the debugging all looks very sensible to me. I don't know off the top of my head what might be wrong. If I were debugging this, I would definitely try turning on the contact force visualization (available in the new meshcat starting with the docker image I used in deepnote for clutter.ipynb). Failing that, I might try to separate the contact interactions from the dynamics of the bolt... you could take the robot out completely and apply a spatial force to the bolt using `MultibodyPlant::get_applied_spatial_force_input_port()`. – Russ Tedrake Oct 18 '21 at 12:00
  • Thank you, Professor, `get_applied_spatial_force_input_port` sounds like the way to go. – Dmitri K Oct 18 '21 at 13:40
  • Regarding forces visualization: that was a step I tried, but forgot to mention in the question :D Some boilerplate code, which turns this visualization on has failed due to `MescatVisualizer` / `MescatVisualizerCpp` compatibility. Maybe I'll push through to make it work, or will post another question. – Dmitri K Oct 18 '21 at 13:47
  • For future readers of this thread: the answer, accepted [here](http://stackoverflow.com/questions/61271126/applying-an-external-force-to-an-object-in-pydrake "here"), is relevant to the solution. – Dmitri K Oct 18 '21 at 21:06

0 Answers0