Open lyhbuaa opened 2 years ago
ikcon() % Numerical inverse kinematics with joint limits
Please send a complete example
@petercorke maybe I can provide a complete example of panda.
Notice that I have removed the panda hand from panda, so I rewrite the mdl_panda.m with changes to this line (other code leaves the same):
panda_withoutTool = SerialLink([L1 L2 L3 L4 L5 L6 L7], 'name', 'PANDA', 'manufacturer', 'Franka-Emika');
.
Then I record the current joint_pose and end-effector pose from franka_ros:
joint_pose:[0.17940711027697512, 0.4618104288114321, -0.07474237419220439, -1.3189208764310127, -0.01820848535001278, 1.8252406880905954, 0.8094893078808263]
cartesian_pose(in column-major format):[0.7681760305042956, -0.6365870159800344, 0.06828292058998993, 0.0, -0.6381994665792949, -0.7698674017593761, 0.002371616784093802, 0.0, 0.05105905420752297, -0.04539994266409048, -0.9976631787279365, 0.0, 0.6951395624527535, 0.081864847706614, 0.4775871098049651, 1.0]
When I use the following command, the forward kinematics returns right:
panda_withoutTool.fkine(joint_pose)
ans= 0.7682 -0.6382 0.0511 0.6951
-0.6366 -0.7699 -0.0454 0.08186
0.0683 0.0024 -0.9977 0.4776
0 0 0 1
However, when I use the inverse kinematic function, the answer turns out to violate the joint limit:
panda_withoutTool.ikine(ans)
ans=0.1268 1.3965 -0.0078 0.3852 -0.0503 1.0569 0.7919
It can be seen that the joint 4 violate the joint limits which is defined in .qlim : [-3.0718,-0.0698]
I am using the ikine() function to find the inverse solution of the manipulator, and I found that the qlim that set in the link didn't work,In the results, some of the joint Angle exceeds the limit. I don't know why and how to improve.
THANK YOU!