C++ (Cpp) move_arm Examples

Example #1

File: move_arm_joint_goal.cpp Project: bajo/uashh-rvl-ros-pkg

int main(int argc, char **argv){
	ros::init (argc, argv, "move_arm_joint_goal_test");
	ros::NodeHandle nh;
	actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm("move_SchunkArm",true); //move_right_arm

	move_arm.waitForServer();
	ROS_INFO("Connected to server");

	arm_navigation_msgs::MoveArmGoal goalB;
	std::vector<std::string> names(5); // don't forget this number
	names[0] = "arm_joint_1";
	names[1] = "arm_joint_2";
	names[2] = "arm_joint_3";
	names[3] = "arm_joint_4";
	names[4] = "arm_joint_5";

	goalB.motion_plan_request.group_name = "SchunkArm";
	goalB.motion_plan_request.num_planning_attempts = 1;
	goalB.motion_plan_request.allowed_planning_time = ros::Duration(5.0);

	goalB.motion_plan_request.planner_id = std::string("");
	goalB.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
	goalB.motion_plan_request.goal_constraints.joint_constraints.resize(names.size());

	/* joint_constraints : Each joint constraints is specified with a joint_name,
	the goal position that we want the joint to reach and
	a tolerance above and below this position that we are willing to accept.
	Thus, the accepted range of tolerances is [position-tolerance_below,position+tolerance_above]
	from wiki */
	for (unsigned int i = 0 ; i < goalB.motion_plan_request.goal_constraints.joint_constraints.size(); ++i)
	{
		goalB.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = names[i];
		goalB.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0;
		goalB.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.01;
		goalB.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.01;
	}
//	goalB.motion_plan_request.goal_constraints.joint_constraints[0].position = -2.0;

	if (nh.ok())
	{
		bool finished_within_time = false;
		move_arm.sendGoal(goalB);
		finished_within_time = move_arm.waitForResult(ros::Duration(200.0));
		if (!finished_within_time)
		{
			move_arm.cancelGoal();
			ROS_INFO("Timed out achieving goal A");
		}
		else
		{
			actionlib::SimpleClientGoalState state = move_arm.getState();
			bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
			if(success)
				ROS_INFO("Action finished: %s",state.toString().c_str());
			else
				ROS_INFO("Action failed: %s",state.toString().c_str());
		}
	}
	ros::shutdown();
}

Example #2

File: move_arm_manual2.cpp Project: meghag/duplo_v0

int main(int argc, char **argv){
  ros::init (argc, argv, "move_arm_pose_goal_test");
  ros::NodeHandle nh;

  actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm("move_right_arm",true);
  move_arm.waitForServer();
  ROS_INFO("Connected to server");
  arm_navigation_msgs::MoveArmGoal goalA;

  goalA.motion_plan_request.group_name = "right_arm";
  goalA.motion_plan_request.num_planning_attempts = 1;
  goalA.motion_plan_request.planner_id = std::string("");
  goalA.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
  goalA.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
  
  arm_navigation_msgs::SimplePoseConstraint desired_pose;
  desired_pose.header.frame_id = "torso_lift_link";
  desired_pose.link_name = "r_wrist_roll_link";
  desired_pose.pose.position.x = 0.6; //0.6;
  desired_pose.pose.position.y = -0.6;//-0.5;
  desired_pose.pose.position.z = -0.1;//0

  desired_pose.pose.orientation.x = 0.0;
  desired_pose.pose.orientation.y = 0.0;
  desired_pose.pose.orientation.z = 0.0;
  desired_pose.pose.orientation.w = 1.0;

  desired_pose.absolute_position_tolerance.x = 0.02;
  desired_pose.absolute_position_tolerance.y = 0.02;
  desired_pose.absolute_position_tolerance.z = 0.02;

  desired_pose.absolute_roll_tolerance = 0.04;
  desired_pose.absolute_pitch_tolerance = 0.04;
  desired_pose.absolute_yaw_tolerance = 0.04;
  
  arm_navigation_msgs::addGoalConstraintToMoveArmGoal(desired_pose,goalA);

  if (nh.ok())
  {
	bool finished_within_time = false;
    move_arm.sendGoal(goalA);
    finished_within_time = move_arm.waitForResult(ros::Duration(200.0));
    if (!finished_within_time)
    {
      move_arm.cancelGoal();
      ROS_INFO("Timed out achieving goal A");
    }
    else
    {
      actionlib::SimpleClientGoalState state = move_arm.getState();
      bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
      if(success)
        ROS_INFO("Action finished: %s",state.toString().c_str());
      else
        ROS_INFO("Action failed: %s",state.toString().c_str());
    }
  }
  ros::shutdown();
}

Example #3

File: regression_test_invalid_joint_goal.cpp Project: Arbart/cob_manipulation_sandbox

TEST(MoveArm, goToJointGoal)
{
  ros::NodeHandle nh;
  ros::NodeHandle private_handle("~");
  actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm(nh, "move_right_arm");
  boost::thread spin_thread(&spinThread);

  move_arm.waitForServer();
  ROS_INFO("Connected to server");
  
  arm_navigation_msgs::MoveArmGoal goalB;
  std::vector<std::string> names(7);
  names[0] = "r_shoulder_pan_joint";
  names[1] = "r_shoulder_lift_joint";
  names[2] = "r_upper_arm_roll_joint";
  names[3] = "r_elbow_flex_joint";
  names[4] = "r_forearm_roll_joint";
  names[5] = "r_wrist_flex_joint";
  names[6] = "r_wrist_roll_joint";

  goalB.motion_plan_request.group_name = "right_arm";
  private_handle.param<std::string>("planner_id",goalB.motion_plan_request.planner_id,std::string("chomp_planner_longrange"));
  private_handle.param<std::string>("planner_service_name",goalB.planner_service_name,std::string("/chomp_planner_longrange/plan_path"));
    
  goalB.motion_plan_request.goal_constraints.joint_constraints.resize(names.size());
  for (unsigned int i = 0 ; i < goalB.motion_plan_request.goal_constraints.joint_constraints.size(); ++i)
    {
      //      goalB.motion_plan_request.goal_constraints.joint_constraints[i].header.stamp = ros::Time::now();
      //      goalB.motion_plan_request.goal_constraints.joint_constraints[i].header.frame_id = "base_link";
      goalB.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = names[i];
      goalB.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0;
      goalB.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.1;
      goalB.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.1;
    }
    
  goalB.motion_plan_request.goal_constraints.joint_constraints[0].position = -2.0;
  goalB.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2;
  goalB.motion_plan_request.goal_constraints.joint_constraints[5].position = 0.15;
   
  if (nh.ok())
    {
      bool finished_within_time = false;
      move_arm.sendGoal(goalB);
      finished_within_time = move_arm.waitForResult(ros::Duration(10.0));
      actionlib::SimpleClientGoalState state = move_arm.getState();
      bool success = (state == actionlib::SimpleClientGoalState::ABORTED);
      EXPECT_TRUE(success);
      ROS_INFO("Action finished: %s",state.toString().c_str());
    }
  ros::shutdown();
  spin_thread.join();
}

Example #4

File: initialPose.cpp Project: 1508189250/birl_baxter

int main(int argc, char** argv)
{
  ros::init(argc, argv, "position_controller");
  //ROS_INFO("Main start");  
  
  // Private Namespace
  ros::NodeHandle node("~");
  
  // Instantiate moveBaxter class. 
  position_controller::moveBaxter move_arm(node);
  ros::Duration(1.0).sleep();

  ros::MultiThreadedSpinner spinner(3);
  spinner.spin();

  return 0;
}

Example #5

File: move_right_arm_torso_pose_goal.cpp Project: jordi-pages/reem_tutorials

int main(int argc, char **argv)
{
  // Init the ROS node
  ros::init (argc, argv, "move_arm_joint_goal_test");

  // Precondition: Valid clock
  ros::NodeHandle nh;
  if (!ros::Time::waitForValid(ros::WallDuration(5.0))) // NOTE: Important when using simulated clock
  {
    ROS_FATAL("Timed-out waiting for valid time.");
    return EXIT_FAILURE;
  }

  // Action client for sending motion planing requests
  actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm("move_right_arm_torso", true);

  // Wait for the action client to be connected to the server
  ROS_INFO("Connecting to server...");
  if (!move_arm.waitForServer(ros::Duration(5.0)))
  {
    ROS_ERROR("Timed-out waiting for the move_arm action server.");
    return EXIT_FAILURE;
  }
  ROS_INFO("Connected to server.");

  // Prepare motion plan request with joint-space goal
  arm_navigation_msgs::MoveArmGoal goal;
  goal.motion_plan_request.group_name = "right_arm_torso";
  goal.motion_plan_request.num_planning_attempts = 3;
  goal.motion_plan_request.planner_id = std::string("");
  goal.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
  goal.motion_plan_request.allowed_planning_time = ros::Duration(5.0);

  arm_navigation_msgs::SimplePoseConstraint desired_pose;
  desired_pose.header.frame_id = "base_link";
  desired_pose.link_name = "arm_right_7_link";
  desired_pose.pose.position.x = 0.30;
  desired_pose.pose.position.y = -0.3;
  desired_pose.pose.position.z = 1.13;

  desired_pose.pose.orientation.x = 0.5;
  desired_pose.pose.orientation.y = -0.5;
  desired_pose.pose.orientation.z = 0.5;
  desired_pose.pose.orientation.w = -0.5;

  desired_pose.absolute_position_tolerance.x = 0.02;
  desired_pose.absolute_position_tolerance.y = 0.02;
  desired_pose.absolute_position_tolerance.z = 0.02;

  desired_pose.absolute_roll_tolerance = 0.05;
  desired_pose.absolute_pitch_tolerance = 0.05;
  desired_pose.absolute_yaw_tolerance = 0.05;

  // Send motion plan request
  arm_navigation_msgs::addGoalConstraintToMoveArmGoal(desired_pose,goal);
  if (nh.ok())
  {
    bool finished_within_time = false;
    move_arm.sendGoal(goal);
    finished_within_time = move_arm.waitForResult(ros::Duration(15.0));
    if (!finished_within_time)
    {
      move_arm.cancelGoal();
      ROS_INFO("Timed out achieving task-space goal.");
    }
    else
    {
      actionlib::SimpleClientGoalState state = move_arm.getState();
      bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
      if(success)
        ROS_INFO("Action finished: %s",state.toString().c_str());
      else
        ROS_INFO("Action failed: %s",state.toString().c_str());
    }
  }
  ros::shutdown();
}

Example #6

File: pick_and_place_size.cpp Project: meghag/duplo_v0

int main(int argc, char **argv)
{
	//initialize the ROS node
	ros::init(argc, argv, "manipulator");
	ros::NodeHandle nh;

	ROS_INFO("Waiting to receive a point cloud to manipulate\n");

	bool pickup_success;
	bool model_fit;
	int model_id;
	Gripper gripper;
    geometry_msgs::Point reset_posn;
	reset_posn.x = 0.6;	reset_posn.y = -0.5; reset_posn.z = 0;
	geometry_msgs::Point red;
	red.x = 0.6;  red.y = -0.5; red.z = -0.1;  //Red bin
	geometry_msgs::Point blue;
	blue.x = 0.6;  blue.y = -0.6; blue.z = -0.1;	//Blue bin
	geometry_msgs::Point green;
	green.x = 0.6; green.y = -0.7; green.z = -0.1;  //Green bin
	geometry_msgs::Point big, medium, small;

	big = red;
	medium = blue;
	small = green;
	
//	const geometry_msgs::Point yellow = {0.45, -0.5, 0};  //Yellow bin
//	const geometry_msgs::Point orange = {0.35, -0.5, 0};  //Orange bin

	int red_color = 0xff0000; 
	const float RGB_RED = *reinterpret_cast<float*>(&red_color);
	int blue_color = 0xff; 
	const float RGB_BLUE = *reinterpret_cast<float*>(&blue_color);
	int green_color = 0xff00; 
	const float RGB_GREEN = *reinterpret_cast<float*>(&green_color);
	
	ros::Subscriber sub = nh.subscribe("pick_place",1,manipulate);
	ros::ServiceClient client_newpcd = 
		nh.serviceClient<duplo_v0::Get_New_PCD>("get_new_pcd");
    duplo_v0::Get_New_PCD srv_newpcd;

	//set service and action names
	const std::string OBJECT_DETECTION_SERVICE_NAME = "/object_detection";
	const std::string COLLISION_PROCESSING_SERVICE_NAME = 
		"/tabletop_collision_map_processing/tabletop_collision_map_processing";
	const std::string PICKUP_ACTION_NAME = 
		"/object_manipulator/object_manipulator_pickup";
	//const std::string PLACE_ACTION_NAME = 
	//	"/object_manipulator/object_manipulator_place";
	const std::string MODEL_FITTING_SERVICE_NAME = 
		"/object_recognition";

	//create service and action clients
	ros::ServiceClient object_detection_srv;
	ros::ServiceClient collision_processing_srv;
	actionlib::SimpleActionClient<object_manipulation_msgs::PickupAction> 
		pickup_client(PICKUP_ACTION_NAME, true);
	//	actionlib::SimpleActionClient<object_manipulation_msgs::PlaceAction> 
	//		place_client(PLACE_ACTION_NAME, true);
	ros::ServiceClient model_fitting_srv;

	//wait for detection client
	while ( !ros::service::waitForService(OBJECT_DETECTION_SERVICE_NAME, 
	                                      ros::Duration(2.0)) && nh.ok() ) 
	{
		ROS_INFO("Waiting for object detection service to come up");
	}
	if (!nh.ok()) exit(0);
	object_detection_srv = 
		nh.serviceClient<tabletop_object_detector::TabletopDetection>
		(OBJECT_DETECTION_SERVICE_NAME, true);

	//wait for collision map processing client
	while ( !ros::service::waitForService(COLLISION_PROCESSING_SERVICE_NAME, 
	                                      ros::Duration(2.0)) && nh.ok() ) 
	{
		ROS_INFO("Waiting for collision processing service to come up");
	}
	if (!nh.ok()) exit(0);
	collision_processing_srv = 
		nh.serviceClient
		<tabletop_collision_map_processing::TabletopCollisionMapProcessing>
		(COLLISION_PROCESSING_SERVICE_NAME, true);

	//wait for pickup client
	while(!pickup_client.waitForServer(ros::Duration(2.0)) && nh.ok())
	{
		ROS_INFO_STREAM("Waiting for action client " << PICKUP_ACTION_NAME);
	}
	if (!nh.ok()) exit(0);  

	//wait for place client
	/*	while(!place_client.waitForServer(ros::Duration(2.0)) && nh.ok())
	{
		ROS_INFO_STREAM("Waiting for action client " << PLACE_ACTION_NAME);
 	}
	if (!nh.ok()) exit(0);    
	*/

	/*while ( !ros::service::waitForService(MODEL_FITTING_SERVICE_NAME, 
	                                      ros::Duration(2.0)) && nh.ok() ) 
	{
		ROS_INFO("Waiting for model fitting service to come up");
	}
	if (!nh.ok()) exit(0);
	model_fitting_srv = nh.serviceClient<tabletop_object_detector::TabletopObjectRecognition>
		(MODEL_FITTING_SERVICE_NAME, true);
*/
	while (ros::ok() && start_manipulation == false) {
		ros::spinOnce();
		if (start_manipulation==true) {
			start_manipulation = false;

			//call the tabletop detection
			ROS_INFO("Calling tabletop detector");
			tabletop_object_detector::TabletopDetection detection_call;
			//we want recognized database objects returned
			//set this to false if you are using the pipeline without the database
			detection_call.request.return_clusters = true;
			//we want the individual object point clouds returned as well
			detection_call.request.return_models = false;
			if (!object_detection_srv.call(detection_call))
			{
				ROS_ERROR("Tabletop detection service failed");
				return false;
			}
			if (detection_call.response.detection.result != 
			    detection_call.response.detection.SUCCESS)
			{
				ROS_ERROR("Tabletop detection returned error code %d", 
				          detection_call.response.detection.result);
				return false;
			}
			if (detection_call.response.detection.clusters.empty() && 
			    detection_call.response.detection.models.empty() )
			{
				ROS_DEBUG("The tabletop detector detected the table, but found no objects");
				//return false;
			}
			detection_call.response.detection.clusters.clear();

			//call collision map processing
			ROS_INFO("Calling collision map processing");
			tabletop_collision_map_processing::TabletopCollisionMapProcessing 
				processing_call;
			//pass the result of the tabletop detection 
			processing_call.request.detection_result = detection_call.response.detection;
			//ask for the exising map and collision models to be reset
			//processing_call.request.reset_static_map = true;
			processing_call.request.reset_collision_models = true;
			processing_call.request.reset_attached_models = true;
			//ask for a new static collision map to be taken with the laser
			//after the new models are added to the environment
			//processing_call.request.take_static_collision_map = false;
			//ask for the results to be returned in base link frame
			processing_call.request.desired_frame = "base_link";
			if (!collision_processing_srv.call(processing_call))
			{
				ROS_ERROR("Collision map processing service failed");
				return false;
			}
			//the collision map processor returns instances of graspable objects
			if (processing_call.response.graspable_objects.empty())
			{
				ROS_DEBUG("Collision map processing returned no graspable objects");
				//return false;
			}

			//call object pickup
			ROS_INFO("Calling the pickup action");
			object_manipulation_msgs::PickupGoal pickup_goal;

			//pass one of the graspable objects returned by the collission map processor

			/*********************************************************************/
			/*********************************************************************/
			/********** Changed *********/
			//pickup_goal.target = processing_call.response.graspable_objects.at(0);

			object_manipulation_msgs::GraspableObject object;
			//object.reference_frame_id = "/openni_rgb_frame";
			sensor_msgs::PointCloud goal_pcd;
			sensor_msgs::PointCloud2 pick_cluster;
			/*toROSMsg(to_pick,pick_cluster);

			tf::TransformListener listener;
			sensor_msgs::PointCloud2 transformed;
			pick_cluster.header.frame_id = "openni_rgb_optical_frame";
			listener.waitForTransform("openni_rgb_optical_frame","base_link",
			                          pick_cluster.header.stamp,ros::Duration(2.0));
			pcl_ros::transformPointCloud("base_link",pick_cluster,transformed,listener);
*/
			if (sensor_msgs::convertPointCloud2ToPointCloud(to_pick,goal_pcd) == true)
			{
				ROS_INFO("Frame Id of input point cloud cluster is: %s\n", goal_pcd.header.frame_id.c_str());
				ROS_INFO("Target frame id is: %s\n", detection_call.response.detection.table.pose.header.frame_id.c_str());
				goal_pcd.header.frame_id = "base_link";
				goal_pcd.header.stamp = ros::Time::now();

				object.cluster = goal_pcd;
				object.reference_frame_id = "base_link";
				pickup_goal.target = object;
				ROS_INFO("Set the goal target as a graspable object\n");

			} else {
				ROS_ERROR("Conversion from pointcloud2 to pointcloud failed.\n");
				return false;
			}

			/**** Fitting a model to goal_pcd ****/
	/*		tabletop_object_detector::TabletopObjectRecognition fitting_call;
			fitting_call.request.clusters.push_back(goal_pcd);
			fitting_call.request.num_models = 1;
			fitting_call.request.perform_fit_merge = false;

			if (!model_fitting_srv.call(fitting_call))
			{
				ROS_ERROR("Model fitting service failed");
				model_fit = false;
				//return false;
			} else {
				model_fit = true;
				model_id = fitting_call.response.cluster_model_indices[0];
			}
*/
			//pass the name that the object has in the collision environment
			//this name was also returned by the collision map processor
			//pickup_goal.collision_object_name = 
			//  processing_call.response.collision_object_names.at(0);
			/*********************************************************************/
			/*********************************************************************/

			//pass the collision name of the table, also returned by the collision 
			//map processor
			pickup_goal.collision_support_surface_name = 
				processing_call.response.collision_support_surface_name;

			/*********************************************************************/
			/*********************************************************************/
			/******* Added: allowing collisions with the table ******/
			pickup_goal.allow_gripper_support_collision = true;
			/*********************************************************************/
			/*********************************************************************/

			//pick up the object with the right arm
			pickup_goal.arm_name = "right_arm";
			//specify the desired distance between pre-grasp and final grasp
			//pickup_goal.desired_approach_distance = 0.1;
			//pickup_goal.min_approach_distance = 0.05;
			//we will be lifting the object along the "vertical" direction
			//which is along the z axis in the base_link frame
			geometry_msgs::Vector3Stamped direction;
			direction.header.stamp = ros::Time::now();
			direction.header.frame_id = "base_link";
			direction.vector.x = 0;
			direction.vector.y = 0;
			direction.vector.z = 1;
			pickup_goal.lift.direction = direction;
			//request a vertical lift of 10cm after grasping the object
			pickup_goal.lift.desired_distance = 0.15;
			pickup_goal.lift.min_distance = 0.1;
			//do not use tactile-based grasping or tactile-based lift
			pickup_goal.use_reactive_lift = false;
			pickup_goal.use_reactive_execution = false;

			//send the goal
			pickup_client.sendGoal(pickup_goal);
			while (!pickup_client.waitForResult(ros::Duration(5.0)))
			{
				ROS_INFO("Waiting for the pickup action...");
			}
			object_manipulation_msgs::PickupResult pickup_result = 
				*(pickup_client.getResult());
			if (pickup_client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
			{
				ROS_ERROR("The pickup action has failed with result code %d", 
				          pickup_result.manipulation_result.value);
				pickup_success = false;
				//return false;
			} 
			else 
			{
				ROS_INFO("The pickup action succeeded.");   
				pickup_success= true;
			}
			if (pickup_success) {
				pcl::PointCloud<pcl::PointXYZRGB> temp;
				fromROSMsg(to_pick,temp);
				float longest_dim = find_longest_dim(temp);
				ROS_INFO("PP: Longest dimension of cluster = %f", longest_dim);				   
				if (longest_dim > 0.09) {
					//Place in size A bin
					ROS_INFO("PP: Placing in big bin");
					move_arm(big);
				} else if (longest_dim >= 0.05 && longest_dim <= 0.08) {
					//Place in size B bin
					ROS_INFO("PP: Placing in medium bin");
					move_arm(medium);
				} else if (longest_dim < 0.05) {
					//Place in size C bin
					ROS_INFO("PP: Placing in small bin");
					move_arm(small);
				} else {
					move_arm(reset_posn);
				}
				gripper.open();
			} else {
				move_arm(reset_posn);
			}
			RobotHead head;
			head.lookAt("base_link", 0.2, 0.0, 1.0);
			gripper.open();

			ros::Duration(2).sleep();
			
			// Ask for new PCD
			srv_newpcd.request.question = true;

			if (client_newpcd.call(srv_newpcd)) {
				ROS_INFO("Requesting for new point cloud.");
			} else {
				ROS_ERROR("Failed to call service get new pcd.");
				return 1;
	  		}
		}
		//ros::spin();
	}
	return 0;
}

Example #7

File: move_arm_simple_pose_goal.cpp Project: nttputus/wp6_manipulator

int main(int argc, char **argv){
  ros::init (argc, argv, "move_arm_pose_goal_test");
  ros::NodeHandle nh;
//
  planning_environment::CollisionModels* collisionModels;
  planning_models::KinematicState* kinematicState;

  collisionModels = new planning_environment::CollisionModels("robot_description");
  kinematicState = new planning_models::KinematicState(collisionModels->getKinematicModel());

  ros::service::waitForService(SET_PLANNING_SCENE_DIFF_NAME);
  ROS_INFO("Waiting for planning scene service");
  ros::ServiceClient set_planning_scene_diff_client = nh.serviceClient<arm_navigation_msgs::SetPlanningSceneDiff>(SET_PLANNING_SCENE_DIFF_NAME);

  set_planning_scene_diff_client.waitForExistence();
  ROS_INFO("Planning scene service is now available");

  arm_navigation_msgs::SetPlanningSceneDiff::Request planning_scene_req;
  arm_navigation_msgs::SetPlanningSceneDiff::Response planning_scene_res;
  planning_environment::convertKinematicStateToRobotState(*kinematicState, ros::Time::now(), collisionModels->getWorldFrameId(), planning_scene_req.planning_scene_diff.robot_state);
  
  if(!set_planning_scene_diff_client.call(planning_scene_req, planning_scene_res)) {
    ROS_WARN("Can't get planning scene");
  }
  ROS_INFO("Successfully set planning scene");

  collisionModels->revertPlanningScene(kinematicState);
  collisionModels->setPlanningScene(planning_scene_res.planning_scene);
//
  actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm("move_arm",true);
  move_arm.waitForServer();
  ROS_INFO("Connected to server");
  arm_navigation_msgs::MoveArmGoal goalA;

  goalA.motion_plan_request.group_name = "arm";
  goalA.motion_plan_request.num_planning_attempts = 1;
  goalA.motion_plan_request.planner_id = std::string("");
  goalA.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
  goalA.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
  
  arm_navigation_msgs::SimplePoseConstraint desired_pose;
  desired_pose.header.frame_id = "base_link";
  desired_pose.link_name = "body9";
  desired_pose.pose.position.x = 0.5;
  desired_pose.pose.position.y = 1.188;
  desired_pose.pose.position.z = 1.3;

  desired_pose.pose.orientation.x = 0.0;
  desired_pose.pose.orientation.y = 0.0;
  desired_pose.pose.orientation.z = 0.0;
  desired_pose.pose.orientation.w = 1.0;

  desired_pose.absolute_position_tolerance.x = 0.02;
  desired_pose.absolute_position_tolerance.y = 0.02;
  desired_pose.absolute_position_tolerance.z = 0.02;

  desired_pose.absolute_roll_tolerance = 0.04;
  desired_pose.absolute_pitch_tolerance = 0.04;
  desired_pose.absolute_yaw_tolerance = 0.04;
  
  arm_navigation_msgs::addGoalConstraintToMoveArmGoal(desired_pose,goalA);

  if (nh.ok())
  {
    bool finished_within_time = false;
    move_arm.sendGoal(goalA);
    finished_within_time = move_arm.waitForResult(ros::Duration(200.0));
    if (!finished_within_time)
    {
      move_arm.cancelGoal();
      ROS_INFO("Timed out achieving goal A");
    }
    else
    {
      actionlib::SimpleClientGoalState state = move_arm.getState();
      bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
      if(success)
        ROS_INFO("Action finished: %s",state.toString().c_str());
      else
        ROS_INFO("Action failed: %s",state.toString().c_str());
    }
  }
  ros::shutdown();
}

Example #8

File: move_left_arm.cpp Project: meghag/object_search

int main(int argc, char** argv)
{

    ros::init(argc, argv, "left_arm_mover");
    ros::NodeHandle n;

    if (argc != 4) {
        ROS_ERROR("Incorrect number of arguments");
        return -1;
    }

    ros::Publisher pose_pub = n.advertise<geometry_msgs::PoseStamped>("reset_position",1);

    //actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm("move_left_arm",true);
    actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm("move_left_arm",true);
    move_arm.waitForServer();
    ROS_INFO("DUPLO: Connected to server");
    arm_navigation_msgs::MoveArmGoal goalA;

    //goalA.motion_plan_request.group_name = "left_arm";
    goalA.motion_plan_request.group_name = "left_arm";
    goalA.motion_plan_request.num_planning_attempts = 1;
    goalA.motion_plan_request.planner_id = std::string("");
    goalA.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
    goalA.motion_plan_request.allowed_planning_time = ros::Duration(5.0);

    arm_navigation_msgs::SimplePoseConstraint desired_pose;
    desired_pose.header.frame_id = "base_link";
    desired_pose.header.stamp = ros::Time::now();
    //desired_pose.link_name = "l_wrist_roll_link";
    desired_pose.link_name = "l_wrist_roll_link";
//	desired_pose.link_name = "r_gripper_palm_link";
    desired_pose.pose.position.x = atof(argv[1]); // = go_to;
    desired_pose.pose.position.y = atof(argv[2]);
    desired_pose.pose.position.z = atof(argv[3]);
//	desired_pose.pose.position.z = go_to.z + 0.194;
    /*
      	desired_pose.pose.position.x =  0.6; //0.75;
    	desired_pose.pose.position.y = -0.5;//-0.188;
    	desired_pose.pose.position.z = 0;
    */
    desired_pose.pose.orientation.x = 0;
    desired_pose.pose.orientation.y = 0;
    desired_pose.pose.orientation.z = 0;
    desired_pose.pose.orientation.w = 1;

    geometry_msgs::PoseStamped to_pub;
    to_pub.pose = desired_pose.pose;
    to_pub.header.frame_id = "base_link"; //desired_pose.header.frame_id;
    to_pub.header.stamp = ros::Time::now();
    pose_pub.publish(to_pub);


    /*
    	desired_pose.pose.orientation.x = -0.74;
    	desired_pose.pose.orientation.y = -0.04;
    	desired_pose.pose.orientation.z = 0.67;
    	desired_pose.pose.orientation.w = -0.04;
    */
    desired_pose.absolute_position_tolerance.x = 0.02;
    desired_pose.absolute_position_tolerance.y = 0.02;
    desired_pose.absolute_position_tolerance.z = 0.02;

    desired_pose.absolute_roll_tolerance = 0.04;
    desired_pose.absolute_pitch_tolerance = 0.04;
    desired_pose.absolute_yaw_tolerance = 0.04;

    arm_navigation_msgs::addGoalConstraintToMoveArmGoal(desired_pose,goalA);


    bool finished_within_time = false;
    move_arm.sendGoal(goalA);
    finished_within_time = move_arm.waitForResult(ros::Duration(100.0));
    if (!finished_within_time) {
        move_arm.cancelGoal();
        ROS_INFO("DUPLO: Timed out achieving goal A");
        return -1;
    } else {
        actionlib::SimpleClientGoalState state = move_arm.getState();
        bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
        if(success) {
            ROS_INFO("DUPLO: Action finished: %s",state.toString().c_str());
            return 0;
        } else {
            ROS_INFO("DUPLO: Action failed: %s",state.toString().c_str());
            return -1;
        }
    }

    ros::spin();

    return 0;
}

Example #9

File: kuka_or.cpp Project: ortslil64/kuka_position_dynamic_controller

int main(int argc, char **argv)
{
int i, j, N=300;
double jj;
float** angles;
double th_time;
double Vc;
Task current_position, Temp;
 Task temp_position;
  ros::init(argc, argv, "kuka_or");

  ros::NodeHandle n;


	j0_pub_  = n.advertise<std_msgs::Float64>("/kuka/kuka_1_controller/command", 10000);//joint 1
	j2_pub_  = n.advertise<std_msgs::Float64>("/kuka/kuka_3_controller/command", 10000);//joint 2
	j4_pub_  = n.advertise<std_msgs::Float64>("/kuka/kuka_5_controller/command", 10000);//joint 3
	j5_pub_  = n.advertise<std_msgs::Float64>("/kuka/kuka_left_finger_controller/command", 10000); //left fingre
	j6_pub_  = n.advertise<std_msgs::Float64>("/kuka/kuka_right_finger_controller/command", 10000);//right fingre
	hight_pub  = n.advertise<std_msgs::Float64>("/kuka_z", 10000);//right fingre
  ros::Subscriber sub_z = n.subscribe("/position_z", 10000,z_callback);
  ros::Subscriber sub_joint = n.subscribe("/kuka/joint_states", 10000,jointCallback);
  ros::Rate loop_rate(30);
  ros::Duration dt_loop(0.001/N);
  ros::Duration pause(0.15);
  ros::Duration open_time(0.01);

std_msgs::Float64 j1;
std_msgs::Float64 j2;
std_msgs::Float64 j3;
std_msgs::Float64 j4;
std_msgs::Float64 j5;
std_msgs::Float64 j6;
std_msgs::Float64 j7;
std_msgs::Float64 z_to_pub;
int stage=0;
/////// avishai's code ///////









while(ros::ok()) {

float q[]={(float)j0_pos_public,(float)j1_pos_public,(float)j2_pos_public};
float dq[3]={0};
current_position=DirectKinematics3R(q,dq);
Vc=VelocityCalc(global_z);
z_to_pub.data=current_position.z;
hight_pub.publish(z_to_pub);

//ROS_INFO("\nx:%f \nz:%f",current_position.x,current_position.z);

if(stage==0)
{
for (jj=0; jj<100 ; jj++)
{
z_to_pub.data=current_position.z;
hight_pub.publish(z_to_pub);
move_arm(0.3+(jj/300),0.05,1.57,0);
loop_rate.sleep();

ros::spinOnce();
}
stage=1;
}
if(stage==1)
{
for (jj=0; jj<50 ; jj++)
{
z_to_pub.data=current_position.z;
hight_pub.publish(z_to_pub);
move_arm(0.6333+(jj/300),0.05,1.57,-0.5);
loop_rate.sleep();
ros::spinOnce();
}
stage=2;
}
if(stage==2)
{
for (jj=0; jj<50 ; jj++)
{
z_to_pub.data=current_position.z;
hight_pub.publish(z_to_pub);
move_arm(0.8,0.05,1.57,-0.5+(jj/30));
loop_rate.sleep();
ros::spinOnce();
}
stage=3;
}
if(stage==3)
{
for (jj=0; jj<100 ; jj++)
{
z_to_pub.data=current_position.z;
hight_pub.publish(z_to_pub);
move_arm(0.8-(jj/800),0.05+(jj/3000),1.57,10);
loop_rate.sleep();
ros::spinOnce();
}
stage=4;
}
//ROS_INFO("\nteta 1:%f \nteta 2 :%f \nteta 3:%f ",j0_pos_public,j1_pos_public,j2_pos_public);




///////brings the arm to catch the object///////
//ROS_INFO("\nteta 1:%f \nteta 2 :%f \nteta 3:%f \nteta 5 :%f \nteta 6:%f \nstage :%d",j0_pos_public,j1_pos_public,j2_pos_public,j3_pos_public,j4_pos_public,stage);



////////////////////





if(change==1 && stage==4){

	angles =  TrajectoryGenerator(N, 0.0005, current_position.x, current_position.z, Vc*50);
////// moving the arm to get specific velocity/////////

	for (i=0; i<N; i++) {
q[0]=(float)j0_pos_public;
q[1]=(float)j1_pos_public;
q[2]=(float)j2_pos_public;
		j1.data=angles[i][0];
		j2.data=angles[i][1];
		j3.data=angles[i][2];
current_position=DirectKinematics3R(q,dq);
		Temp = DirectKinematics3R(angles[i],dq);
		ROS_INFO("\nz:%f   %f",j0_pos_public,j1.data);
		j0_pub_.publish(j1);
		j2_pub_.publish(j2);
		j4_pub_.publish(j3);
		dt_loop.sleep();
		//ROS_INFO("\z:%f ",current_position.z);
                ros::spinOnce();
current_position=DirectKinematics3R(q,dq);

////
z_to_pub.data=current_position.z;
hight_pub.publish(z_to_pub);
/////
if(i>N-50)
{

close_arm(-10);


}


	}

pause.sleep();
close_arm(100);
////////////////////////////////////////////////////

for(i=0;i<100;i++)
{
z_to_pub.data=current_position.z;
hight_pub.publish(z_to_pub);
open_time.sleep();
close_arm(1-0.011*i);
ros::spinOnce();
}
z_to_pub.data=current_position.z;
hight_pub.publish(z_to_pub);
close_arm(100);

////// opening and closing the arm to change the location of the object/////////
/*
close_arm(-100);
th_time=(Vc-sqrt((pow(Vc,2)-2*g*global_z)))/g;
for (i=0; i<th_time; i++) {
pause.sleep();
 ros::spinOnce();
}
ROS_INFO("\nh_time:%f ",th_time);
ros::spinOnce();
close_arm(2);
*/
ros::spinOnce();
change=0;
}

//////////////////////////////////////////////////////////////////////


   
    

    
    
    
  
		
		ros::spinOnce();
	}


  return 0;
}

Example #10

File: move_arm_joint_goal.cpp Project: gt-ros-pkg/hrl

int main(int argc, char **argv){
    ros::init (argc, argv, "move_arm_joint_goal_test");
    ros::NodeHandle nh;
    actionlib::SimpleActionClient<move_arm_msgs::MoveArmAction> move_arm("move_right_arm",true);

    move_arm.waitForServer();
    ROS_INFO("Connected to server");

    move_arm_msgs::MoveArmGoal goalB;
    std::vector<std::string> names(7);
    names[0] = "r_shoulder_pan_joint";
    names[1] = "r_shoulder_lift_joint";
    names[2] = "r_upper_arm_roll_joint";
    names[3] = "r_elbow_flex_joint";
    names[4] = "r_forearm_roll_joint";
    names[5] = "r_wrist_flex_joint";
    names[6] = "r_wrist_roll_joint";

    goalB.motion_plan_request.group_name = "right_arm";
    goalB.motion_plan_request.num_planning_attempts = 1;
    goalB.motion_plan_request.allowed_planning_time = ros::Duration(5.0);

    goalB.motion_plan_request.planner_id= std::string("");
    goalB.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
    goalB.motion_plan_request.goal_constraints.joint_constraints.resize(names.size());

    for (unsigned int i = 0 ; i < goalB.motion_plan_request.goal_constraints.joint_constraints.size(); ++i)
    {
        goalB.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = names[i];
        goalB.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0;
        goalB.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.05;
        goalB.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.05;
    }

    goalB.motion_plan_request.goal_constraints.joint_constraints[0].position = -0.082971740453390508;
    goalB.motion_plan_request.goal_constraints.joint_constraints[1].position = -0.00024050597696160733;
    goalB.motion_plan_request.goal_constraints.joint_constraints[2].position = -1.7613330259531139;
    goalB.motion_plan_request.goal_constraints.joint_constraints[3].position = -1.1626271435626965;
    goalB.motion_plan_request.goal_constraints.joint_constraints[4].position = -0.032142093106232839;
    goalB.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.19405610249061456;
    goalB.motion_plan_request.goal_constraints.joint_constraints[6].position = 0.18618502991750852;



    if (nh.ok())
    {
        bool finished_within_time = false;
        move_arm.sendGoal(goalB);
        finished_within_time = move_arm.waitForResult(ros::Duration(200.0));
        if (!finished_within_time)
        {
            move_arm.cancelGoal();
            ROS_INFO("Timed out achieving goal A");
        }
        else
        {
            actionlib::SimpleClientGoalState state = move_arm.getState();
            bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
            if(success)
                ROS_INFO("Action finished: %s",state.toString().c_str());
            else
                ROS_INFO("Action failed: %s",state.toString().c_str());
        }
    }
    ros::shutdown();
}

Example #11

File: debug.c Project: narumiya/stm32

void debug(void)
{
	robot_information_t robot;
	target_cam_t target_cam;

	unsigned char str[10] = "\0";

	unsigned short 	old_sw = 0,
								start_sw =0,
								old_limit_sw = 0;

	float target_degree = 0.00;

	transmit_usb("[ r : reset] \n\r");
	transmit_usb("[ 1 : encoder]\n\r");
	transmit_usb("[ 2 : AD]\n\r");
	transmit_usb("[ 3 : coordinate]\n\r");
	transmit_usb("[ 4 : sonic_waves]\n\r");
	transmit_usb("[ 5 : encoder reset]\n\r");
	transmit_usb("[ 6 : cam_inf]\n\r");
	transmit_usb("[ 7 : target_cam]\n\r");
	transmit_usb("[ 8 : inf&target]\n\r");
	transmit_usb("[ 9 : start_switch]\n\r");
	transmit_usb("[10 : limit_switch]\n\r");
	transmit_usb("[11 : motor]\n\r");
	transmit_usb("[12 : reverse motor]\n\r");
	transmit_usb("[13 : stop motor]\n\r");
	transmit_usb("[14 : arm motor]\n\r");
	transmit_usb("[15 : reverse arm motor]\n\r");
	//transmit_usb("[p : p gain adjustment]\n\r");
	//transmit_usb("[d : d gain adjustment]\n\r");
	GPIO_ResetBits(ENC_RESET);

	while(strcmp(str, "r") != 0){
		if(usb_available() != 0){
			usb_get_string(str);
		}
		buzzer_stop();
		//sonic_waves(&robot);

		if(count_time(3) >= INTERRUPT_TIME){
			reset_count_time(3);
			get_robot_inf( &robot );
			cam_data(&target_cam, &robot);
			start_sw =positive_chattering(START_SW,1);
			robot.sw.limit_sw = negative_chattering(LIMIT_SW,2);
			robot.ad = get_ADC1_value(0);
		}

		if(strcmp(str, "1") == 0){
			f_print(PC,"ENCL",robot.enc_cnt.l);
			f_print(PC,"ENCR",robot.enc_cnt.r);
			f_print(PC,"ENCF",robot.enc_cnt.f);
			put_enter(PC);

		}else if(strcmp(str, "2") == 0){
			f_print(PC,"AD",robot.ad);
			put_enter(PC);

		}else if(strcmp(str, "3") == 0){
			f2_print(PC,"now_coord",robot.coord.c_x, robot.coord.c_y);
			f_print(PC,"deg",robot.angle.degree);
			put_enter(PC);

		}else if(strcmp(str, "4") == 0){
			f_print(PC,"time",count_time(2));
			f_print(PC,"dis",robot.waves);
			put_enter(PC);

		}else if(strcmp(str, "5") == 0){
			GPIO_SetBits(ENC_RESET);
			//str[0] = 'r';

		}else if(strcmp(str, "6") == 0){
			f2_print(PC, "under", target_cam.under_x, target_cam.under_y );
			f2_print(PC, "over", target_cam.over_x, target_cam.over_y );
			put_enter(PC);

		}else if(strcmp(str, "7") == 0){
			f2_print(PC, "target_cam", (target_cam.x) * cos(D_TO_R(robot.angle.degree))+robot.coord.c_x, (target_cam.y)*sin(D_TO_R(robot.angle.degree))+robot.coord.c_y);
			put_enter(PC);

		}else if(strcmp(str, "8") == 0){
			f2_print(PC, "under", target_cam.under_x, target_cam.under_y);
			f2_print(PC, "over", target_cam.over_x, target_cam.over_y );
			target_degree = get_target_degree(ROBO_TO_CENTER - robot.coord.c_x, robot.coord.c_y);
			f2_print(PC, "target_cam", (target_cam.x) * cos(D_TO_R(target_degree)) + robot.coord.c_x, (target_cam.y) * sin(D_TO_R(target_degree)) + robot.coord.c_y);
			put_enter(PC);

		}else if(strcmp(str, "9") == 0){
			if(robot.sw.start_sw != old_sw ){
				f_print(PC,"sw",start_sw);
				put_enter(PC);
			}

		}else if(strcmp(str, "10") == 0){
			if(robot.sw.limit_sw != old_limit_sw ){
				f_print(PC,"limit_sw",robot.sw.limit_sw);
				put_enter(PC);
			}

		}else if(strcmp(str, "11") == 0){
			move(50, 50, 50);

		}else if(strcmp(str, "12") == 0){
			move(-50, -50, -50);

		}else if(strcmp(str, "13") == 0){
			move(0, 0, 0);
			move_arm(0);

		}else if(strcmp(str, "14") == 0){
			move_arm(100);

		}else if(strcmp(str, "15") == 0){
			move_arm(-100);
		}

		old_limit_sw = robot.sw.start_sw;
		old_sw = start_sw;
	}
}

Example #12

File: move_arm_simple_pose_goal.cpp Project: felix-kolbe/uashh-rvl-ros-pkg

int main(int argc, char **argv) {
    ros::init (argc, argv, "move_arm_pose_goal_test");
    ros::NodeHandle nh;
    actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm("move_SchunkArm",true);
    move_arm.waitForServer();
    ROS_INFO("Connected to server");
    arm_navigation_msgs::MoveArmGoal goalA;

    double	x = 0.5,
            y = 0.1,
            z = 0.5,
            R = -M_PI_2, // greifer 'gerade' drehen
            P = M_PI_2, // M_PI_2 negativ: greifer zeigt nach oben. und umgekehrt
            Y = 0;

    // 0 0 0.6   -M_PI_2  M_PI_4 0
    // 0.3 0 0.4   -M_PI_2  M_PI_2 0
    // 0.5 0 0.3   -M_PI_2  M_PI_2 0
    // 0.4 0.1 0.4   -M_PI_2  M_PI_2 -M_PI_2

    if(argc >= 1+6) { // cmd name + args
        x = parseArg(argv[1]);
        y = parseArg(argv[2]);
        z = parseArg(argv[3]);
        R = parseArg(argv[4]);
        P = parseArg(argv[5]);
        Y = parseArg(argv[6]);
    }

    ROS_INFO_STREAM("x="<<x<<" y="<<y<<" z="<<z<<" R="<<R<<" P="<<P<<" Y="<<Y);

    goalA.motion_plan_request.group_name = "SchunkArm";
    goalA.motion_plan_request.num_planning_attempts = 2;
    goalA.motion_plan_request.planner_id = std::string("");
    goalA.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
    goalA.motion_plan_request.allowed_planning_time = ros::Duration(5.0);

    arm_navigation_msgs::SimplePoseConstraint desired_pose;
    desired_pose.header.frame_id = "arm_base_link";
    desired_pose.link_name = "gripper";
    desired_pose.pose.position.x = x;
    desired_pose.pose.position.y = y;
    desired_pose.pose.position.z = z;

    double yaw = tan(desired_pose.pose.position.y / desired_pose.pose.position.x);
    ROS_INFO_STREAM("yaw="<<yaw);

    if(argc >= 1+7 && !strcmp(argv[7], "calcyaw")) {
        ROS_INFO("Using calculated yaw");
        Y = yaw;
    }

    desired_pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(R, P, Y);

    ROS_INFO_STREAM(desired_pose.pose.orientation);


    desired_pose.absolute_position_tolerance.x = 0.02;
    desired_pose.absolute_position_tolerance.y = 0.02;
    desired_pose.absolute_position_tolerance.z = 0.02;

    desired_pose.absolute_roll_tolerance = 0.04;
    desired_pose.absolute_pitch_tolerance = 0.04;
    desired_pose.absolute_yaw_tolerance = 0.04;

    arm_navigation_msgs::addGoalConstraintToMoveArmGoal(desired_pose,goalA);

    if (nh.ok())
    {
        bool finished_within_time = false;
        move_arm.sendGoal(goalA);
        finished_within_time = move_arm.waitForResult(ros::Duration(200.0));
        if (!finished_within_time)
        {
            move_arm.cancelGoal();
            ROS_INFO("Timed out achieving goal A");
        }
        else
        {
            actionlib::SimpleClientGoalState state = move_arm.getState();
            bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
            if(success)
                ROS_INFO("Action finished: %s",state.toString().c_str());
            else
                ROS_INFO("Action failed: %s",state.toString().c_str());
        }
    }
    ros::shutdown();
}

Example #13

File: move_arm_pose_goal.cpp Project: DevasenaInupakutika/uuisrc-ros-pkg

int main(int argc, char **argv){
	ros::init (argc, argv, "move_arm_pose_goal_test");
	ros::NodeHandle nh;

	move_arm_msgs::MoveArmGoal goalA;

	goalA.motion_plan_request.group_name = "schunk_hand";
	goalA.motion_plan_request.num_planning_attempts = 1;
	goalA.motion_plan_request.planner_id = std::string("SBLkConfig");
	goalA.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
	goalA.motion_plan_request.allowed_planning_time = ros::Duration(2.0);

	motion_planning_msgs::SimplePoseConstraint desired_pose;
	desired_pose.header.frame_id = "/ScitosBase";
	desired_pose.link_name = "PAM100";
	desired_pose.pose.position.x = 0.46;
	desired_pose.pose.position.y = 0.00;
	desired_pose.pose.position.z = 1.14;

	desired_pose.pose.orientation.x = 0.71;
	desired_pose.pose.orientation.y = 0.0;
	desired_pose.pose.orientation.z = 0.71;
	desired_pose.pose.orientation.w = 0.0;

	desired_pose.absolute_position_tolerance.x = 0.02;
	desired_pose.absolute_position_tolerance.y = 0.02;
	desired_pose.absolute_position_tolerance.z = 0.02;

	desired_pose.absolute_roll_tolerance = 0.04;
	desired_pose.absolute_pitch_tolerance = 0.04;
	desired_pose.absolute_yaw_tolerance = 0.04;

	move_arm_msgs::addGoalConstraintToMoveArmGoal(desired_pose,goalA);

	if (!test_ik(nh, desired_pose.pose)) {
		ROS_ERROR("No kinematic solution!");
		ros::shutdown();
		return -1;
	}
	std::string servername = "move_schunk_hand";
	ROS_INFO("Waiting for server %s", servername.c_str());
	actionlib::SimpleActionClient<move_arm_msgs::MoveArmAction> move_arm(servername,true);
	move_arm.waitForServer();
	ROS_INFO("Connected to server");

	if (nh.ok())
	{
		bool finished_within_time = false;
		move_arm.sendGoal(goalA);
		finished_within_time = move_arm.waitForResult(ros::Duration(20.0));
		if (!finished_within_time)
		{
			move_arm.cancelGoal();
			ROS_INFO("Timed out achieving goal A");
		}
		else
		{
			actionlib::SimpleClientGoalState state = move_arm.getState();
			bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
			if(success)
				ROS_INFO("Action finished: %s",state.toString().c_str());
			else
				ROS_INFO("Action failed: %s",state.toString().c_str());
		}
	}
	ros::shutdown();
}

Example #14

File: move_right_arm_joint_goal.cpp Project: jordi-pages/reem_tutorials

int main(int argc, char **argv)
{
  // Init the ROS node
  ros::init (argc, argv, "move_right_arm_joint_goal_test");

  // Precondition: Valid clock
  ros::NodeHandle nh;
  if (!ros::Time::waitForValid(ros::WallDuration(5.0))) // NOTE: Important when using simulated clock
  {
    ROS_FATAL("Timed-out waiting for valid time.");
    return EXIT_FAILURE;
  }

  // Action client for sending motion planing requests
  actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm("move_right_arm_torso", true);

  // Wait for the action client to be connected to the server
  ROS_INFO("Connecting to server...");
  if (!move_arm.waitForServer(ros::Duration(5.0)))
  {
    ROS_ERROR("Timed-out waiting for the move_arm action server.");
    return EXIT_FAILURE;
  }
  ROS_INFO("Connected to server.");

  // Prepare motion plan request with joint-space goal
  arm_navigation_msgs::MoveArmGoal goal;
  std::vector<std::string> names(9);
  names[0] = "torso_1_joint";
  names[1] = "torso_2_joint";
  names[2] = "arm_right_1_joint";
  names[3] = "arm_right_2_joint";
  names[4] = "arm_right_3_joint";
  names[5] = "arm_right_4_joint";
  names[6] = "arm_right_5_joint";
  names[7] = "arm_right_6_joint";
  names[8] = "arm_right_7_joint";

  goal.motion_plan_request.group_name = "right_arm_torso";
  goal.motion_plan_request.num_planning_attempts = 1;
  goal.motion_plan_request.allowed_planning_time = ros::Duration(5.0);

  goal.motion_plan_request.planner_id= std::string("");
  goal.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
  goal.motion_plan_request.goal_constraints.joint_constraints.resize(names.size());

  for (unsigned int i = 0 ; i < goal.motion_plan_request.goal_constraints.joint_constraints.size(); ++i)
  {
    goal.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = names[i];
    goal.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0;
    goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.05;
    goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.05;
  }

  goal.motion_plan_request.goal_constraints.joint_constraints[0].position =  0.0;
  goal.motion_plan_request.goal_constraints.joint_constraints[1].position =  0.0;
  goal.motion_plan_request.goal_constraints.joint_constraints[2].position =  1.2;
  goal.motion_plan_request.goal_constraints.joint_constraints[3].position =  0.15;
  goal.motion_plan_request.goal_constraints.joint_constraints[4].position = -1.5708;
  goal.motion_plan_request.goal_constraints.joint_constraints[5].position =  1.3;
  goal.motion_plan_request.goal_constraints.joint_constraints[6].position =  0.0;
  goal.motion_plan_request.goal_constraints.joint_constraints[7].position =  0.0;
  goal.motion_plan_request.goal_constraints.joint_constraints[8].position =  0.0;

  // Send motion plan request
  if (nh.ok())
  {
    bool finished_within_time = false;
    move_arm.sendGoal(goal);
    finished_within_time = move_arm.waitForResult(ros::Duration(15.0));
    if (!finished_within_time)
    {
      move_arm.cancelGoal();
      ROS_INFO("Timed out achieving joint-space goal.");
    }
    else
    {
      actionlib::SimpleClientGoalState state = move_arm.getState();
      bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
      if(success)
        ROS_INFO("Action finished: %s",state.toString().c_str());
      else
        ROS_INFO("Action failed: %s",state.toString().c_str());
    }
  }
  ros::shutdown();
}

Example #15

File: move_arm_joint_goal.cpp Project: nttputus/wp6_manipulator

int main(int argc, char **argv){
  ros::init (argc, argv, "move_arm_joint_goal_test");
  ros::NodeHandle nh;
  actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm("move_arm",true);

//
  planning_environment::CollisionModels* collisionModels;
  planning_models::KinematicState* kinematicState;

  collisionModels = new planning_environment::CollisionModels("robot_description");
  kinematicState = new planning_models::KinematicState(collisionModels->getKinematicModel());

  ros::service::waitForService(SET_PLANNING_SCENE_DIFF_NAME);
  ROS_INFO("Waiting for planning scene service");
  ros::ServiceClient set_planning_scene_diff_client = nh.serviceClient<arm_navigation_msgs::SetPlanningSceneDiff>(SET_PLANNING_SCENE_DIFF_NAME);

  set_planning_scene_diff_client.waitForExistence();
  ROS_INFO("Planning scene service is now available");

  arm_navigation_msgs::SetPlanningSceneDiff::Request planning_scene_req;
  arm_navigation_msgs::SetPlanningSceneDiff::Response planning_scene_res;
  planning_environment::convertKinematicStateToRobotState(*kinematicState, ros::Time::now(), collisionModels->getWorldFrameId(), planning_scene_req.planning_scene_diff.robot_state);
  
  if(!set_planning_scene_diff_client.call(planning_scene_req, planning_scene_res)) {
    ROS_WARN("Can't get planning scene");
  }
  ROS_INFO("Successfully set planning scene");

  collisionModels->revertPlanningScene(kinematicState);
  collisionModels->setPlanningScene(planning_scene_res.planning_scene);
//

  move_arm.waitForServer();
  ROS_INFO("Connected to server");

  arm_navigation_msgs::MoveArmGoal goalB;
  std::vector<std::string> names(9);
  names[0] = "body1";
  names[1] = "body2";
  names[2] = "body3";
  names[3] = "body4";
  names[4] = "body5";
  names[5] = "body6";
  names[6] = "body7";
  names[7] = "body8";
  names[8] = "body9";

  goalB.motion_plan_request.group_name = "arm_cartesian";
  goalB.motion_plan_request.num_planning_attempts = 1;
  goalB.motion_plan_request.allowed_planning_time = ros::Duration(5.0);

  goalB.motion_plan_request.planner_id= std::string("");
  goalB.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
  goalB.motion_plan_request.goal_constraints.joint_constraints.resize(names.size());

  for (unsigned int i = 0 ; i < goalB.motion_plan_request.goal_constraints.joint_constraints.size(); ++i)
  {
    goalB.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = names[i];
    goalB.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0;
    goalB.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.1;
    goalB.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.1;
  }
    
  goalB.motion_plan_request.goal_constraints.joint_constraints[1].position = -0.5;
  goalB.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2;
  goalB.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.15;
   
  if (nh.ok())
  {
    bool finished_within_time = false;
    move_arm.sendGoal(goalB);
    finished_within_time = move_arm.waitForResult(ros::Duration(200.0));
    if (!finished_within_time)
    {
      move_arm.cancelGoal();
      ROS_INFO("Timed out achieving goal A");
    }
    else
    {
      actionlib::SimpleClientGoalState state = move_arm.getState();
      bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
      if(success)
        ROS_INFO("Action finished: %s",state.toString().c_str());
      else
        ROS_INFO("Action failed: %s",state.toString().c_str());
    }
  }
  ros::shutdown();
}

Example #16

File: move_arm_joint_goal.cpp Project: cpc-pk/care-o-bot

int main(int argc, char **argv){
  ros::init (argc, argv, "move_arm_joint_goal_test");
  ros::NodeHandle nh;
  actionlib::SimpleActionClient<move_arm_msgs::MoveArmAction> move_arm("move_arm",true);

  move_arm.waitForServer();
  ROS_INFO("Connected to server");

  move_arm_msgs::MoveArmGoal goalB;
  std::vector<std::string> names(7);
  names[0] = "arm_1_joint";//"r_shoulder_pan_joint";
  names[1] = "arm_2_joint";//"r_shoulder_lift_joint";
  names[2] = "arm_3_joint";//"r_upper_arm_roll_joint";
  names[3] = "arm_4_joint";//"r_elbow_flex_joint";
  names[4] = "arm_5_joint";//"r_forearm_roll_joint";
  names[5] = "arm_6_joint";//"r_wrist_flex_joint";
  names[6] = "arm_7_joint";//"r_wrist_roll_joint";

  goalB.motion_plan_request.group_name = "arm";//"right_arm";
  goalB.motion_plan_request.num_planning_attempts = 1;
  goalB.motion_plan_request.allowed_planning_time = ros::Duration(5.0);

  goalB.motion_plan_request.planner_id= std::string("");
  goalB.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
  goalB.motion_plan_request.goal_constraints.joint_constraints.resize(names.size());

  for (unsigned int i = 0 ; i < goalB.motion_plan_request.goal_constraints.joint_constraints.size(); ++i)
  {
    goalB.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = names[i];
    goalB.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0;
    goalB.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.1;
    goalB.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.1;
  }
	
	//PR2:  
  //goalB.motion_plan_request.goal_constraints.joint_constraints[0].position = -2.0;
  //goalB.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2; //-1.0;
  //goalB.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.15;

	//COB:
	//goalB.motion_plan_request.goal_constraints.joint_constraints[0].position = 0.8;//pole-pos1
	//goalB.motion_plan_request.goal_constraints.joint_constraints[0].position = -1.0;//pole-pos2
	//goalB.motion_plan_request.goal_constraints.joint_constraints[1].position = -0.78;
	
	//over-tablet
	//-0.97877458873047019, -1.5948518814806336, 2.0263840730501208, 1.4992515760970839, 0.48346032199394173, 0.79316104671682552, -3.8301333079173459
	//goalB.motion_plan_request.goal_constraints.joint_constraints[0].position = -0.98;
	//goalB.motion_plan_request.goal_constraints.joint_constraints[1].position = -1.59;
	//goalB.motion_plan_request.goal_constraints.joint_constraints[2].position = 2.03;
	//goalB.motion_plan_request.goal_constraints.joint_constraints[3].position = 1.50;
	//goalB.motion_plan_request.goal_constraints.joint_constraints[4].position = 0.48;
	//goalB.motion_plan_request.goal_constraints.joint_constraints[5].position = 0.79;
	//goalB.motion_plan_request.goal_constraints.joint_constraints[6].position = -3.83;


  if (nh.ok())
  {
    bool finished_within_time = false;
    move_arm.sendGoal(goalB);
    finished_within_time = move_arm.waitForResult(ros::Duration(200.0));
    if (!finished_within_time)
    {
      move_arm.cancelGoal();
      ROS_INFO("Timed out achieving goal A");
    }
    else
    {
      actionlib::SimpleClientGoalState state = move_arm.getState();
      bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
      if(success)
        ROS_INFO("Action finished: %s",state.toString().c_str());
      else
        ROS_INFO("Action failed: %s",state.toString().c_str());
    }
  }
  ros::shutdown();
}

Example #17

File: regression_test_pose_goal_floorobj_no_ik.cpp Project: Arbart/cob_manipulation_sandbox

TEST(MoveArm, goToPoseGoal)
{
  ros::NodeHandle nh;
  ros::NodeHandle private_handle("~");
  actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm(nh, "move_right_arm");
  boost::thread spin_thread(&spinThread);

  move_arm.waitForServer();
  ROS_INFO("Connected to server");
  arm_navigation_msgs::MoveArmGoal goalA;

  goalA.motion_plan_request.group_name = "right_arm";
  goalA.motion_plan_request.num_planning_attempts = 1;
  private_handle.param<std::string>("planner_id",goalA.motion_plan_request.planner_id,std::string("chomp_planner_longrange"));
  private_handle.param<std::string>("planner_service_name",goalA.planner_service_name,std::string("/chomp_planner_longrange/plan_path"));

  goalA.motion_plan_request.allowed_planning_time = ros::Duration(10.0);
    
  goalA.motion_plan_request.goal_constraints.set_position_constraints_size(1);
  goalA.motion_plan_request.goal_constraints.position_constraints[0].header.stamp = ros::Time::now();
  goalA.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id = "torso_lift_link";
    
  goalA.motion_plan_request.goal_constraints.position_constraints[0].link_name = "r_wrist_roll_link";
  goalA.motion_plan_request.goal_constraints.position_constraints[0].position.x = 0.15;
  goalA.motion_plan_request.goal_constraints.position_constraints[0].position.y = -0.95;
  goalA.motion_plan_request.goal_constraints.position_constraints[0].position.z = 0;
    
  goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.type =  arm_navigation_msgs::Shape::BOX;
  goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
  goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
  goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);

  goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.vertices.resize(1);
  goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.vertices[0].x = 0.15;
  goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.vertices[0].y = -0.95;
  goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.vertices[0].z = 0.0;

  goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.x = 0.0;
  goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.y = 0.0;
  goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.z = 0.0;
  goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.w = 1.0;

  goalA.motion_plan_request.goal_constraints.position_constraints[0].weight = 1.0;

  goalA.motion_plan_request.goal_constraints.set_orientation_constraints_size(1);
  goalA.motion_plan_request.goal_constraints.orientation_constraints[0].header.stamp = ros::Time::now();
  goalA.motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id = "torso_lift_link";    
  goalA.motion_plan_request.goal_constraints.orientation_constraints[0].link_name = "r_wrist_roll_link";
  goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.x = 0.0;
  goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.y = 0.0;
  goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.z = -0.7071;
  goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.w = 0.7071;
    
  goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_roll_tolerance = 0.2;
  goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_pitch_tolerance = 0.2;
  goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_yaw_tolerance = 0.2;
  
  goalA.motion_plan_request.goal_constraints.orientation_constraints[0].weight = 1.0;

  /* move arm should send the pose constraint straight to the planner */
  goalA.disable_ik = true;
  
  std::vector<std::string> names(7);
  names[0] = "r_shoulder_pan_joint";
  names[1] = "r_shoulder_lift_joint";
  names[2] = "r_upper_arm_roll_joint";
  names[3] = "r_elbow_flex_joint";
  names[4] = "r_forearm_roll_joint";
  names[5] = "r_wrist_flex_joint";
  names[6] = "r_wrist_roll_joint";


  int num_test_attempts = 0;
  int max_attempts = 5;
  bool success = false;


  while (nh.ok())
  {
    bool finished_within_time = false;
    move_arm.sendGoal(goalA);
    finished_within_time = move_arm.waitForResult(ros::Duration(200.0));
    actionlib::SimpleClientGoalState state = move_arm.getState();
    success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
    if ((!finished_within_time || !success) && num_test_attempts < max_attempts)
    {
      move_arm.cancelAllGoals();
      ROS_INFO("Timed out achieving goal A, trying again. Trying again, attempt: %d",num_test_attempts);
      num_test_attempts++;
    }
    else
    {
      if(!success)
      {
        ROS_INFO("Action unsuccessful");
        move_arm.cancelAllGoals();
      }
      ROS_INFO("Action finished: %s",state.toString().c_str());
      break;
    }
  }
  EXPECT_TRUE(success);
  ros::shutdown();
  spin_thread.join();
}

Example #18

File: regression_test_cylinder_constraint.cpp Project: Arbart/cob_manipulation_sandbox

TEST(MoveArm, goToPoseGoal)
{
  ros::NodeHandle nh;
  actionlib::SimpleActionClient<move_arm::MoveArmAction> move_arm(nh, "move_right_arm");
  boost::thread spin_thread(&spinThread);

  move_arm.waitForServer();
  ROS_INFO("Connected to server");
  move_arm::MoveArmGoal goalA;

  //getting a monitor so that we can track the configuration of the arm
  planning_environment::RobotModels rm("robot_description");
  EXPECT_TRUE(rm.loadedModels());
    
  tf::TransformListener tf;
  planning_environment::KinematicModelStateMonitor km(&rm, &tf);
  km.waitForState();
  //should have the state at this point  

  goalA.goal_constraints.set_pose_constraint_size(1);
  goalA.goal_constraints.pose_constraint[0].pose.header.stamp = ros::Time::now();
  goalA.goal_constraints.pose_constraint[0].pose.header.frame_id = "base_link";
  
  //starting configuration
  //-position [x, y, z]    = [0.77025, -.18, 0.73]
  //-rotation [x, y, z, w] = [0, -0.05, 0, 0]

  goalA.goal_constraints.pose_constraint[0].type = arm_navigation_msgs::PoseConstraint::POSITION_X 
    + arm_navigation_msgs::PoseConstraint::POSITION_Y 
    + arm_navigation_msgs::PoseConstraint::POSITION_Z
    + arm_navigation_msgs::PoseConstraint::ORIENTATION_R 
    + arm_navigation_msgs::PoseConstraint::ORIENTATION_P 
    + arm_navigation_msgs::PoseConstraint::ORIENTATION_Y;
  
  goalA.goal_constraints.pose_constraint[0].link_name = "r_wrist_roll_link";
  goalA.goal_constraints.pose_constraint[0].pose.pose.position.x = 0.60;
  goalA.goal_constraints.pose_constraint[0].pose.pose.position.y = -0.25;
  goalA.goal_constraints.pose_constraint[0].pose.pose.position.z = 0.84;
    
  goalA.goal_constraints.pose_constraint[0].pose.pose.orientation.x = 0;
  goalA.goal_constraints.pose_constraint[0].pose.pose.orientation.y = 0;
  goalA.goal_constraints.pose_constraint[0].pose.pose.orientation.z = 0;
  goalA.goal_constraints.pose_constraint[0].pose.pose.orientation.w = 1.0;

  goalA.goal_constraints.pose_constraint[0].position_tolerance_above.x = 0.01;
  goalA.goal_constraints.pose_constraint[0].position_tolerance_above.y = 0.01;
  goalA.goal_constraints.pose_constraint[0].position_tolerance_above.z = 0.01;
  goalA.goal_constraints.pose_constraint[0].position_tolerance_below.x = 0.01;
  goalA.goal_constraints.pose_constraint[0].position_tolerance_below.y = 0.01;
  goalA.goal_constraints.pose_constraint[0].position_tolerance_below.z = 0.01;

  goalA.goal_constraints.pose_constraint[0].orientation_tolerance_above.x = 0.01;
  goalA.goal_constraints.pose_constraint[0].orientation_tolerance_above.y = 0.01;
  goalA.goal_constraints.pose_constraint[0].orientation_tolerance_above.z = 0.01;
  goalA.goal_constraints.pose_constraint[0].orientation_tolerance_below.x = 0.01;
  goalA.goal_constraints.pose_constraint[0].orientation_tolerance_below.y = 0.01;
  goalA.goal_constraints.pose_constraint[0].orientation_tolerance_below.z = 0.01;

  goalA.goal_constraints.pose_constraint[0].orientation_importance = 0.2;

  goalA.contacts.resize(2);
  goalA.contacts[0].links.push_back("r_gripper_l_finger_link");
  goalA.contacts[0].links.push_back("r_gripper_r_finger_link");
  goalA.contacts[0].links.push_back("r_gripper_l_finger_tip_link");
  goalA.contacts[0].links.push_back("r_gripper_r_finger_tip_link");
  
  goalA.contacts[0].depth = 0.04;
  goalA.contacts[0].bound.type = arm_navigation_msgs::Object::SPHERE;
  goalA.contacts[0].bound.dimensions.push_back(0.3);
  goalA.contacts[0].pose = goalA.goal_constraints.pose_constraint[0].pose;
  
  goalA.contacts[1].links.push_back("r_gripper_palm_link");
  goalA.contacts[1].links.push_back("r_wrist_roll_link");
  goalA.contacts[1].depth = 0.02;
  goalA.contacts[1].bound.type = arm_navigation_msgs::Object::SPHERE;
  goalA.contacts[1].bound.dimensions.push_back(0.2);
  goalA.contacts[1].pose = goalA.goal_constraints.pose_constraint[0].pose;
  
  if (nh.ok())
  {
    bool finished_within_time = false;
    move_arm.sendGoal(goalA);
    finished_within_time = move_arm.waitForResult(ros::Duration(10.0));
    
    EXPECT_TRUE(finished_within_time);
    if (!finished_within_time)
    {
      move_arm.cancelGoal();
      ROS_INFO("Timed out achieving goal A");
    }
    else
    {
      actionlib::SimpleClientGoalState state = move_arm.getState();
      ROS_INFO("Action finished: %s",state.toString().c_str());
      EXPECT_TRUE(state == actionlib::SimpleClientGoalState::SUCCEEDED);

      double dist_pose;
      double dist_angle;
      
      diffConfig(km,goalA,dist_pose,dist_angle);

      //close enough - summed tolerances
      EXPECT_TRUE(dist_pose < .005+.005+.01);
      EXPECT_TRUE(dist_angle < .005*3);

      EXPECT_TRUE(finalStateMatchesGoal(km,goalA));
    
    }
  }

  nh.setParam( "/move_right_arm/long_range_only", true);

  move_arm::MoveArmGoal goalB;
  
  //starting configuration
  //-position [x, y, z]    = [0.77025, -.18, 0.73]
  //-rotation [x, y, z, w] = [0, -0.05, 0, 0]

  goalB.goal_constraints.set_pose_constraint_size(1);
  goalB.goal_constraints.pose_constraint[0].pose.header.stamp = ros::Time::now();
  goalB.goal_constraints.pose_constraint[0].pose.header.frame_id = "base_link";
  goalB.goal_constraints.pose_constraint[0].type = arm_navigation_msgs::PoseConstraint::POSITION_X 
    + arm_navigation_msgs::PoseConstraint::POSITION_Y 
    + arm_navigation_msgs::PoseConstraint::POSITION_Z
    + arm_navigation_msgs::PoseConstraint::ORIENTATION_R 
    + arm_navigation_msgs::PoseConstraint::ORIENTATION_P 
    + arm_navigation_msgs::PoseConstraint::ORIENTATION_Y;
  
  goalB.goal_constraints.pose_constraint[0].link_name = "r_wrist_roll_link";
  goalB.goal_constraints.pose_constraint[0].pose.pose.position.x = 0.60;
  goalB.goal_constraints.pose_constraint[0].pose.pose.position.y = -0.25;
  goalB.goal_constraints.pose_constraint[0].pose.pose.position.z = 0.5;
  
  goalB.goal_constraints.pose_constraint[0].pose.pose.orientation.x = 0;
  goalB.goal_constraints.pose_constraint[0].pose.pose.orientation.y = 0;
  goalB.goal_constraints.pose_constraint[0].pose.pose.orientation.z = 0;
  goalB.goal_constraints.pose_constraint[0].pose.pose.orientation.w = 1.0;
  
  goalB.goal_constraints.pose_constraint[0].position_tolerance_above.x = 0.05;
  goalB.goal_constraints.pose_constraint[0].position_tolerance_above.y = 0.05;
  goalB.goal_constraints.pose_constraint[0].position_tolerance_above.z = 0.05;
  goalB.goal_constraints.pose_constraint[0].position_tolerance_below.x = 0.05;
  goalB.goal_constraints.pose_constraint[0].position_tolerance_below.y = 0.05;
  goalB.goal_constraints.pose_constraint[0].position_tolerance_below.z = 0.05;

  goalB.goal_constraints.pose_constraint[0].orientation_tolerance_above.x = 0.05;
  goalB.goal_constraints.pose_constraint[0].orientation_tolerance_above.y = 0.05;
  goalB.goal_constraints.pose_constraint[0].orientation_tolerance_above.z = 0.05;
  goalB.goal_constraints.pose_constraint[0].orientation_tolerance_below.x = 0.05;
  goalB.goal_constraints.pose_constraint[0].orientation_tolerance_below.y = 0.05;
  goalB.goal_constraints.pose_constraint[0].orientation_tolerance_below.z = 0.05;

  goalB.goal_constraints.pose_constraint[0].orientation_importance = 0.2;

  goalB.path_constraints.set_pose_constraint_size(1);
  goalB.path_constraints.pose_constraint[0].pose.header.stamp = ros::Time::now();
  goalB.path_constraints.pose_constraint[0].pose.header.frame_id = "base_link";
  goalB.path_constraints.pose_constraint[0].type = arm_navigation_msgs::PoseConstraint::POSITION_X 
    + arm_navigation_msgs::PoseConstraint::POSITION_Y
    + arm_navigation_msgs::PoseConstraint::POSITION_Z
    + arm_navigation_msgs::PoseConstraint::ORIENTATION_R
    + arm_navigation_msgs::PoseConstraint::ORIENTATION_P; 
//     + arm_navigation_msgs::PoseConstraint::ORIENTATION_Y;
  goalB.path_constraints.pose_constraint[0].link_name = "r_wrist_roll_link";
  goalB.path_constraints.pose_constraint[0].pose.pose.position.x = 0.60;
  goalB.path_constraints.pose_constraint[0].pose.pose.position.y = -0.25;
  goalB.path_constraints.pose_constraint[0].pose.pose.position.z = .45;
    
  goalB.path_constraints.pose_constraint[0].pose.pose.orientation.x = 0;
  goalB.path_constraints.pose_constraint[0].pose.pose.orientation.y = 0;
  goalB.path_constraints.pose_constraint[0].pose.pose.orientation.z = 0;
  goalB.path_constraints.pose_constraint[0].pose.pose.orientation.w = 1.0;

  goalB.path_constraints.pose_constraint[0].position_tolerance_above.x = 0.1;
  goalB.path_constraints.pose_constraint[0].position_tolerance_above.y = 0.1;
  goalB.path_constraints.pose_constraint[0].position_tolerance_below.x = 0.1;
  goalB.path_constraints.pose_constraint[0].position_tolerance_below.y = 0.1;
  goalB.path_constraints.pose_constraint[0].position_tolerance_above.z = 10.0;
  goalB.path_constraints.pose_constraint[0].position_tolerance_below.z = 0.1;

  goalB.path_constraints.pose_constraint[0].orientation_tolerance_above.x = 0.1;
  goalB.path_constraints.pose_constraint[0].orientation_tolerance_above.y = 0.1;
//   goalB.path_constraints.pose_constraint[0].orientation_tolerance_above.z = 0.05;
  goalB.path_constraints.pose_constraint[0].orientation_tolerance_below.x = 0.1;
  goalB.path_constraints.pose_constraint[0].orientation_tolerance_below.y = 0.1;
//   goalB.path_constraints.pose_constraint[0].orientation_tolerance_below.z = 0.05;

  goalB.path_constraints.pose_constraint[0].orientation_importance = 0.4;

  goalB.contacts.resize(2);
  goalB.contacts[0].links.push_back("r_gripper_l_finger_link");
  goalB.contacts[0].links.push_back("r_gripper_r_finger_link");
  goalB.contacts[0].links.push_back("r_gripper_l_finger_tip_link");
  goalB.contacts[0].links.push_back("r_gripper_r_finger_tip_link");
  
  goalB.contacts[0].depth = 0.04;
  goalB.contacts[0].bound.type = arm_navigation_msgs::Object::SPHERE;
  goalB.contacts[0].bound.dimensions.push_back(0.3);
  goalB.contacts[0].pose = goalB.goal_constraints.pose_constraint[0].pose;
  
  goalB.contacts[1].links.push_back("r_gripper_palm_link");
  goalB.contacts[1].links.push_back("r_wrist_roll_link");
  goalB.contacts[1].depth = 0.02;
  goalB.contacts[1].bound.type = arm_navigation_msgs::Object::SPHERE;
  goalB.contacts[1].bound.dimensions.push_back(0.2);
  goalB.contacts[1].pose = goalB.goal_constraints.pose_constraint[0].pose;
  
  if (nh.ok())
  {
    move_arm.sendGoal(goalB);

    ros::Time start_time = ros::Time::now();
    ros::Duration elapsed(0.0);

    //trying ticks in case time gets wonky
    unsigned int ticks=0;

    while(1) {
      
      bool result_during_cycle = move_arm.waitForResult(ros::Duration(.2));
      
      //got some result before time out
      if(result_during_cycle) {
        break;
      }
      
      elapsed = ros::Time::now()-start_time;

      std::cout << "Time " << elapsed.toSec() << std::endl;

      //checking if we've gone over max time - if so bail
      if(elapsed.toSec() > 10.0 || ticks++ > 50) break;

      goalB.path_constraints.pose_constraint[0].pose.pose.orientation.x = 0;
      goalB.path_constraints.pose_constraint[0].pose.pose.orientation.y = 0;

      //check that the path obeys constraints
      EXPECT_TRUE(currentStateSatisfiesPathConstraints(km,goalB));
      
    }

    if (elapsed.toSec() > 10.0)
    {
      move_arm.cancelGoal();
      ROS_INFO("Timed out achieving goal A");
      EXPECT_TRUE(elapsed.toSec() < 10.0);
    }
    else
    {
      actionlib::SimpleClientGoalState state = move_arm.getState();
      ROS_INFO("Action finished: %s",state.toString().c_str());
      EXPECT_TRUE(state == actionlib::SimpleClientGoalState::SUCCEEDED);

      double dist_pose;
      double dist_angle;
      
      diffConfig(km,goalA,dist_pose,dist_angle);

      //close enough - summed tolerances
      EXPECT_TRUE(dist_pose < .01+.01+.002);
      EXPECT_TRUE(dist_angle < .05*3);

      EXPECT_TRUE(finalStateMatchesGoal(km,goalB));
    }
  }

  ros::shutdown();
  spin_thread.join();
}

Example #19

File: move_right_arm_multiple_goals_move_arm.cpp Project: sid24ss/sbpl_arm_planning

int main(int argc, char **argv){
  ros::init (argc, argv, "move_arm_pose_goal_test");
  ros::NodeHandle nh;
  ros::NodeHandle ph("~");
  std::string arm_name;
  ROS_INFO("using right arm"); 
//  actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm(nh,"move_arm");
  actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm("move_right_arm",true);
  move_arm.waitForServer();
  ROS_INFO("Connected to server");
  arm_navigation_msgs::MoveArmGoal goalA;


  ph.param<std::string>("arm_name", arm_name,std::string("right_arm"));
  goalA.motion_plan_request.group_name = arm_name;
  goalA.motion_plan_request.num_planning_attempts = 1;
  goalA.motion_plan_request.planner_id = std::string("");
  goalA.planner_service_name = std::string("sbpl_planning/plan_path");

  double duration;
  ph.param<double>("planning_duration",duration,5.0);
  goalA.motion_plan_request.allowed_planning_time = ros::Duration(duration);

  double number_of_goals;
  ph.param<double>("number_of_goals",number_of_goals,1.0);

  double goal_x,goal_y,goal_z,goal_roll,goal_pitch,goal_yaw;

  ROS_INFO("Number of goals received: %d",int(number_of_goals));

 std::stringstream ssTemp;
  std::string sTemp;
  char paramTemp[80];
  for (int i = 0; i < int(number_of_goals); ++i)
  {
    ssTemp.str(std::string());
    ssTemp.clear();
    ssTemp << int(i);
    sTemp = ssTemp.str();
    ROS_INFO("Processing goal number : %s ",sTemp.c_str());

    paramTemp[0] = '\0';
    strcat(paramTemp,"goal_x_");
    strcat(paramTemp,sTemp.c_str());
    ph.param<double>(paramTemp,goal_x,0.75);
    paramTemp[0] = '\0';
    strcat(paramTemp,"goal_y_");
    strcat(paramTemp,sTemp.c_str());
    ph.param<double>(paramTemp,goal_y,-0.188);
    paramTemp[0] = '\0';
    strcat(paramTemp,"goal_z_");
    strcat(paramTemp,sTemp.c_str());
    ph.param<double>(paramTemp,goal_z,0.0);

    paramTemp[0] = '\0';
    strcat(paramTemp,"goal_roll_");
    strcat(paramTemp,sTemp.c_str());
    ph.param<double>(paramTemp,goal_roll,1.0);
    paramTemp[0] = '\0';
    strcat(paramTemp,"goal_pitch_");
    strcat(paramTemp,sTemp.c_str());
    ph.param<double>(paramTemp,goal_pitch,0.5);
    paramTemp[0] = '\0';
    strcat(paramTemp,"goal_yaw_");
    strcat(paramTemp,sTemp.c_str());
    ph.param<double>(paramTemp,goal_yaw,0.2);

    tf::Quaternion gripper_goal;
    geometry_msgs::Quaternion gripper_goal_msg;
    gripper_goal.setRPY(goal_roll,goal_pitch,goal_yaw);
    tf::quaternionTFToMsg(gripper_goal,gripper_goal_msg);

    arm_navigation_msgs::SimplePoseConstraint desired_pose;
    desired_pose.header.frame_id = "base_link";
    desired_pose.link_name = "r_wrist_roll_link";
    desired_pose.pose.position.x = goal_x;
    desired_pose.pose.position.y = goal_y;
    desired_pose.pose.position.z = goal_z;
    ROS_INFO("[send_move_arm_goal] Goal number: %d arm_name: %s frame: %s position: %0.3f %0.3f %0.3f orientation: %0.2f %0.2f %0.2f %0.2f",(i+1),arm_name.c_str(),desired_pose.header.frame_id.c_str(),goal_x,goal_y,goal_z,gripper_goal_msg.x,gripper_goal_msg.y,gripper_goal_msg.z,gripper_goal_msg.w);

    desired_pose.pose.orientation.x = gripper_goal_msg.x;
    desired_pose.pose.orientation.y = gripper_goal_msg.y;
    desired_pose.pose.orientation.z = gripper_goal_msg.z;
    desired_pose.pose.orientation.w = gripper_goal_msg.w;

    desired_pose.absolute_position_tolerance.x = 0.01;
    desired_pose.absolute_position_tolerance.y = 0.01;
    desired_pose.absolute_position_tolerance.z = 0.01;

    desired_pose.absolute_roll_tolerance = 0.08;
    desired_pose.absolute_pitch_tolerance = 0.08;
    desired_pose.absolute_yaw_tolerance = 0.08;
    ROS_INFO("Attaching Pose to goal");
    arm_navigation_msgs::addGoalConstraintToMoveArmGoal(desired_pose,goalA);
  
  }

  goalA.accept_partial_plans = true;
  goalA.accept_invalid_goals = true;
  goalA.disable_collision_monitoring = true;

  if (nh.ok())
  {
    bool finished_within_time = false;
    ROS_INFO("Sending Goal");
    move_arm.sendGoal(goalA);
    finished_within_time = move_arm.waitForResult(ros::Duration(100.0));
    if (!finished_within_time)
    {
      move_arm.cancelGoal();
      ROS_INFO("Timed out achieving goal A");
    }
    else
    {
      actionlib::SimpleClientGoalState state = move_arm.getState();
      bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
      if(success)
        ROS_INFO("Action finished: %s",state.toString().c_str());
      else
        ROS_INFO("Action failed: %s",state.toString().c_str());
    }
  }
  ros::shutdown();
}

Example #20

File: oscillate_move.cpp Project: Arbart/cob_manipulation_sandbox

int main(int argc, char **argv)
{
    ros::init(argc, argv, "test_move_right_arm");
    ros::NodeHandle nh;

    actionlib::SimpleActionClient<move_arm::MoveArmAction> move_arm(nh, "move_right_arm");

    move_arm::MoveArmGoal goalA;
    move_arm::MoveArmGoal goalB;
    
    goalA.goal_constraints.set_pose_constraint_size(1);
    goalA.goal_constraints.pose_constraint[0].pose.header.stamp = ros::Time::now();
    goalA.goal_constraints.pose_constraint[0].pose.header.frame_id = "torso_lift_link";
    
    goalA.goal_constraints.pose_constraint[0].link_name = "r_wrist_roll_link";
    goalA.goal_constraints.pose_constraint[0].pose.pose.position.x = 0.75;
    goalA.goal_constraints.pose_constraint[0].pose.pose.position.y = -0.188;
    goalA.goal_constraints.pose_constraint[0].pose.pose.position.z = 0;
    
    goalA.goal_constraints.pose_constraint[0].pose.pose.orientation.x = 0;
    goalA.goal_constraints.pose_constraint[0].pose.pose.orientation.y = 0;
    goalA.goal_constraints.pose_constraint[0].pose.pose.orientation.z = 0;
    goalA.goal_constraints.pose_constraint[0].pose.pose.orientation.w = 1;

    goalA.goal_constraints.pose_constraint[0].position_tolerance_above.x = 0.003;
    goalA.goal_constraints.pose_constraint[0].position_tolerance_above.y = 0.003;
    goalA.goal_constraints.pose_constraint[0].position_tolerance_above.z = 0.003;
    goalA.goal_constraints.pose_constraint[0].position_tolerance_below.x = 0.003;
    goalA.goal_constraints.pose_constraint[0].position_tolerance_below.y = 0.003;
    goalA.goal_constraints.pose_constraint[0].position_tolerance_below.z = 0.003;
    
    goalA.goal_constraints.pose_constraint[0].orientation_tolerance_above.x = 0.005;
    goalA.goal_constraints.pose_constraint[0].orientation_tolerance_above.y = 0.005;
    goalA.goal_constraints.pose_constraint[0].orientation_tolerance_above.z = 0.005;
    goalA.goal_constraints.pose_constraint[0].orientation_tolerance_below.x = 0.005;
    goalA.goal_constraints.pose_constraint[0].orientation_tolerance_below.y = 0.005;
    goalA.goal_constraints.pose_constraint[0].orientation_tolerance_below.z = 0.005;  

    goalA.goal_constraints.pose_constraint[0].orientation_importance = 0.1;
    goalA.goal_constraints.pose_constraint[0].type =
	arm_navigation_msgs::PoseConstraint::POSITION_X + arm_navigation_msgs::PoseConstraint::POSITION_Y + arm_navigation_msgs::PoseConstraint::POSITION_Z + 
	arm_navigation_msgs::PoseConstraint::ORIENTATION_R + arm_navigation_msgs::PoseConstraint::ORIENTATION_P + arm_navigation_msgs::PoseConstraint::ORIENTATION_Y;
    
    
    std::vector<std::string> names(7);
    names[0] = "r_shoulder_pan_joint";
    names[1] = "r_shoulder_lift_joint";
    names[2] = "r_upper_arm_roll_joint";
    names[3] = "r_elbow_flex_joint";
    names[4] = "r_forearm_roll_joint";
    names[5] = "r_wrist_flex_joint";
    names[6] = "r_wrist_roll_joint";
    
    goalB.goal_constraints.joint_constraint.resize(names.size());
    for (unsigned int i = 0 ; i < goalB.goal_constraints.joint_constraint.size(); ++i)
    {
	goalB.goal_constraints.joint_constraint[i].header.stamp = ros::Time::now();
	goalB.goal_constraints.joint_constraint[i].header.frame_id = "base_link";
	goalB.goal_constraints.joint_constraint[i].joint_name = names[i];
	goalB.goal_constraints.joint_constraint[i].value.resize(1);
	goalB.goal_constraints.joint_constraint[i].tolerance_above.resize(1);
	goalB.goal_constraints.joint_constraint[i].tolerance_below.resize(1);
	goalB.goal_constraints.joint_constraint[i].value[0] = 0.0;
	goalB.goal_constraints.joint_constraint[i].tolerance_below[0] = 0.0;
	goalB.goal_constraints.joint_constraint[i].tolerance_above[0] = 0.0;
    }
    
    goalB.goal_constraints.joint_constraint[0].value[0] = -2.0;
    goalB.goal_constraints.joint_constraint[3].value[0] = -0.1;
    goalB.goal_constraints.joint_constraint[5].value[0] = 0.15;
    
    while (nh.ok())
    {
	ros::spinOnce();

	bool finished_within_time;
	move_arm.sendGoal(goalA);
	finished_within_time = move_arm.waitForGoalToFinish(ros::Duration(10.0));
	
	if (!finished_within_time)
	{
            move_arm.cancelGoal();
            std::cerr << "Timed out achieving goal A" << std::endl;
	}
	else
            std::cout << "Final state is " << move_arm.getTerminalState().toString() << std::endl;
	
	move_arm.sendGoal(goalB);
	finished_within_time = move_arm.waitForGoalToFinish(ros::Duration(10.0));
	
	if (!finished_within_time)
	{
            move_arm.cancelGoal();
            std::cerr << "Timed out achieving goal B" << std::endl;
	}
	else
            std::cout << "Final state is " << move_arm.getTerminalState().toString() << std::endl;
    }
    
    return 0;
}

Example #21

File: carryarm_server.cpp Project: buzzer/tams_pr2

    bool moveRightArm ( const arm_navigation_msgs::MoveArmGoal * newArmGoal )
    {
      actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm("move_right_arm",true);

      ROS_INFO("Waiting for server..");
      move_arm.waitForServer();
      ROS_INFO("Connected to server");

      arm_navigation_msgs::MoveArmGoal armGoal;
      std::vector<std::string> names(7);

      names[0] = "r_shoulder_pan_joint";
      names[1] = "r_shoulder_lift_joint";
      names[2] = "r_upper_arm_roll_joint";
      names[3] = "r_elbow_flex_joint";
      names[4] = "r_forearm_roll_joint";
      names[5] = "r_wrist_flex_joint";
      names[6] = "r_wrist_roll_joint";

      armGoal.motion_plan_request.group_name = "right_arm";
      armGoal.motion_plan_request.num_planning_attempts = 1;
      armGoal.motion_plan_request.allowed_planning_time = ros::Duration(5.0);

      armGoal.motion_plan_request.planner_id= std::string("");
      armGoal.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
      armGoal.motion_plan_request.goal_constraints.joint_constraints.resize(names.size());

      for (unsigned int i = 0 ; i < armGoal.motion_plan_request.goal_constraints.joint_constraints.size(); ++i)
      {
        armGoal.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = names[i];
        armGoal.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0;
        armGoal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.1;
        armGoal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.1;
        // Set the joint values
        armGoal.motion_plan_request.goal_constraints.joint_constraints[i].position =
          newArmGoal->motion_plan_request.goal_constraints.joint_constraints[i].position;
      }

      bool result = true;
      bool success = true;

      if (nh_.ok())
      {
        bool finished_within_time = false;
        ROS_INFO("Sending arm goal..");
        move_arm.sendGoal(armGoal);
        finished_within_time = move_arm.waitForResult(ros::Duration(200.0));

        if (!finished_within_time)
        {
          move_arm.cancelGoal();
          ROS_INFO("Timed out achieving arm goal");
          success = false;
        }
        else
        {
          actionlib::SimpleClientGoalState state = move_arm.getState();
          success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
          if(success)
            ROS_INFO("Action finished: %s",state.toString().c_str());
          else
            ROS_INFO("Action failed: %s",state.toString().c_str());

        }
      }
      result = success;

      return result;
    }