Ask Your Question
2

Position Only Inverse Kinematics

asked 2016-12-26 22:31:41 -0500

Ravi Joshi gravatar image

updated 2016-12-26 22:34:00 -0500

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 ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2017-05-17 08:03:18 -0500

hc gravatar image

updated 2017-05-17 08:03:41 -0500

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.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2016-12-26 22:31:41 -0500

Seen: 332 times

Last updated: May 17 '17