コード例 #1
0
bool ServiceWout::planAndMove()
{
	if(!trigger){
		return false;
	}

	trigger = false;

	ROS_INFO("Setting target pose...");

	ROS_INFO("Reference frame: %s", group->getEndEffectorLink().c_str());

	group->setPoseTarget(stored_pose);

	ROS_INFO("Preparing to plan...");

	moveit::planning_interface::MoveGroup::Plan my_plan;
	group->plan(my_plan);

	sleep(10.0);

	ROS_INFO("Preparing to move...");
  
	group->move();

	return true;
}
コード例 #2
0
ファイル: move_server.cpp プロジェクト: hcrmines/IntBotAPC
bool Actuator::moveToPose(geometry_msgs::Pose goalPose, char armName){
	bool isPlanningSuccess;
	moveit::planning_interface::MoveGroup::Plan my_plan;
	rightArm.setStartState(*rightArm.getCurrentState());
	std::string planningArm = "left_gripper";
	if(armName == amazon::MoveToGoal::RIGHT_ARM){
		planningArm = "right_gripper";
	}
	else if(armName != amazon::MoveToGoal::LEFT_ARM){
		ROS_WARN("Unknown arm received. Using left arm");
	}
	rightArm.setPoseTarget(goalPose, planningArm.c_str());

	// call the planner to compute the plan and visualize it
	isPlanningSuccess = rightArm.plan(my_plan);

	/* *****************  FOR DEBUGGING ***************/
	ROS_INFO("Visualizing plan %s", isPlanningSuccess?"":"FAILED");

	if(isPlanningSuccess){
		//rightArm.execute(my_plan);
		return true;
	}
	else{
		return false;
	}
}
コード例 #3
0
  void leapmotionCallback(const leap_motion::leapros2::ConstPtr& dataHand)
{
      dataHand_=(*dataHand);
      
      
      //ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED");
      
      // Both limits for x,y,z to avoid small changes
      Updifferencex=dataLastHand_.palmpos.x+0.2;
      Downdifferencex=dataLastHand_.palmpos.x-0.2;
      Updifferencez=dataLastHand_.palmpos.z+0.3;
      Downdifferencez=dataLastHand_.palmpos.z-0.2;
      Updifferencey=dataLastHand_.palmpos.y+0.5;
      Downdifferencey=dataLastHand_.palmpos.y-0.5;
      //joint_msg_leap.header.stamp = ros::Time::now();
      
      if ((dataHand_.palmpos.x<Downdifferencex)||(dataHand_.palmpos.x>Updifferencex)||(dataHand_.palmpos.y<Downdifferencey)||(dataHand_.palmpos.y>Updifferencey)||(dataHand_.palmpos.z<Downdifferencez)||(dataHand_.palmpos.z>Updifferencez))
      {
      ros::AsyncSpinner spinner(1);
      spinner.start();
      //target_pose2.position.y +=(dataHand_.palmpos.x-dataLastHand_.palmpos.x)/100 ;
      target_pose2.position.z +=(dataHand_.palmpos.y-dataLastHand_.palmpos.y)/100 ;
      //target_pose2.position.x +=-(dataHand_.palmpos.z-dataLastHand_.palmpos.z)/100 ;
      if(target_pose2.position.z>Uplimitez)target_pose2.position.z=Uplimitez;
      if(target_pose2.position.z<Downlimitez)target_pose2.position.z=Downlimitez;
      //target_pose2.position.z += 0.2
      
      pgroup->setPoseTarget(target_pose2);
      bool success = pgroup->plan(*pplan);
      //pgroup->setStartStateToCurrentState();

      //we change the initial state
      //robot_state::RobotState start_state(*pgroup->getCurrentState());
      //ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED");
      //target_pose2.position.x=dataHand_.palmpos.x-dataLastHand_.palmpos.x;
      //target_pose2.position.y=dataHand_.palmpos.y-dataLastHand_.palmpos.y;
      //target_pose2.position.z=dataHand_.palmpos.z-dataLastHand_.palmpos.z;*/
      //ROS_INFO("Move ARM");
      //sleep(5.0);
    /*const robot_state::JointModelGroup *joint_model_group =
    start_state.getJointModelGroup(pgroup->getName());
    if(start_state.setFromIK(joint_model_group, target_pose2))
    {
    ROS_INFO("ik calculated");
    sleep (1);
    }/*
    

      pgroup->setStartState(start_state);
      spinner.stop();
      
      //group.setPoseTarget(target_pose2);
      /*moveit::planning_interface::MoveGroup::Plan my_plan;
      bool success = group.plan(my_plan);
      
      
      */
      }
      else
      {
      //printf("Palmpos \n X: %f\n  Y: %f\n Z: %f\n ",dataHand_.palmpos.x,dataHand_.palmpos.y,dataHand_.palmpos.z);
      }
      //save new value of the last position of the hand
       dataLastHand_=(*dataHand);

      //we print the position of all the hand
      
      //}

}