Position Only Inverse Kinematics

edit

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 ...