Ejemplo n.º 1
0
kinematic_constraints::ConstraintEvaluationResult kinematic_constraints::JointConstraint::decide(const planning_models::KinematicState &state, bool verbose) const
{
  if (!joint_model_)
    return ConstraintEvaluationResult(true, 0.0);
  
  const planning_models::KinematicState::JointState *joint = state.getJointState(joint_model_->getName());
  
  if (!joint)
  {
    ROS_WARN_STREAM("No joint in state with name '" << joint_model_->getName() << "'"); 
    return ConstraintEvaluationResult(true, 0.0);
  }
  
  double current_joint_position = joint->getVariableValues()[0];
  if (!local_variable_name_.empty())
  {
    const std::map<std::string, unsigned int> &index_map = joint->getVariableIndexMap();
    std::map<std::string, unsigned int>::const_iterator it = index_map.find(joint_variable_name_);
    if (it == index_map.end())
    {   
      ROS_WARN_STREAM("Local name '" << local_variable_name_ << "' is not known to joint state with name '" << joint_model_->getName() << "'");
      return ConstraintEvaluationResult(true, 0.0);
    }
    else
      current_joint_position = joint->getVariableValues()[it->second];
  }
  
  double dif = 0.0;
  
  // compute signed shortest distance for continuous joints
  if (joint_is_continuous_)
  {
    dif = normalizeAngle(current_joint_position) - joint_position_;
    
    if (dif > boost::math::constants::pi<double>())
      dif = 2.0*boost::math::constants::pi<double>() - dif;
    else
      if (dif < -boost::math::constants::pi<double>())
        dif += 2.0*boost::math::constants::pi<double>(); // we include a sign change to have dif > 0
    // however, we want to include proper sign for diff, as the tol below is may be different from tol above
    if (current_joint_position < joint_position_)
      dif = -dif;
  }
  else
    dif = current_joint_position - joint_position_;
  
  // check bounds
  bool result = dif <= joint_tolerance_above_ && dif >= -joint_tolerance_below_;
  if (verbose)
    ROS_INFO("Constraint %s:: Joint name: '%s', actual value: %f, desired value: %f, tolerance_above: %f, tolerance_below: %f",
             result ? "satisfied" : "violated", joint_variable_name_.c_str(),
             current_joint_position, joint_position_, joint_tolerance_above_, joint_tolerance_below_);
  return ConstraintEvaluationResult(result, constraint_weight_ * fabs(dif));
}
//returns true if the joint_state_map sets all the joints in the state, 
bool planning_environment::setRobotStateAndComputeTransforms(const arm_navigation_msgs::RobotState &robot_state,
                                                             planning_models::KinematicState& state)
{
  if(robot_state.joint_state.name.size() != robot_state.joint_state.position.size()) {
    ROS_INFO_STREAM("Different number of names and positions: " << robot_state.joint_state.name.size() 
                    << " " << robot_state.joint_state.position.size());
    return false;
  }
  std::map<std::string, double> joint_state_map;
  for (unsigned int i = 0 ; i < robot_state.joint_state.name.size(); ++i)
  {
    joint_state_map[robot_state.joint_state.name[i]] = robot_state.joint_state.position[i];
  }
  std::vector<std::string> missing_states;
  bool complete = state.setKinematicState(joint_state_map, missing_states);
  if(!complete) {
    if(missing_states.empty()) {
      ROS_INFO("Incomplete, but no missing states");
    }
  }
  std::map<std::string, bool> has_missing_state_map;
  for(unsigned int i = 0; i < missing_states.size(); i++) {
    has_missing_state_map[missing_states[i]] = false;
  }
  bool need_to_update = false;
  if(robot_state.multi_dof_joint_state.joint_names.size() > 1) {
    ROS_INFO("More than 1 joint names");
  }
  for(unsigned int i = 0; i < robot_state.multi_dof_joint_state.joint_names.size(); i++) {
    std::string joint_name = robot_state.multi_dof_joint_state.joint_names[i];
    if(!state.hasJointState(joint_name)) {
      ROS_WARN_STREAM("No joint matching multi-dof joint " << joint_name);
      continue;
    }
    planning_models::KinematicState::JointState* joint_state = state.getJointState(joint_name);
    if(robot_state.multi_dof_joint_state.frame_ids.size() <= i) {
      ROS_INFO_STREAM("Robot state msg had bad multi_dof transform - not enough frame ids");
    } else if(robot_state.multi_dof_joint_state.child_frame_ids.size() <= i) {
      ROS_INFO_STREAM("Robot state msg had bad multi_dof transform - not enough child frame ids");
    } else if(robot_state.multi_dof_joint_state.frame_ids[i] != joint_state->getParentFrameId() ||
              robot_state.multi_dof_joint_state.child_frame_ids[i] != joint_state->getChildFrameId()) {
      ROS_WARN_STREAM("Robot state msg has bad multi_dof transform - frame ids or child frame_ids do not match up with joint");
    } else {
      tf::StampedTransform transf;
      tf::poseMsgToTF(robot_state.multi_dof_joint_state.poses[i], transf);
      joint_state->setJointStateValues(transf);
      need_to_update = true;
      //setting true for all individual joint names because we're setting the transform
      for(unsigned int i = 0; i < joint_state->getJointStateNameOrder().size(); i++) {
        has_missing_state_map[joint_state->getJointStateNameOrder()[i]] = true;
      }
    }
  }
  if(need_to_update) {
    state.updateKinematicLinks();
  }
  if(!complete) {
    for(std::map<std::string, bool>::iterator it = has_missing_state_map.begin();
        it != has_missing_state_map.end();
        it++) {
      if(!it->second) {
        return false;
      }
    }
    return true;
  }
  return true;
}