ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | Q&A answers.ros.org

# Position Only Inverse Kinematics

I am trying to compute the joint configurations for a given end-effector position. Note that, usually we need to provide the pose of the end-effector, which consist of position and orientation of end-effector.

In this experiment, I don't have the control over the orientation, hence I want the Inverse Kinematics to find out suitable orientation for the given position by itself . Below is the code snippet of two inverse kinematics solvers comparing their performance-

ROS_INFO_STREAM("*** Testing KDL with " << num_samples << " random samples");

for (uint i = 0; i < num_samples; i++) {
fk_solver.JntToCart(JointList[i], end_effector_pose);
KDL::Frame position_only(end_effector_pose.p);
double elapsed = 0;
result = nominal; // start with nominal
start_time = boost::posix_time::microsec_clock::local_time();
do {
q = result; // when iterating start with last solution
rc = kdl_solver.CartToJnt(q, position_only, result);
diff = boost::posix_time::microsec_clock::local_time() - start_time;
elapsed = diff.total_nanoseconds() / 1e9;
} while (rc < 0 && elapsed < timeout);
total_time += elapsed;
if (rc >= 0)
success++;

if (int((double)i / num_samples * 100) % 10 == 0)
ROS_INFO_STREAM_THROTTLE(1, int((i) / num_samples * 100) << "\% done");
}

ROS_INFO_STREAM("KDL found " << success << " solutions (" << 100.0 * success / num_samples << "\%) with an average of " << total_time / num_samples << " secs per sample");

total_time = 0;
success = 0;

ROS_INFO_STREAM("*** Testing TRAC-IK with " << num_samples << " random samples");

for (uint i = 0; i < num_samples; i++) {
fk_solver.JntToCart(JointList[i], end_effector_pose);
KDL::Frame position_only(end_effector_pose.p);
double elapsed = 0;
start_time = boost::posix_time::microsec_clock::local_time();
rc = tracik_solver.CartToJnt(nominal, position_only, result);
diff = boost::posix_time::microsec_clock::local_time() - start_time;
elapsed = diff.total_nanoseconds() / 1e9;
total_time += elapsed;
if (rc >= 0)
success++;

if (int((double)i / num_samples * 100) % 10 == 0)
ROS_INFO_STREAM_THROTTLE(1, int((i) / num_samples * 100) << "\% done");
}

ROS_INFO_STREAM("TRAC-IK found " << success << " solutions (" << 100.0 * success / num_samples << "\%) with an average of " << total_time / num_samples << " secs per sample");


The complete code is shared here. I observed that the both of the solvers are not able to perform well in this situation. Below is the output reported at terminal-

started roslaunch server http://localhost:38438/

SUMMARY
========

PARAMETERS
* /robot_description: <?xml version="1....
* /rosdistro: indigo
* /rosversion: 1.11.20
* /trac_ik_tests/chain_end: left_gripper
* /trac_ik_tests/chain_start: base
* /trac_ik_tests/num_samples: 1000
* /trac_ik_tests/timeout: 0.005
* /trac_ik_tests/urdf_param: /robot_description

NODES
/
trac_ik_tests (trac_ik_examples/ik_tests)

ROS_MASTER_URI=http://localhost:11311

core service [/rosout] found
process[trac_ik_tests-1]: started with pid [12645]
[ INFO] [1482810727.074051134]: IK Using joint left_s0 -1.70168 1.70168
[ INFO] [1482810727.074101021]: IK Using joint left_s1 -2.147 1.047
[ INFO] [1482810727.074111914]: IK Using joint left_e0 -3.05418 3.05418
[ INFO] [1482810727.074120316]: IK Using joint left_e1 -0.05 2.618
[ INFO] [1482810727.074129354]: IK Using joint left_w0 -3.059 3.059
[ INFO] [1482810727.074138501]: IK Using joint left_w1 -1.5708 2.094
[ INFO] [1482810727.074146281]: IK Using joint left_w2 -3.059 3.059
[ INFO] [1482810727.074333967]: Using 7 joints
[ INFO] [1482810727.074753732]: *** Testing KDL with 1000 random samples
[ INFO] [1482810727.547641243, 1115.429000000]: 10% done
[ INFO] [1482810728.930574499, 1116.803000000]: 40% done
[ INFO] [1482810730.296520427, 1118.165000000]: 70% done
[ INFO] [1482810731.606167939, 1119.474000000]: KDL found 111 solutions (11.1%) with an average of 0.00452833 secs per sample
[ INFO] [1482810731.606197656, 1119.474000000]: *** Testing TRAC-IK with 1000 random samples
[ INFO] [1482810731 ...
edit retag close merge delete

Sort by ยป oldest newest most voted

The reason for this is that the way you are describing the frame, it is not position only. Any KDL frame requires both the orientation and position. If either one is not provided, it defaults to Identity in case of rotation and zero in case of translation.

Hence you are providing a pose with the set position and identity orientation.

more