예제 #1
0
controllerErrorCode ArmController::setHandCartesianPosLinear(double x, double y, double z, double roll, double pitch, double yaw, double timestep, double stepSize)
{

  double pos;
  //Set up controller
  setMode(CONTROLLER_POSITION);
  this->timeStep = timestep;
  this->stepSize = stepSize;

  //Calculate end position
  KDL::Vector endPos(x,y,z);
  KDL::Rotation endRot;
  endRot = endRot.RPY(roll,pitch,yaw);
  endFrame.p = endPos;
  endFrame.M = endRot;  

  //Get current location
  KDL::JntArray q = KDL::JntArray(pr2_kin.nJnts);

  //Get joint positions
  for(int i=0;i<ARM_MAX_JOINTS;i++){   
    armJointControllers[i].getPosAct(&pos);
    q(i) = pos;
  }

  KDL::Frame f;
  pr2_kin.FK(q,f);

  startPosition = f.p; //Record Starting location
  KDL::Vector move = endPos-startPosition; //Vector to ending position

  double dist = move.Norm(); //Distance to move
  moveDirection = move/dist; //Normalize movement vector
    
  rotInterpolator.SetStartEnd(f.M, endRot);
  double total_angle = rotInterpolator.Angle();

  nSteps = (int)(dist/stepSize);
  if(nSteps==0){
    std::cout<<"ArmController.cpp: Error:: number of steps calculated to be 0"<<std::endl;
    return CONTROLLER_COMPUTATION_ERROR;
  } 
  angleStep = total_angle/nSteps;
  lastTime= getTime(); //Record first time marker
  stepIndex = 0; //Reset step index

  linearInterpolation = true;
  return CONTROLLER_ALL_OK;
}
void UrdfModelMarker::setJointAngle(boost::shared_ptr<const Link> link, double joint_angle){
  string link_frame_name_ =  tf_prefix_ + link->name;
  boost::shared_ptr<Joint> parent_joint = link->parent_joint;

  if(parent_joint == NULL){
    return;
  }

  KDL::Frame initialFrame;
  KDL::Frame presentFrame;
  KDL::Rotation rot;
  KDL::Vector rotVec;
  KDL::Vector jointVec;

  std_msgs::Header link_header;

  int rotation_count = 0;
  
  switch(parent_joint->type){
  case Joint::REVOLUTE:
  case Joint::CONTINUOUS:
    {
      if(joint_angle > M_PI){
	rotation_count = (int)((joint_angle + M_PI) / (M_PI * 2));
	joint_angle -= rotation_count * M_PI * 2;
      }else if(joint_angle < -M_PI){
	rotation_count = (int)((- joint_angle + M_PI) / (M_PI * 2));
	joint_angle -= rotation_count * M_PI * 2;
      }
      linkProperty *link_property = &linkMarkerMap[link_frame_name_];
      link_property->joint_angle = joint_angle;
      link_property->rotation_count = rotation_count;

      tf::poseMsgToKDL (link_property->initial_pose, initialFrame);
      tf::poseMsgToKDL (link_property->initial_pose, presentFrame);
      jointVec = KDL::Vector(link_property->joint_axis.x,
			     link_property->joint_axis.y,
			     link_property->joint_axis.z);

      presentFrame.M = KDL::Rotation::Rot(jointVec, joint_angle) * initialFrame.M;
      tf::poseKDLToMsg(presentFrame, link_property->pose);

      break;
    }
  case Joint::PRISMATIC:
    {
      linkProperty *link_property = &linkMarkerMap[link_frame_name_];
      link_property->joint_angle = joint_angle;
      link_property->rotation_count = rotation_count;
      tf::poseMsgToKDL (link_property->initial_pose, initialFrame);
      tf::poseMsgToKDL (link_property->initial_pose, presentFrame);
      jointVec = KDL::Vector(link_property->joint_axis.x,
			     link_property->joint_axis.y,
			     link_property->joint_axis.z);
      jointVec = jointVec / jointVec.Norm(); // normalize vector
      presentFrame.p = joint_angle * jointVec + initialFrame.p;
      tf::poseKDLToMsg(presentFrame, link_property->pose);
      break;
    }
  default:
    break;
  }

  link_header.stamp = ros::Time(0);
  link_header.frame_id = linkMarkerMap[link_frame_name_].frame_id;

  server_->setPose(link_frame_name_, linkMarkerMap[link_frame_name_].pose, link_header);
  //server_->applyChanges();
  callSetDynamicTf(linkMarkerMap[link_frame_name_].frame_id, link_frame_name_, Pose2Transform(linkMarkerMap[link_frame_name_].pose));
}
void UrdfModelMarker::getJointState(boost::shared_ptr<const Link> link)
{
  string link_frame_name_ =  tf_prefix_ + link->name;
  boost::shared_ptr<Joint> parent_joint = link->parent_joint;
  if(parent_joint != NULL){
    KDL::Frame initialFrame;
    KDL::Frame presentFrame;
    KDL::Rotation rot;
    KDL::Vector rotVec;
    KDL::Vector jointVec;
    double jointAngle;
    double jointAngleAllRange;
    switch(parent_joint->type){
    case Joint::REVOLUTE:
    case Joint::CONTINUOUS:
      {
	linkProperty *link_property = &linkMarkerMap[link_frame_name_];
	tf::poseMsgToKDL (link_property->initial_pose, initialFrame);
	tf::poseMsgToKDL (link_property->pose, presentFrame);
	rot = initialFrame.M.Inverse() * presentFrame.M;
	jointAngle = rot.GetRotAngle(rotVec);
	jointVec = KDL::Vector(link_property->joint_axis.x,
			       link_property->joint_axis.y,
			       link_property->joint_axis.z);
	if( KDL::dot(rotVec,jointVec) < 0){
	  jointAngle = - jointAngle;
	}
	if(link_property->joint_angle > M_PI/2 && jointAngle < -M_PI/2){
	  link_property->rotation_count += 1;
	}else if(link_property->joint_angle < -M_PI/2 && jointAngle > M_PI/2){
	  link_property->rotation_count -= 1;
	}
	link_property->joint_angle = jointAngle;
	jointAngleAllRange = jointAngle + link_property->rotation_count * M_PI * 2;

	if(parent_joint->type == Joint::REVOLUTE && parent_joint->limits != NULL){
	  bool changeMarkerAngle = false;
	  if(jointAngleAllRange < parent_joint->limits->lower){
	    jointAngleAllRange = parent_joint->limits->lower + 0.001;
	    changeMarkerAngle = true;
	  }
	  if(jointAngleAllRange > parent_joint->limits->upper){
	    jointAngleAllRange = parent_joint->limits->upper - 0.001;
	    changeMarkerAngle = true;
	  }

	  if(changeMarkerAngle){
	    setJointAngle(link, jointAngleAllRange);
	  }
	}

	joint_state_.position.push_back(jointAngleAllRange);
	joint_state_.name.push_back(parent_joint->name);
	break;
      }
    case Joint::PRISMATIC:
      {
	KDL::Vector pos;
	linkProperty *link_property = &linkMarkerMap[link_frame_name_];
	tf::poseMsgToKDL (link_property->initial_pose, initialFrame);
	tf::poseMsgToKDL (link_property->pose, presentFrame);

	pos = presentFrame.p - initialFrame.p;

	jointVec = KDL::Vector(link_property->joint_axis.x,
			       link_property->joint_axis.y,
			       link_property->joint_axis.z);
	jointVec = jointVec / jointVec.Norm(); // normalize vector
	jointAngle = KDL::dot(jointVec, pos);

	link_property->joint_angle = jointAngle;
	jointAngleAllRange = jointAngle;

	if(parent_joint->type == Joint::PRISMATIC && parent_joint->limits != NULL){
	  bool changeMarkerAngle = false;
	  if(jointAngleAllRange < parent_joint->limits->lower){
	    jointAngleAllRange = parent_joint->limits->lower + 0.003;
	    changeMarkerAngle = true;
	  }
	  if(jointAngleAllRange > parent_joint->limits->upper){
	    jointAngleAllRange = parent_joint->limits->upper - 0.003;
	    changeMarkerAngle = true;
	  }
	  if(changeMarkerAngle){
	    setJointAngle(link, jointAngleAllRange);
	  }
	}

	joint_state_.position.push_back(jointAngleAllRange);
	joint_state_.name.push_back(parent_joint->name);
	break;
      }
    case Joint::FIXED:
      break;
    default:
      break;
    }
    server_->applyChanges();
  }

  for (std::vector<boost::shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++){
    getJointState(*child);
  }
  return;
}