How to generate a trajectory that the end effector stays at a fixed point while other joints move for a redundant manipulator?

Hi everyone !

I want to generate a trajectory that the end effector stays at a fixed point while another link reaches at certain point on Franka-Emika Panda. I find Plan a Reaching Trajectory With Multiple Kinematic Constraints and try to modify the example script. But I can't fix the ee at a certain point.

I use constraintCartesianBounds to confine the position of ee:

EndEffectorStaysAt = constraintCartesianBounds(gripper);% gripper = 'panda_link7';
EndEffectorStaysAt.Bounds = [0.5500, 0.5600; ...% x      the exact position is [0.5550,0,0.6150]
                           -0.01, 0.01; ...% y
                           0.6100, 0.6200];% z
EndEffectorStaysAt.Weights=[1 1 1];

The other constraint that describes which point link4 is reaching is defined as follows(I put a cup at that point)

distanceFromCup = constraintPositionTarget('cupFrame');
distanceFromCup.ReferenceBody = 'panda_link4';
distanceFromCup.PositionTolerance = 0.005;
distanceFromCup.TargetPosition = [0,0,0];
distanceFromCup.Weights = 1;

and here is my solver:

gik = generalizedInverseKinematics('RigidBodyTree', lbr, 'ConstraintInputs', {'cartesian','position'})
[qWaypoints(2,:),solutionInfo] = gik(q0, EndEffectorStaysAt,distanceFromCup);

However the result shows obviously that the gripper(ee) doesn't follow the constraint, while the link4 does.

where is my problem? thanks!

Answer 1

[Karsh Tharyani]

Hi Xiangjie,

Thanks for your question. I don't think you are doing anything wrong definining your constraints. I ran the following short reproducible based on your description:

lbr = loadrobot("frankaEmikaPanda", "DataFormat","row");
gripper = 'panda_link7';
EndEffectorStaysAt = constraintCartesianBounds(gripper);
EndEffectorStaysAt.Bounds = [0.5500, 0.5600; ...% x  the exact position is [0.5550,0,0.6150]
                           -0.01, 0.01;      ...% y
                           0.6100, 0.6200];     % z
EndEffectorStaysAt.Weights=[1 1 1];
cupHeight = 0.5;
cupRadius = 0.05;
cupPosition = [-0.2, 0.2, cupHeight/2];
body = rigidBody('cupFrame');
setFixedTransform(body.Joint, trvec2tform(cupPosition))
addBody(lbr, body, lbr.BaseName);
distanceFromCup = constraintPositionTarget('cupFrame');
distanceFromCup.ReferenceBody = 'panda_link4';
distanceFromCup.PositionTolerance = 0.005;
distanceFromCup.TargetPosition = [0,0,0];
distanceFromCup.Weights = 1;
numWaypoints = 5;
q0 = homeConfiguration(lbr);
qWaypoints = repmat(q0, numWaypoints, 1);
gik = generalizedInverseKinematics('RigidBodyTree', lbr, ...
    'ConstraintInputs', {'cartesian','position'});
[qWaypoints(2,:),solutionInfo] = gik(q0, EndEffectorStaysAt,distanceFromCup);

If you look at the solutionInfo below:

solutionInfo

solutionInfo = 

  struct with fields:

              Iterations: 1500
       NumRandomRestarts: 30
    ConstraintViolations: [1×2 struct]
                ExitFlag: 2
                  Status: 'best available'

the ExitFlag is 2 — Maximum number of iterations reached. (You can learn more on how to read the "solution info" from here and more about exit flags here: ExitFlags)

Given above, it likely means that the problem you have defined is kinematically infeasible and the solver is outputting the 'best available' solution. If you feel that there should be a feasible solution, you can try tuning the SolverParameters based on the exit flag.

You can also try using the interactiveRigidBodyTree and try to see whether moving a certain marker body particularly is causing the violation of these constraints. This will help you in fine tuning your constraints. You can try something along the lines of:

irbt = interactiveRigidBodyTree(lbr);
irbt.Constraints = {EndEffectorStaysAt, distanceFromCup};

If you still feel that the results are not as expected, please feel free to reach out to Technical Support with your reproduction steps.

Best,

Karsh