How to generate a trajectory that the end effector stays at a fixed point while other joints move for a redundant manipulator?
2 次查看(过去 30 天)
显示 更早的评论
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.
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.
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/490015/image.png)
where is my problem? thanks!
0 个评论
回答(1 个)
Karsh Tharyani
2021-1-19
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
0 个评论
另请参阅
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!