CODEWITHSUNDEEP
nonlinear-optimizationdrake
kiwiwan
kiwiwan

Reputation: 1

sliding_friction_complementarity_constraint for a box manipulation

I want to do the project that two iiwa manipulator each with a point finger to carry a box or do the job like drake Gallery’s “IIWA Bimanual Manipulation”, based on trajectory optimization method in drake.

So I start with a simple test that a passive box with x direction initial velocity on the ground, and to find the trajectory of the box. I use trajectory optimization similar to Michael Posa’s paper, but instead of the friction and complementary constrain with sliding_friction_complementarity_constraint from drake. My code write with python based pydrake in the file test_only_box_passive_sliding_friction_issue.ipynb.

But now I get stuck here that I couldn’t get a solution with this simple test. So I want to get some help from drake team.
There are some questions:

  1.Is it because my initial guess is bad?

  2. Whether I should replace the nonliear friction cone with linear friction to improve the solution process?

  3. In the sliding_friction_complementarity_constraint, whether I should add static friction cone constrain to the fstatic variable? I observe that when contact tangential velocity is not zero,only can infer the normal fstatic is zero,but the tangential fstatic is not constrained even if the static friction cone constrain add to the f.

  4.Whether I should use other method? I know this test is a simulate problem, is not a true trajectory optimization problem, and LCP method could more appropriate to solve it. But my goal is to planning two iiwa manipulation, LCP method seem not so good for trajectory optimization according to Michael Posa’s paper.

Any advise for my project would be very appreciated!

Upvotes: 0

Views: 96

Answers (1)

Hongkai Dai
Hongkai Dai

Reputation: 2766

I glanced quickly on your code, a couple of issues

  1. You use quaternion to represent the box orientation, and also use the backward Euler integration for the quaternion, namely MapQuaternionDotToAngularVelocity((quat[n+1] - quat[n])/h) = angular_velocity. I wouldn't recommend doing it this way, because (quat[n+1] - quat[n])/h is not a valid quaternion derivative at time n or n+1 (A valid quaternion time derivative needs to be perpendicular to the quaternion). So I would recommend to use QuaternionEulerIntegrationConstraint
  2. I would suggest to first remove the sliding friction cone constraint and optimize the trajectory, see whether the solver can find a solution. This should verify that your kinematic and dynamic constraint (except the friction force) is correct.

In the sliding_friction_complementarity_constraint, whether I should add static friction cone constrain to the fstatic variable? I observe that when contact tangential velocity is not zero,only can infer the normal fstatic is zero,but the tangential fstatic is not constrained even if the static friction cone constrain add to the f.

I think you care about the total friction force (coming from both f_static and f_sliding) is within the friction cone. So as long as you have that constraint |f_tangential | ≤ μ f_normal , the friction cone constraint is satisfied. You don't have to impose the friction cone constraint on the static force.

Upvotes: 0

Related Questions