예제 #1
0
bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
                                           const std::vector<double> &ik_seed_state,
                                           double timeout,
                                           std::vector<double> &solution,
                                           const IKCallbackFn &solution_callback,
                                           moveit_msgs::MoveItErrorCodes &error_code,
                                           const std::vector<double> &consistency_limits,
                                           const kinematics::KinematicsQueryOptions &options) const
{
  ros::WallTime n1 = ros::WallTime::now();
  if(!active_)
  {
    ROS_ERROR_NAMED("kdl","kinematics not active");
    error_code.val = error_code.NO_IK_SOLUTION;
    return false;
  }

  if(ik_seed_state.size() != dimension_)
  {
    ROS_ERROR_STREAM_NAMED("kdl","Seed state must have size " << dimension_ << " instead of size " << ik_seed_state.size());
    error_code.val = error_code.NO_IK_SOLUTION;
    return false;
  }

  if(!consistency_limits.empty() && consistency_limits.size() != dimension_)
  {
    ROS_ERROR_STREAM_NAMED("kdl","Consistency limits be empty or must have size " << dimension_ << " instead of size " << consistency_limits.size());
    error_code.val = error_code.NO_IK_SOLUTION;
    return false;
  }

  KDL::JntArray jnt_seed_state(dimension_);
  KDL::JntArray jnt_pos_in(dimension_);
  KDL::JntArray jnt_pos_out(dimension_);

  KDL::ChainFkSolverPos_recursive fk_solver(kdl_chain_);
  KDL::ChainIkSolverVel_pinv_mimic ik_solver_vel(kdl_chain_, joint_model_group_->getMimicJointModels().size(), redundant_joint_indices_.size(), position_ik_);
  KDL::ChainIkSolverPos_NR_JL_Mimic ik_solver_pos(kdl_chain_, joint_min_, joint_max_, fk_solver, ik_solver_vel, max_solver_iterations_, epsilon_, position_ik_);
  ik_solver_vel.setMimicJoints(mimic_joints_);
  ik_solver_pos.setMimicJoints(mimic_joints_);

  if ((redundant_joint_indices_.size() > 0) && !ik_solver_vel.setRedundantJointsMapIndex(redundant_joints_map_index_))
  {
    ROS_ERROR_NAMED("kdl","Could not set redundant joints");
    return false;
  }

  if(options.lock_redundant_joints)
  {
    ik_solver_vel.lockRedundantJoints();
  }

  solution.resize(dimension_);

  KDL::Frame pose_desired;
  tf::poseMsgToKDL(ik_pose, pose_desired);

  ROS_DEBUG_STREAM_NAMED("kdl","searchPositionIK2: Position request pose is " <<
                   ik_pose.position.x << " " <<
                   ik_pose.position.y << " " <<
                   ik_pose.position.z << " " <<
                   ik_pose.orientation.x << " " <<
                   ik_pose.orientation.y << " " <<
                   ik_pose.orientation.z << " " <<
                   ik_pose.orientation.w);
  //Do the IK
  for(unsigned int i=0; i < dimension_; i++)
    jnt_seed_state(i) = ik_seed_state[i];
  jnt_pos_in = jnt_seed_state;

  unsigned int counter(0);
  while(1)
  {
    //    ROS_DEBUG_NAMED("kdl","Iteration: %d, time: %f, Timeout: %f",counter,(ros::WallTime::now()-n1).toSec(),timeout);
    counter++;
    if(timedOut(n1,timeout))
    {
      ROS_DEBUG_NAMED("kdl","IK timed out");
      error_code.val = error_code.TIMED_OUT;
      ik_solver_vel.unlockRedundantJoints();
      return false;
    }
    int ik_valid = ik_solver_pos.CartToJnt(jnt_pos_in, pose_desired, jnt_pos_out);
    ROS_DEBUG_NAMED("kdl","IK valid: %d", ik_valid);
    if(!consistency_limits.empty())
    {
      getRandomConfiguration(jnt_seed_state, consistency_limits, jnt_pos_in, options.lock_redundant_joints);
      if( (ik_valid < 0 && !options.return_approximate_solution) || !checkConsistency(jnt_seed_state, consistency_limits, jnt_pos_out))
      {
        ROS_DEBUG_NAMED("kdl","Could not find IK solution: does not match consistency limits");
        continue;
      }
    }
    else
    {
      getRandomConfiguration(jnt_pos_in, options.lock_redundant_joints);
      ROS_DEBUG_NAMED("kdl","New random configuration");
      for(unsigned int j=0; j < dimension_; j++)
        ROS_DEBUG_NAMED("kdl","%d %f", j, jnt_pos_in(j));

      if(ik_valid < 0 && !options.return_approximate_solution)
      {
        ROS_DEBUG_NAMED("kdl","Could not find IK solution");
        continue;
      }
    }
    ROS_DEBUG_NAMED("kdl","Found IK solution");
    for(unsigned int j=0; j < dimension_; j++)
      solution[j] = jnt_pos_out(j);
    if(!solution_callback.empty())
      solution_callback(ik_pose,solution,error_code);
    else
      error_code.val = error_code.SUCCESS;

    if(error_code.val == error_code.SUCCESS)
    {
      ROS_DEBUG_STREAM_NAMED("kdl","Solved after " << counter << " iterations");
      ik_solver_vel.unlockRedundantJoints();
      return true;
    }
  }
  ROS_DEBUG_NAMED("kdl","An IK that satisifes the constraints and is collision free could not be found");
  error_code.val = error_code.NO_IK_SOLUTION;
  ik_solver_vel.unlockRedundantJoints();
  return false;
}
예제 #2
0
int main(int argc, char *argv[]) 
{
    ros::init(argc, argv, "kinematics_publisher");
    ros::NodeHandle nh;
    ros::Publisher joint_publisher;
    ros::Publisher frame_publisher;

    joint_publisher = nh.advertise<sensor_msgs::JointState>("kinematics/joint_states", 1, true);
    frame_publisher = nh.advertise<geometry_msgs::PoseStamped>("/frame_result", 1, true);

    std::vector<std::string> names;
    names.resize(2);
    names.at(0) = "base_to_body";
    names.at(1) = "body_to_head";

    sensor_msgs::JointState my_joints;
    my_joints.name = names;
    my_joints.position.resize(2);
    my_joints.position.at(0) =  0.2;
    my_joints.position.at(1) = 0,2;

    joint_publisher.publish(my_joints);

    ros::spinOnce();
    std::cin.get();
	bool exit_value;
	KDL::Tree my_tree;
	urdf::Model my_model;
	

    if (!kdl_parser::treeFromParam("/robot_description", my_tree)){
        ROS_ERROR("Failed to construct kdl tree");
        return false;
    }


    std::cout << "Number of links of the urdf:" << my_tree.getNrOfSegments() << std::endl;
    std::cout << "Number of Joints of the urdf:" << my_tree.getNrOfJoints() << std::endl;

    KDL::Chain my_chain;
    my_tree.getChain("world", "webcam", my_chain);

    std::cout << "Number of links of the CHAIN:" << my_chain.getNrOfSegments() << std::endl;
    std::cout << "Number of Joints of the CHAIN:" << my_chain.getNrOfJoints() << std::endl;


    KDL::ChainFkSolverPos_recursive fksolver(my_chain);
    KDL::JntArray q(my_chain.getNrOfJoints());
    q(0) =  my_joints.position.at(0);
    q(1) =  my_joints.position.at(1);

    KDL::Frame webcam;
    fksolver.JntToCart(q,webcam);

    geometry_msgs::Pose webcam_pose;
    tf::poseKDLToMsg(webcam, webcam_pose);

    geometry_msgs::PoseStamped webcam_stamped;
    webcam_stamped.pose = webcam_pose;
    webcam_stamped.header.frame_id = "world";
    webcam_stamped.header.stamp = ros::Time::now();

    frame_publisher.publish(webcam_stamped);
    ros::spinOnce();

    KDL::ChainJntToJacSolver jacobian_solver(my_chain);
    KDL::Jacobian J;
    J.resize(2);
    jacobian_solver.JntToJac(q, J);

    std::cout << J.data << std::endl;

    webcam.M.DoRotY(-20*KDL::deg2rad);

    tf::poseKDLToMsg(webcam, webcam_pose);
    webcam_stamped.pose = webcam_pose;
    webcam_stamped.header.stamp = ros::Time::now();

    frame_publisher.publish(webcam_stamped);
    ros::spinOnce();

    std::cin.get();

    KDL::ChainIkSolverVel_pinv ik_solver_vel(my_chain);
    KDL::ChainIkSolverPos_NR ik_solver_pos(my_chain,fksolver,ik_solver_vel);
    KDL::JntArray q_sol(2);

    ik_solver_pos.CartToJnt(q, webcam,q_sol);

    my_joints.position.at(0) =  q_sol(0);
    my_joints.position.at(1) = q_sol(1);

    joint_publisher.publish(my_joints);

    ros::spinOnce();
    std::cin.get();

    KDL::Vector g (0.0,0.0,-9.81);
    KDL::ChainDynParam my_chain_params(my_chain, g);
    KDL::ChainIdSolver_RNE id_solver(my_chain, g);

    KDL::JntArray G(2);
    my_chain_params.JntToGravity(q_sol,G);

    KDL::JntSpaceInertiaMatrix H(2);
    my_chain_params.JntToMass(q_sol, H);

    KDL::JntArray C(2);
    KDL::JntArray q_dot(2);
    q_dot(0) = 0.02;
    q_dot(1) = 0.04;
    my_chain_params.JntToCoriolis(q_sol,q_dot, C);

    std::cout << "g(q): " <<G.data << std::endl;
    std::cout << "M(q): " <<H.data << std::endl;
    std::cout << "C(q,q_dot): " <<C.data << std::endl;


    KDL::JntArray q_dotdot(2);
    q_dotdot(0) = -0.02;
    q_dotdot(1) = 0.02;
    KDL::Wrenches f(4);
    KDL::JntArray tau(2);
    id_solver.CartToJnt(q_sol, q_dot, q_dotdot, f, tau);
    std::cout << "Tau: " <<tau.data << std::endl;

    std::cin.get();
    moveit::planning_interface::MoveGroup my_group("webcam_robot");
    my_group.setNamedTarget("look_left");
    my_group.move();


    return false;
}