/**
	HACK!
*/
void Character::updateJointOrdering(){
	if( jointOrder.size() == joints.size() ) 
		return; // HACK assume ordering is ok
	jointOrder.clear();

	if (!root)
		return;
	DynamicArray<ArticulatedRigidBody*> bodies;
	bodies.push_back(root);

	int currentBody = 0;

	while ((uint)currentBody<bodies.size()){
		//add all the children joints to the list
		for (int i=0;i<bodies[currentBody]->getChildJointCount();i++){
			Joint *j = bodies[currentBody]->getChildJoint(i);
			jointOrder.push_back( getJointIndex(j->getName()) );
			bodies.push_back(bodies[currentBody]->getChildJoint(i)->getChild());
		}
		currentBody++;
	}

	reverseJointOrder.resize( jointOrder.size() );
	for( uint i=0; i < jointOrder.size(); ++i )
		reverseJointOrder[jointOrder[i]] = i;

}
/* Method to calculate the FK for the required joint configuration
 *  @returns true if successful
 */
bool arm_kinematics::Kinematics::getPositionFK(
    std::string frame_id, const sensor_msgs::JointState &joint_configuration,
    geometry_msgs::PoseStamped &result) {
  KDL::Frame p_out;
  KDL::JntArray jnt_pos_in;
  tf::Stamped<tf::Pose> tf_pose;

  // Copying the positions of the joints relative to its index in the KDL chain
  jnt_pos_in.resize(num_joints);
  for (unsigned int i = 0; i < num_joints; i++) {
    int tmp_index = getJointIndex(joint_configuration.name[i]);
    if (tmp_index >= 0)
      jnt_pos_in(tmp_index) = joint_configuration.position[i];
  }

  int num_segments = chain.getNrOfSegments();
  ROS_DEBUG("Number of Segments in the KDL chain: %d", num_segments);
  if (fk_solver->JntToCart(jnt_pos_in, p_out, num_segments) >= 0) {
    tf_pose.frame_id_ = root_name;
    tf_pose.stamp_ = ros::Time();
    tf::poseKDLToTF(p_out, tf_pose);
    try {
      tf_listener.transformPose(frame_id, tf_pose, tf_pose);
    } catch (...) {
      ROS_ERROR("Could not transform FK pose to frame: %s", frame_id.c_str());
      return false;
    }
    tf::poseStampedTFToMsg(tf_pose, result);
  } else {
    ROS_ERROR("Could not compute FK for endpoint.");
    return false;
  }
  return true;
}
bool extractJointState(const sensor_msgs::JointState& joint_state,
                       const kinematics_msgs::KinematicSolverInfo &chain_info,
                       std::vector<double>& values)
{
    values.resize(chain_info.joint_names.size());
    for(unsigned int i = 0; i < chain_info.joint_names.size(); i++) {
        const std::string& joint_name = chain_info.joint_names[i];
        int values_ind = getJointIndex(joint_name, chain_info);
        if(values_ind == -1) {
            ROS_WARN_STREAM("Can't find joint " << joint_name << " in chain");
            return false;
        }
        bool found = false;
        for(unsigned int j = 0; j < joint_state.name.size(); j++) {
            if(joint_state.name[j] == joint_name) {
                found = true;
                values[values_ind] = joint_state.position[j];
                ROS_INFO_STREAM("Setting joint name " << joint_name << " ind " << values_ind << " value " << joint_state.position[j]);
                break;
            }
        }
        if(!found) {
            ROS_WARN_STREAM("Can't find joint " << joint_name << " in joint state");
            return false;
        }
    }
    return true;
}
Пример #4
0
IndexArray HuboDescription::getJointIndices(StringArray joint_names) const
{
    IndexArray result;
    for(size_t i=0; i < joint_names.size(); ++i)
    {
        result.push_back(getJointIndex(joint_names[i]));
    }
    return result;
}
Пример #5
0
bool PR2ArmKinematics::getPositionFK(kinematics_msgs::GetPositionFK::Request &request, 
                                     kinematics_msgs::GetPositionFK::Response &response)
{
  if(!active_)
  {
    ROS_ERROR("FK service not active");
    return true;
  }

  if(!checkFKService(request,response,fk_solver_info_))
    return true;

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

  jnt_pos_in.resize(dimension_);
  for(int i=0; i < (int) request.robot_state.joint_state.position.size(); i++) 
  {
    int tmp_index = getJointIndex(request.robot_state.joint_state.name[i],fk_solver_info_);
    if(tmp_index >=0)
      jnt_pos_in(tmp_index) = request.robot_state.joint_state.position[i];
  }

  response.pose_stamped.resize(request.fk_link_names.size());
  response.fk_link_names.resize(request.fk_link_names.size());

  bool valid = true;
  for(unsigned int i=0; i < request.fk_link_names.size(); i++)
  {
    ROS_DEBUG("End effector index: %d",pr2_arm_kinematics::getKDLSegmentIndex(kdl_chain_,request.fk_link_names[i]));
    ROS_DEBUG("Chain indices: %d",kdl_chain_.getNrOfSegments());
    if(jnt_to_pose_solver_->JntToCart(jnt_pos_in,p_out,pr2_arm_kinematics::getKDLSegmentIndex(kdl_chain_,request.fk_link_names[i])) >=0)
    {
      tf_pose.frame_id_ = root_name_;
      tf_pose.stamp_ = ros::Time();
      tf::PoseKDLToTF(p_out,tf_pose);
      tf::poseStampedTFToMsg(tf_pose,pose);
      if(!transformPose(request.header.frame_id, pose, response.pose_stamped[i])) {
	response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
	return false;
      }
      response.fk_link_names[i] = request.fk_link_names[i];
      response.error_code.val = response.error_code.SUCCESS;
    }
    else
    {
      ROS_ERROR("Could not compute FK for %s",request.fk_link_names[i].c_str());
      response.error_code.val = response.error_code.NO_FK_SOLUTION;
      valid = false;
    }
  }
  return true;
}
Пример #6
0
//this assumes that everything has been checked and is in the correct frame
bool PR2ArmKinematics::getPositionIKHelper(kinematics_msgs::GetPositionIK::Request &request, 
                                           kinematics_msgs::GetPositionIK::Response &response)
{
  KDL::Frame pose_desired;
  tf::poseMsgToKDL(request.ik_request.pose_stamped.pose, pose_desired);

  //Do the IK
  KDL::JntArray jnt_pos_in;
  KDL::JntArray jnt_pos_out;
  jnt_pos_in.resize(dimension_);
  for(int i=0; i < dimension_; i++)
  {
    int tmp_index = getJointIndex(request.ik_request.ik_seed_state.joint_state.name[i],ik_solver_info_);
    if(tmp_index >=0)
    {
      jnt_pos_in(tmp_index) = request.ik_request.ik_seed_state.joint_state.position[i];
    }
    else
    {
      ROS_ERROR("i: %d, No joint index for %s",i,request.ik_request.ik_seed_state.joint_state.name[i].c_str());
    }
  }
  
  int ik_valid = pr2_arm_ik_solver_->CartToJntSearch(jnt_pos_in,
                                                     pose_desired,
                                                     jnt_pos_out,
                                                     request.timeout.toSec());
  if(ik_valid == pr2_arm_kinematics::TIMED_OUT)
    response.error_code.val = response.error_code.TIMED_OUT;
  if(ik_valid == pr2_arm_kinematics::NO_IK_SOLUTION)
    response.error_code.val = response.error_code.NO_IK_SOLUTION;

  response.solution.joint_state.header = request.ik_request.pose_stamped.header;
  
  if(ik_valid >= 0)
  {
    response.solution.joint_state.name = ik_solver_info_.joint_names;
    response.solution.joint_state.position.resize(dimension_);
    for(int i=0; i < dimension_; i++)
    {
      response.solution.joint_state.position[i] = jnt_pos_out(i);
      ROS_DEBUG("IK Solution: %s %d: %f",response.solution.joint_state.name[i].c_str(),i,jnt_pos_out(i));
    }
    response.error_code.val = response.error_code.SUCCESS;
    return true;
  }
  else
  {
    ROS_DEBUG("An IK solution could not be found");   
    return true;
  }
}
/* Method to calculate the IK for the required end pose
 *  @returns true if successful
 */
bool arm_kinematics::Kinematics::getPositionIK(
    const geometry_msgs::PoseStamped &pose_stamp,
    const sensor_msgs::JointState &seed, sensor_msgs::JointState *result) {

  geometry_msgs::PoseStamped pose_msg_in = pose_stamp;
  tf::Stamped<tf::Pose> transform;
  tf::Stamped<tf::Pose> transform_root;
  tf::poseStampedMsgToTF(pose_msg_in, transform);

  //Do the IK
  KDL::JntArray jnt_pos_in;
  KDL::JntArray jnt_pos_out;

  jnt_pos_in.resize(num_joints);
  // Copying the positions of the joints relative to its index in the KDL chain
  for (unsigned int i = 0; i < num_joints; i++) {
    int tmp_index = getJointIndex(seed.name[i]);
    if (tmp_index >= 0) {
      jnt_pos_in(tmp_index) = seed.position[i];
    } else {
      ROS_ERROR("i: %d, No joint index for %s", i, seed.name[i].c_str());
    }
  }

  //Convert F to our root_frame
  try {
    tf_listener.transformPose(root_name, transform, transform_root);
  } catch (...) {
    ROS_ERROR("Could not transform IK pose to frame: %s", root_name.c_str());
    return false;
  }

  KDL::Frame F_dest;
  tf::transformTFToKDL(transform_root, F_dest);

  int ik_valid = ik_solver_pos->CartToJnt(jnt_pos_in, F_dest, jnt_pos_out);

  if (ik_valid >= 0) {
    result->name = info.joint_names;
    result->position.resize(num_joints);
    for (unsigned int i = 0; i < num_joints; i++) {
      result->position[i] = jnt_pos_out(i);
      ROS_DEBUG("IK Solution: %s %d: %f", result->name[i].c_str(), i,
                jnt_pos_out(i));
    }
    return true;
  } else {
    ROS_DEBUG("An IK solution could not be found");
    return false;
  }
}
 void reorderJointState(sensor_msgs::JointState &joint_state, 
                        const kinematics_msgs::KinematicSolverInfo &chain_info)
 {
   sensor_msgs::JointState  tmp_state = joint_state;
   for(unsigned int i=0; i < joint_state.position.size(); i++)
   {
     int tmp_index = getJointIndex(joint_state.name[i],chain_info);
     if(tmp_index >=0)
     {
       tmp_state.position[tmp_index] = joint_state.position[i];
       tmp_state.name[tmp_index] = joint_state.name[i];
     }
   }
   joint_state = tmp_state;
 }
Пример #9
0
Joint* Robot::getJoint(const std::string &name)
{
  int i = getJointIndex(name);
  return i >= 0 ? joints_[i] : NULL;
}
Пример #10
0
bool Model::isJointNameUsed(const std::string jointName)
{
    return (JOINT_INVALID_INDEX != getJointIndex(jointName));
}
Пример #11
0
bool TreeKinematics::getPositionIk(tree_kinematics::get_tree_position_ik::Request &request,
                                   tree_kinematics::get_tree_position_ik::Response &response)
{
  ik_srv_duration_ = ros::Time::now().toSec();
  ROS_DEBUG("getPositionIK method invoked.");

  // extract current joint positions from the request
  KDL::JntArray q, q_desi;
  double nr_of_jnts = request.pos_ik_request[0].ik_seed_state.joint_state.name.size();
  q.resize(nr_of_jnts);
  q_desi.resize(nr_of_jnts);
  for (unsigned int i=0; i < nr_of_jnts; ++i)
  {
    int tmp_index = getJointIndex(request.pos_ik_request[0].ik_seed_state.joint_state.name[i]);
    if (tmp_index >=0)
    {
      q(tmp_index) = request.pos_ik_request[0].ik_seed_state.joint_state.position[i];
      ROS_DEBUG("joint '%s' is now number '%d'", request.pos_ik_request[0].ik_seed_state.joint_state.name[i].c_str(),
                                                tmp_index);
    }
    else
    {
      ROS_FATAL("i: %d, No joint index for %s!",i,request.pos_ik_request[0].ik_seed_state.joint_state.name[i].c_str());
      ROS_FATAL("Service call aborted.");
      return false;
    }
  }

  // convert pose messages into transforms from the root frame to the given poses and further into KDL::Frames
  geometry_msgs::PoseStamped pose_msg_in;
  geometry_msgs::PoseStamped pose_msg_transformed;
  tf::Stamped<tf::Pose> transform;
  tf::Stamped<tf::Pose> transform_root;
  KDL::Frames desired_poses;
  KDL::Frame desired_pose;
  for(unsigned int i = 0; i < request.pos_ik_request.size(); ++i)
  {
    pose_msg_in = request.pos_ik_request[i].pose_stamped;
    try
    {
      tf_listener_.waitForTransform(tree_root_name_, pose_msg_in.header.frame_id, pose_msg_in.header.stamp,
      ros::Duration(0.1));
      tf_listener_.transformPose(tree_root_name_, pose_msg_in, pose_msg_transformed);
    }
    catch (tf::TransformException const &ex)
    {
      ROS_FATAL("%s",ex.what());
      ROS_FATAL("Could not transform IK pose from '%s' to frame '%s'", tree_root_name_.c_str(),
      pose_msg_in.header.frame_id.c_str());
      response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
      return false;
    }
    tf::PoseMsgToKDL(pose_msg_transformed.pose, desired_pose);
    desired_poses[request.pos_ik_request[i].ik_link_name] = desired_pose;
  }

  // use the solver to compute desired joint positions
  ik_duration_ = ros::Time::now().toSec();
  int ik_ret = ik_pos_solver_->CartToJnt_it(q, desired_poses, q_desi); // NOTE: Before it was CartToJnt (without the _it). What's the difference?
  ik_duration_ = ros::Time::now().toSec() - ik_duration_;
  ik_duration_median_ = ((ik_duration_median_ * (loop_count_ - 1)) + ik_duration_) / loop_count_;

  // insert the solver's result into the service response
  if (ik_ret >= 0 || ik_ret == -3)
  {
    response.solution.joint_state.name = info_.joint_names;
    response.solution.joint_state.position.resize(nr_of_jnts_);
    response.solution.joint_state.velocity.resize(nr_of_jnts_);
    for (unsigned int i=0; i < nr_of_jnts_; ++i)
    {
      response.solution.joint_state.position[i] = q_desi(i);
      response.solution.joint_state.velocity[i] = (q_desi(i) - q(i)) * srv_call_frequency_;
      ROS_DEBUG("IK Solution: %s %d: pos = %f, vel = %f",response.solution.joint_state.name[i].c_str(), i,
      response.solution.joint_state.position[i], response.solution.joint_state.velocity[i]);
    }
    response.error_code.val = response.error_code.SUCCESS;

    ik_srv_duration_ = ros::Time::now().toSec() - ik_srv_duration_;
    ik_srv_duration_median_ = ((ik_srv_duration_median_ * (loop_count_ - 1)) + ik_srv_duration_) / loop_count_;
    loop_count_++;

    ROS_INFO_THROTTLE(1.0, "tree_kinematics: ik_srv_duration %f and median %f", ik_srv_duration_,
    ik_srv_duration_median_);
    ROS_INFO_THROTTLE(1.0, "tree_kinematics: ik_duration %f and median %f", ik_duration_, ik_duration_median_);

    if (ik_ret == -3)
    {
      ROS_WARN_THROTTLE(1.0, "Maximum iterations reached! (error code = %d)", ik_ret);
    }
  }
  else
  {
    ROS_WARN("An IK solution could not be found (error code = %d)", ik_ret);
    response.error_code.val = response.error_code.NO_IK_SOLUTION;

    ik_srv_duration_ = ros::Time::now().toSec() - ik_srv_duration_;
    ik_srv_duration_median_ = ((ik_srv_duration_median_ * (loop_count_ - 1)) + ik_srv_duration_) / loop_count_;
    loop_count_++;

    ROS_INFO_THROTTLE(1.0, "tree_kinematics: ik_srv_duration %f and median %f", ik_srv_duration_,
    ik_srv_duration_median_);
    ROS_INFO_THROTTLE(1.0, "tree_kinematics: ik_duration %f and median %f", ik_duration_, ik_duration_median_);
  }
  return true;
}
Пример #12
0
bool TreeKinematics::getPositionFk(kinematics_msgs::GetPositionFK::Request& request,
                                   kinematics_msgs::GetPositionFK::Response& response)
{
  ROS_DEBUG("getPositionFK method invoked.");
  KDL::JntArray q_in;
  double nr_of_jnts = request.robot_state.joint_state.name.size();
  q_in.resize(nr_of_jnts);

  for (unsigned int i=0; i < nr_of_jnts; ++i)
  {
    int tmp_index = getJointIndex(request.robot_state.joint_state.name[i]);
    if (tmp_index >=0)
    {
      q_in(tmp_index) = request.robot_state.joint_state.position[i];
      ROS_DEBUG("joint '%s' is now number '%d'", request.robot_state.joint_state.name[i].c_str(), tmp_index);
    }
    else
    {
      ROS_FATAL("i: %d, No joint index for %s!", i, request.robot_state.joint_state.name[i].c_str());
      ROS_FATAL("Service call aborted.");
      return false;
    }
  }

  response.pose_stamped.resize(request.fk_link_names.size());
  response.fk_link_names.resize(request.fk_link_names.size());
  KDL::Frame p_out;
  geometry_msgs::PoseStamped pose;
  tf::Stamped<tf::Pose> tf_pose;
  for (unsigned int i=0; i < request.fk_link_names.size(); i++)
  {
    ROS_DEBUG("End effector name: %s",request.fk_link_names[i].c_str());
    int fk_ret = fk_solver_->JntToCart(q_in, p_out, request.fk_link_names[i]);
    if (fk_ret >= 0)
    {
      tf_pose.frame_id_ = tree_root_name_;
      tf_pose.stamp_ = ros::Time::now();
      tf::PoseKDLToTF(p_out, tf_pose);
      try
      {
        tf_listener_.transformPose(request.header.frame_id, tf_pose, tf_pose);
      }
      catch (tf::TransformException const &ex)
      {
        ROS_FATAL("Could not transform FK pose to frame: %s",request.header.frame_id.c_str());
        response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
        return false;
      }
      tf::poseStampedTFToMsg(tf_pose, pose);
      response.pose_stamped[i] = pose;
      response.fk_link_names[i] = request.fk_link_names[i];
      response.error_code.val = response.error_code.SUCCESS;
    }
    else
    {
      ROS_WARN("A FK solution could not be found for %s (error code = %d)",
      request.fk_link_names[i].c_str(), fk_ret);
      response.error_code.val = response.error_code.NO_FK_SOLUTION;
    }
  }

  return true;
}