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

3 views (last 30 days)
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!

Answers (1)

Karsh Tharyani
Karsh Tharyani on 19 Jan 2021
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

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!