Ejemplo n.º 1
0
bool KDLKinematicsPlugin::getPositionFK(const std::vector<std::string> &link_names,
                                        const std::vector<double> &joint_angles,
                                        std::vector<geometry_msgs::Pose> &poses) const
{
  ros::WallTime n1 = ros::WallTime::now();
  if(!active_)
  {
    ROS_ERROR_NAMED("kdl","kinematics not active");
    return false;
  }
  poses.resize(link_names.size());
  if(joint_angles.size() != dimension_)
  {
    ROS_ERROR_NAMED("kdl","Joint angles vector must have size: %d",dimension_);
    return false;
  }

  KDL::Frame p_out;
  geometry_msgs::PoseStamped pose;
  tf::Stamped<tf::Pose> tf_pose;

  KDL::JntArray jnt_pos_in(dimension_);
  for(unsigned int i=0; i < dimension_; i++)
  {
    jnt_pos_in(i) = joint_angles[i];
  }

  KDL::ChainFkSolverPos_recursive fk_solver(kdl_chain_);

  bool valid = true;
  for(unsigned int i=0; i < poses.size(); i++)
  {
    ROS_DEBUG_NAMED("kdl","End effector index: %d",getKDLSegmentIndex(link_names[i]));
    if(fk_solver.JntToCart(jnt_pos_in,p_out,getKDLSegmentIndex(link_names[i])) >=0)
    {
      tf::poseKDLToMsg(p_out,poses[i]);
    }
    else
    {
      ROS_ERROR_NAMED("kdl","Could not compute FK for %s",link_names[i].c_str());
      valid = false;
    }
  }
  return valid;
}
Ejemplo n.º 2
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;
}
            bool unigripper_motoman_plant_t::IK_solver(const config_t& effector_config, space_point_t* computed_state, bool set_grasping, const space_point_t* seed_state, bool do_min)
            {
                double qx,qy,qz,qw;
                double x,y,z;
                effector_config.get_orientation().get(qx,qy,qz,qw);
                effector_config.get_position(x,y,z);
                KDL::Chain chain1;
                my_tree->getChain("base_link","vacuum_volume",chain1);
                KDL::JntArray q(chain1.getNrOfJoints());
                KDL::JntArray q_out(chain1.getNrOfJoints());
                KDL::JntArray q_min(chain1.getNrOfJoints());
                KDL::JntArray q_max(chain1.getNrOfJoints());


                std::vector< double > state_var;
                if( seed_state != NULL )
                    state_space->copy_point_to_vector( seed_state, state_var );
                else
                    state_space->copy_to_vector( state_var );
                q(0) = state_var[0];
                q(1) = state_var[1];
                q(2) = state_var[2];
                q(3) = state_var[3];
                q(4) = state_var[4];
                q(5) = state_var[5];
                q(6) = state_var[6];
                q(7) = state_var[7];
                q(8) = state_var[15];

                q_min(0) = state_space->get_bounds()[0]->get_lower_bound();
                q_min(1) = state_space->get_bounds()[1]->get_lower_bound();
                q_min(2) = state_space->get_bounds()[2]->get_lower_bound();
                q_min(3) = state_space->get_bounds()[3]->get_lower_bound();
                q_min(4) = state_space->get_bounds()[4]->get_lower_bound();
                q_min(5) = state_space->get_bounds()[5]->get_lower_bound();
                q_min(6) = state_space->get_bounds()[6]->get_lower_bound();
                q_min(7) = state_space->get_bounds()[7]->get_lower_bound();
                q_min(8) = state_space->get_bounds()[15]->get_lower_bound();

                q_max(0) = state_space->get_bounds()[0]->get_upper_bound();
                q_max(1) = state_space->get_bounds()[1]->get_upper_bound();
                q_max(2) = state_space->get_bounds()[2]->get_upper_bound();
                q_max(3) = state_space->get_bounds()[3]->get_upper_bound();
                q_max(4) = state_space->get_bounds()[4]->get_upper_bound();
                q_max(5) = state_space->get_bounds()[5]->get_upper_bound();
                q_max(6) = state_space->get_bounds()[6]->get_upper_bound();
                q_max(7) = state_space->get_bounds()[7]->get_upper_bound();
                q_max(8) = state_space->get_bounds()[15]->get_upper_bound();

                KDL::ChainFkSolverPos_recursive fk_solver(chain1);
                KDL::ChainIkSolverVel_pinv ik_solver_vel(chain1);
                KDL::ChainIkSolverPos_NR_JL ik_solver(chain1,q_min,q_max,fk_solver,ik_solver_vel,1000,1e-6);
                KDL::Frame F(KDL::Rotation::Quaternion(qx,qy,qz,qw),KDL::Vector(x,y,z));
                bool ik_res = (ik_solver.CartToJnt(q,F,q_out)>=0);

                if(ik_res)
                {
                    std::vector<double> state_vec;
                    state_vec = state_var;
                    state_vec[0] = q_out(0);
                    state_vec[1] = q_out(1);
                    state_vec[2] = q_out(2);
                    state_vec[3] = q_out(3);
                    state_vec[4] = q_out(4);
                    state_vec[5] = q_out(5);
                    state_vec[6] = q_out(6);
                    state_vec[7] = q_out(7);
                    state_vec[15] = q_out(8);
                    state_vec[16] = set_grasping ? (double)GRIPPER_CLOSED : GRIPPER_OPEN;
                    state_space->copy_vector_to_point( state_vec, computed_state );
                }

                return ik_res;
            }