0
$\begingroup$

Rosanswers logo

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.611295425, 1119.479000000]: 0% done
[ INFO] [1482810732.653511218, 1120.511000000]: 30% done
[ INFO] [1482810733.712859122, 1121.566000000]: 60% done
[ INFO] [1482810734.717607925, 1122.566000000]: 90% done
[ INFO] [1482810735.056312802, 1122.900000000]: TRAC-IK found 395 solutions (39.5%) with an average of 0.0034435 secs per sample
[trac_ik_tests-1] process has finished cleanly
log file: /home/ravi/.ros/log/3591af68-cbe5-11e6-b5f7-2c56dcd23e48/trac_ik_tests-1*.log
all processes on machine have died, roslaunch will exit
shutting down processing monitor...
... shutting down processing monitor complete
done

Intuitively, it looks to me that in position only inverse kinematics, the search space for the solver is much more wider, whereas in case of pose (position and orientation together) the search space is constrained by orientation and effectively reduced significantly.

What's wrong here? Have a look please.


Originally posted by ravijoshi on ROS Answers with karma: 1744 on 2016-12-26

Post score: 2

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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.


Originally posted by hc with karma: 114 on 2017-05-17

This answer was ACCEPTED on the original site

Post score: 1

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.