Re: [ros-users] replicating pr2_arm_kinematics for 6-DOF arm

I finally gave this a try and it did not work for me. Here is a
rewritten version to get joint bound info from the URDF file and to
get joint names from the urdf description as well. I've attached a
revised version.

I tried this on both the PR2 and the Bandit model, and I kept getting
and "ik solver fail" error. This would occur even if I gave the
robot's current end-effector position. Attached are two files, on is a
revised goto_cart.cpp and another is a file that sends a test message.
To run on the pr2 model in gazebo:

./goto_cart _root_name:="l_shoulder_pan_link" _tip_name:="l_wrist_roll_link"
./cmd_ik

Are you using a version of kdl newer than what is being used in
cturtle? Have I messed something up with updating your file?

On Sat, Jul 17, 2010 at 11:37 AM, Morgan Quigley
<mquigley@cs.stanford.edu> wrote:
> Hi Dave,
>
> The Orocos KDL library is great. I just switched to it last week; my
> code is pretty ugly, but it's using the KDL::ChainIkSolverPos_NR_JL
> class, which has joint limits:
>
> http://stanford-ros-pkg.googlecode.com/svn/trunk/openarms/control/goto_cart.cpp
>
> The ik_tool() function is what you're after. You set the joint limits
> when you construct the ChainIkSolverPos_NR_JL object; search for
> "ik_solver_pos" in that .cpp file to see the exact syntax.
>
> (sorry about all the commented code... it's a work in progress)
>
> HTH,
> Morgan
>
>
> On Sat, Jul 17, 2010 at 10:43 AM, David Feil-Seifer
> <david.feilseifer@gmail.com> wrote:
>> I am trying to replicate the IK-functionality of pr2_arm_navigation
>> for our humanoid robot, Bandit. I have attached a xacro file for it's
>> arm. I have it rendering correctly, and am able to have it mimic the
>> movements of a real Bandit with joint_states_publisher and a
>> /joint_states message. I seem to have correctly ported
>> pr2_arm_ik_solver and pr2_arm_kinematics by simply changing all 7's to
>> 6's and changing the names of the root and tip links
>> (bandit_torso_link and left/right_hand_link). However, I'm having
>> trouble with pr2_arm_ik.cpp. Clearly, the math here was meant for a
>> 7-DOF arm, and I don't really know what algorithm is being used here,
>> so I'm having trouble translating that to a 6-DOF arm. I have figured
>> out that in the init function, that I needed to change the
>> link_offsets for the shoulder_upperarm_offset, elbow_wrist offset, and
>> upperarm_elbow_offset values. Those are now correct. I am just having
>> trouble making computeIKShoulderPan and computeIKShoulderRoll to a 6
>> joint arm. Can anyone give me some guidance on how to proceed?
>>
>> I did look at the KDL examples on their website, but they were pretty
>> vague about creating an IK solver with joint limits.
>>
>> Thanks for your help.
>> -Dave
>>
>> _______________________________________________
>> ros-users mailing list
>> ros-users@code.ros.org
>> https://code.ros.org/mailman/listinfo/ros-users
>>
>>
> _______________________________________________
> ros-users mailing list
> ros-users@code.ros.org
> https://code.ros.org/mailman/listinfo/ros-users
>
// bsd license blah blah
#include <cstring>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <tf_conversions/tf_kdl.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/LU>
#include "robot_state_publisher/treefksolverposfull_recursive.hpp"
#include <string>
#include <map>
#include <kdl_parser/kdl_parser.hpp>
#include <kdl/jntarray.hpp>
#include <kdl/chainiksolverpos_nr_jl.hpp>
#include <kdl/chainiksolvervel_pinv.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <bandit_ik/bandit_arm_kinematics_utils.h>
#include "geometry_msgs/Transform.h"
using std::string;
using std::map;
using std::make_pair;
using namespace Eigen;
using namespace bandit_ik;

int num_joints = 0;

sensor_msgs::JointState g_js, g_actual_js;
ros::Publisher *g_joint_pub = NULL;
tf::TransformBroadcaster *g_tf_broadcaster = NULL;
tf::TransformListener *g_tf_listener = NULL;
KDL::TreeFkSolverPosFull_recursive *g_fk_solver = NULL;
KDL::ChainIkSolverPos_NR_JL *g_ik_solver = NULL;
tf::Transform g_target_origin, g_target;
std::vector<double> g_pose;
std::string root_name, tip_name;
KDL::JntArray q_min, q_max;
tf::Transform fk_tool(const std::vector<double> &x) // joint angles 
{
  map<string, double> joint_pos;
  for( int i = 0; i <  num_joints; i++ )
  {
    joint_pos.insert(make_pair(g_js.name[i], x[i]));
  }

  map<string, KDL::Frame> link_poses;
  g_fk_solver->JntToCart(joint_pos, link_poses);
  if (link_poses.size() < num_joints)
    ROS_ERROR("couldn't compute forward kinematics.");
  for (map<string, KDL::Frame>::const_iterator f = link_poses.begin();
      f != link_poses.end(); ++f)
  {
    if (f->first == tip_name)
    {
      tf::Transform tf_frame;
      tf::TransformKDLToTF(f->second, tf_frame);
      return tf_frame;
    }
  }
  return tf::Transform();
}

bool ik_tool(tf::Transform t, std::vector<double> &joints)
{
  // assumes that "joints" coming in is the initial vector
  if (joints.size() != num_joints)
  {
    ROS_ERROR("woah there. the joints vector is supposed to be the start pos");
    return false;
  }
  KDL::JntArray q_init(num_joints), q(num_joints);
  for (int i = 0; i < num_joints; i++)
    q_init.data[i] = joints[i];
  // populate F_dest from tf::Transform parameter
  KDL::Frame F_dest;
  tf::TransformTFToKDL(t, F_dest);
  if (g_ik_solver->CartToJnt(q_init, F_dest, q) < 0)
  {
    ROS_ERROR("ik solver fail");
    return false;
  }
  for (int i = 0; i < num_joints; i++)
    joints[i] = q.data[i];
  return true;
}

void joint_cb(const sensor_msgs::JointState &msg)
{

  for( int i = 0; i < msg.name.size(); i++ )
  {
    for( int j = 0; j < num_joints; j++ )
    {
      if( msg.name[i] == g_js.name[j] )
      {
        g_actual_js.name[j] = g_js.name[j];
        g_actual_js.position[j] = msg.position[i];
      }
    }
  }
}

void target_cb(const geometry_msgs::TransformStamped &t_msg)
{
// assume the transform coming in is a delta away from the center
// of our workspace (for now, at least)

  tf::StampedTransform t;
  std::vector<double> j_ik;
  j_ik.resize(num_joints);
  if (g_actual_js.position.size() >= num_joints)
  {
    for (int i = 0; i < num_joints ; i++)
    {
      j_ik[i] = g_actual_js.position[i]; 
      ROS_INFO( "j_ik[%d]: %0.2f [%0.2f %0.2f]", i, j_ik[i]*180.0/M_PI, q_min.data[i]*180.0/M_PI, q_max.data[i]*180.0/M_PI );
    }
  }

  try
  {
    g_tf_listener->lookupTransform(root_name, tip_name,
                                   ros::Time(0), t);
  }
  catch (tf::TransformException ex)
  {
    ROS_ERROR("%s", ex.what());
    return;
  }

  tf::StampedTransform t_target_in_torso(g_target_origin * t, ros::Time::now(),
                                         "world", "ik_target");
/*
  geometry_msgs::TransformStamped target_trans_msg;
  tf::transformStampedTFToMsg(t_target_in_torso, target_trans_msg);
  g_tf_broadcaster->sendTransform(target_trans_msg);

  tf::StampedTransform t_target_origin(g_target_origin, ros::Time::now(),
                                         root_name, "target_origin");
  geometry_msgs::TransformStamped target_origin_msg;
  tf::transformStampedTFToMsg(t_target_origin, target_origin_msg);
  g_tf_broadcaster->sendTransform(target_origin_msg);
*/
  tf::transformStampedMsgToTF( t_msg, t );

ik_tool(t, j_ik);
g_js.header.stamp = ros::Time::now();
g_js.position = j_ik;
g_joint_pub->publish(g_js);
}

int main(int argc, char **argv)
{
ros::init(argc, argv, "ik_test");
ros::NodeHandle n;
ros::NodeHandle n_private("~");

root_name = "bandit_torso_link";
tip_name = "left_hand_link";

/* DFS: code to get model from parameter server */
urdf::Model robot_model;
std::string robot_desc;
KDL::Chain chain;

  while(!loadRobotModel(n_private,robot_model,root_name,tip_name,robot_desc) && n.ok())
  {
    ROS_ERROR("Could not load robot model. Are you sure the robot model is on the parameter server?");
    ros::Duration(0.5).sleep();
    ros::spinOnce();
  }

  KDL::Tree tree;
  if (!kdl_parser::treeFromString(robot_desc, tree))
  {
    ROS_ERROR("failed to extract kdl tree from xml robot description");
    return 1;
  }
  if (!tree.getNrOfSegments())
  {
    ROS_ERROR("empty tree. sad.");
    return 1;
  }

  if(!getKDLChain(robot_desc,root_name,tip_name,chain))
  {
    ROS_ERROR("Could not load kdl tree");
  }

ROS_INFO("parsed tree successfully");
///////////////////////////////////////////////////////////////////////

geometry_msgs::TransformStamped world_trans;
world_trans.header.frame_id = "world";
world_trans.child_frame_id = root_name;
world_trans.transform.translation.x = 0;
world_trans.transform.translation.y = 0;
world_trans.transform.translation.z = 0;
world_trans.transform.rotation = tf::createQuaternionMsgFromRollPitchYaw(0,0,0);

  // get joint maxs and mins
  boost::shared_ptr<const urdf::Link> link = robot_model.getLink(tip_name);
  while(link && link->name != root_name)
  {
    boost::shared_ptr<const urdf::Joint> joint = robot_model.getJoint(link->parent_joint->name);
    ROS_INFO( "adding joint: [%s]", joint->name.c_str() );
    if(!joint)
    {
      ROS_ERROR("Could not find joint: %s",link->parent_joint->name.c_str());
      return false;
    }
    if(joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
    {
      num_joints++;
    }
    link = robot_model.getLink(link->getParent()->name);
  }

q_min.resize(num_joints);
q_max.resize(num_joints);
g_js.name.resize(num_joints);
g_js.position.resize(num_joints);
g_actual_js.name.resize(num_joints);
g_actual_js.position.resize(num_joints);
KDL::TreeFkSolverPosFull_recursive fk_solver(tree);
g_fk_solver = &fk_solver;
KDL::SegmentMap::const_iterator root_seg = tree.getRootSegment();
string tree_root_name = root_seg->first;
ROS_INFO("root: %s", tree_root_name.c_str());
KDL::ChainFkSolverPos_recursive fk_solver_chain(chain);
KDL::ChainIkSolverVel_pinv ik_solver_vel(chain);

  link = robot_model.getLink(tip_name);
  int i = 0;
  while(link && i < num_joints)
  {
    boost::shared_ptr<const urdf::Joint> joint = robot_model.getJoint(link->parent_joint->name);
    ROS_INFO( "getting bounds for joint: [%s]", joint->name.c_str() );
    if(!joint)
    {
      ROS_ERROR("Could not find joint: %s",link->parent_joint->name.c_str());
      return false;
    }
    if(joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
    {
      if( joint->type != urdf::Joint::CONTINUOUS )
      {
        g_js.name[num_joints-i-1] = joint->name;
        q_min.data[num_joints-i-1] = joint->safety->soft_lower_limit;
        q_max.data[num_joints-i-1] = joint->safety->soft_upper_limit;
      }
      else
      {
        g_js.name[num_joints-i-1] = joint->name;
        q_min.data[num_joints-i-1] = -M_PI;
        q_max.data[num_joints-i-1] = M_PI;
      }
      i++;
    }
    link = robot_model.getLink(link->getParent()->name);
  }

  KDL::ChainIkSolverPos_NR_JL ik_solver_pos(chain, q_min, q_max,
                                            fk_solver_chain, ik_solver_vel, 
                                            100, 1e-6);
  g_ik_solver = &ik_solver_pos;
  ros::Subscriber target_sub = n.subscribe("target_frame", 1, target_cb);
  ros::Subscriber joint_sub = n.subscribe("joint_states", 1, joint_cb);
  ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("target_joints", 1);
  g_joint_pub = &joint_pub; // ugly ugly
  tf::TransformBroadcaster tf_broadcaster;
  tf::TransformListener tf_listener;
  g_tf_broadcaster = &tf_broadcaster;
  g_tf_listener = &tf_listener;
  ros::Rate loop_rate(20);

// set origin to be something we can comfortably reach
g_target_origin = btTransform(btQuaternion::getIdentity(), btVector3(0, 0.1, 0));

  while (ros::ok())
  {
    world_trans.header.stamp = ros::Time::now();
    tf_broadcaster.sendTransform(world_trans);

    loop_rate.sleep();
    ros::spinOnce();
  }
  return 0;
}

#include "ros/ros.h"
#include <geometry_msgs/TransformStamped.h>

int main( int argc, char* argv[] )
{
ros::init(argc,argv,"ik_cmd");
ros::NodeHandle n;
ros::Publisher joint_publisher = n.advertise<geometry_msgs::TransformStamped>("target_frame",1000);

geometry_msgs::TransformStamped t;

t.header.stamp = ros::Time::now();
t.header.frame_id = "l_shoulder_pan_link";

/*
t.transform.translation.x = 0.207;
t.transform.translation.y = 0.131;
t.transform.translation.z = -0.185;

t.transform.rotation.x = 0.093;
t.transform.rotation.y = 0.575;
t.transform.rotation.z = 0.812;
t.transform.rotation.w = -0.033;
*/
t.transform.translation.x = 0.782;
t.transform.translation.y = 0.187;
t.transform.translation.z = 0.151;

t.transform.rotation.x = 0.003;
t.transform.rotation.y = -0.605;
t.transform.rotation.z = -0.002;
t.transform.rotation.w = 0.796;

  while( ros::ok() )
  {
    joint_publisher.publish(t);
    ros::Duration(1).sleep();
    ros::spinOnce();
  }

return 0;
}