1

I am working on a simulation, which contains:

  • a bolt, welded uprightly to the world
  • a nut, connected to the bolt via ScrewJoint. The mass of a nut set to 0.02 kg, the inertia is a diagonal 1.1e-9 *I. This is configured via a .sdf file.
  • an iiwa manipulator, which is beside a point for now.

The problem is that the nut is very hard to manipulate and I cannot find a parameter to adjust, which could've made it more lifelike. To be more specific:

  • I measure the ability of force, applied tangentially in a horizontal plane to the nut, to cause the screwing motion of a joint, that joins the nut with a bolt
  • I'd like to have greater amount of motion at lower forces, and so far I am failing to achieve that
  • My interest in doing this is not idle; I am interested in more complicated simulations, which are also failing when iiwa is coming in a contact with this same joint; I've asked about those here and here. (Both answered partially). To sum up those here: when manipulator grips the nut, the nut fights the screwing in such a manner, that the schunk gripper is forced to unclasp and iiwa is thrown off-track, but the nut remains stationary.
  • I attach below two simpler experiments to better illustrate the issue:

1. Applying tangentially in a horizontal plane 200N force using ExternallyAppliedSpatialForce. enter image description here

Graph notation: (here as well as below) The left graph contains linear quantities (m, m/s, etc) along world's Z axis, the right graph contains angular quantities (in degrees, deg, deg/s, etc) around world's Z axis. The legend entries with a trailing apostrophe use the secondary Y-axis scale; other legend entries use the primary Y-axis scale.

Experiment summary:

This works as expected, 200 N is enough to make the nut spin on a bolt, resulting in the nut traveling vertically along the bolt for just under 1 centimeter, and spinning for over 90 degrees. Note: the externally applied force does not show up on my graph.

2. Applying tangentially in a horizontal plane force using iiwa and a simple position controller. enter image description here

Experiment summary: The force here is approximately the same as before: 70N along tz, but higher (170N) in tx and ty, though it is applied now only for a brief moment. The nut travels just a few degrees or hundredth fractions of centimeter. The video of this unsuccessful interaction is below, the contact forces are visualized using ContactVisualizer.

enter image description here

Please advise me on how to make this screw_joint more compliant?

I've tried varying mass and inertia of the nut (different up to the orders of magnitude) in these experiments, this seems to scale the contact forces, but does not affect acceleration or velocity of the nut after contact.

Dmitri K
  • 331
  • 1
  • 12

2 Answers2

3

I like your experiment using ExternallyAppliedSpatialForce to get an idea of scales, though TBH I didn't quite get the details of this setup.

Things that caught my eye though are about scales, which you can estimate with pen and paper:

  1. Your inertia is 1e-9 kg⋅m²?! Judging from your interaction with the iiwa I estimated a radius of 1cm and with that you'd get 2e-6 kg⋅m², three orders of magnitude larger.
  2. A force of 200 N on a 20 grams nut would cause an acceleration of 10000 m/s². As a reference, that's 1000 times the acceleration of Earth's gravity!

Are these numbers correct? Also, if you happen to have fast interactions (do you?), you might want to estimate a time step that makes sense for your application.

Hopefully this helps!

Alejandro
  • 1,064
  • 1
  • 8
  • 22
  • 1
    0) to address the first (unnumbered) remark about the setup: to simplify, I've replaced the original "nut gripping & screwing" controller (which does not work) with the "nut side-poking" controller (which does not work also - imaged in video). I compare it to `ExternallyAppliedSpatialForce`(which works). 1) The width of nut side is 4cm, but I am adjusting it's inertia to your suggested value. BTW, do you think that the inertial matrix should have off-diagonal terms in my case? – Dmitri K Oct 24 '22 at 20:50
  • 1
    2) It's interesting, because in my experiment (the second block of graphs, without `ExternallyAppliedSpatialForce`) the contact forces are at the same order of magnitude `70N`, but the nut doesn't achieve any extreme accelerations due to this contact. 3) This interaction isn't high-speed; I've decreased the simulation `time_step` 10-fold (down to `2.e-4 sec`) which didn't produce any change on my graphs. – Dmitri K Oct 24 '22 at 20:50
  • 1
    do you have a simple, clean branch, to reproduce this one single problem? if you provide README with instructions on how to compile and run (I won't have time to reverse engineer it) I could try help debugging it. (I recommend sharing a public git branch with an example and README) – Alejandro Oct 24 '22 at 21:50
  • 1
    This experiment takes 1 command to run and is available publicly [here](https://github.com/wf34/nut_screwing_manipulator/tree/experiment-with-manipulating-from-the-side) branch name: `experiment-with-manipulating-from-the-side`. The only nuisance is that the project depends (via bazel dependencies) on a custom branch of `drake`, `experiment/wf34/hybrid_control_for_nut_screwing2`, which is available [here](https://github.com/wf34/drake/tree/experiment/wf34/hybrid_control_for_nut_screwing2). – Dmitri K Oct 25 '22 at 08:54
  • 1
    I can simplify this setup to run with upstream version of drake. But I think I won't get to it until next week. – Dmitri K Oct 26 '22 at 18:41
1

A good thing is that I've fixed it; a bad thing: I don't understand the fix.

Let's restate a problem that I was tacking: a manipulated object reacted quite predictably to the ExternallyAppliedSpatialForce, but couldn't be moved via the contact with the manipulator.

What was done:

  1. I've update drake from 2f340192a9dc79110410faf8a6d54a8615ddca92 (circa 22 Aug, 2022) to 42448c0af1b39f0c46f760e7ae37d77097689ad3 (circa 3 Nov, 2022)
  2. After the update, my experimental setup broke down with assertion Actuation input port for model instance ... must be connected. [Similar to the issue raised in this question.]. My fix was like that:
bolt_n_nut_ = internal::AddAndWeldModelFrom(sdf_path, "nut_and_bolt",
  plant_->world_frame(),
  "bolt", X_WC, plant_);

then later in ManipulationStation::Finalize:

auto zero_torque = builder.template AddSystem<systems::ConstantVectorSource<double>>(
  Eigen::VectorXd::Zero(1));
builder.Connect(zero_torque->get_output_port(),
  plant_->get_actuation_input_port(bolt_n_nut_));
  1. With changes above, a manipuland began to interact with the manipulator: enter image description here enter image description here

Things to note in graphs:

  1. the distance the manipuland has moved grew from fractions of millimeter up to tens of centimeters. The video presents that a nut became manipulable.
  2. This interaction violates the constraints of the ScrewJoint, i.e. the manipuland moves along it's axis without as much rotation
Dmitri K
  • 331
  • 1
  • 12