bool IMarkerRobotState::setToRandomState(double clearance) { static const std::size_t MAX_ATTEMPTS = 1000; for (std::size_t attempt = 0; attempt < MAX_ATTEMPTS; ++attempt) { // Set each planning group to random for (std::size_t i = 0; i < arm_datas_.size(); ++i) { imarker_state_->setToRandomPositions(arm_datas_[i].jmg_); } // Update transforms imarker_state_->update(); planning_scene_monitor::LockedPlanningSceneRO planning_scene(psm_); // Read only lock // Collision check // which planning group to collision check, "" is everything static const bool verbose = false; if (planning_scene->isStateValid(*imarker_state_, "", verbose)) { // Check clearance if (clearance > 0) { // which planning group to collision check, "" is everything if (planning_scene->distanceToCollision(*imarker_state_) < clearance) { continue; // clearance is not enough } } ROS_INFO_STREAM_NAMED(name_, "Found valid random robot state after " << attempt << " attempts"); // Set updated pose from robot state for (std::size_t i = 0; i < arm_datas_.size(); ++i) end_effectors_[i]->setPoseFromRobotState(); // Send to imarker for (std::size_t i = 0; i < arm_datas_.size(); ++i) end_effectors_[i]->sendUpdatedIMarkerPose(); return true; } if (attempt == 100) ROS_WARN_STREAM_NAMED(name_, "Taking long time to find valid random state"); } ROS_ERROR_STREAM_NAMED(name_, "Unable to find valid random robot state for imarker after " << MAX_ATTEMPTS << " attem" "pts"); return false; }
bool IMarkerRobotState::isStateValid(bool verbose) { // Update transforms imarker_state_->update(); planning_scene_monitor::LockedPlanningSceneRO planning_scene(psm_); // Read only lock // which planning group to collision check, "" is everything if (planning_scene->isStateValid(*imarker_state_, "", verbose)) { return true; } return false; }
int main(int argc, char **argv) { ros::init (argc, argv, "arm_kinematics"); ros::AsyncSpinner spinner(1); spinner.start(); planning_scene_monitor::PlanningSceneMonitor psm("robot_description"); psm.startStateMonitor(); sleep(1); /* Load the robot model */ robot_model_loader::RobotModelLoader robot_model_loader("robot_description"); /* Get a shared pointer to the model */ robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel(); /* Construct a planning scene - NOTE: this is for illustration purposes only. The recommended way to construct a planning scene is to use the planning_scene_monitor to construct it for you.*/ planning_scene::PlanningScene planning_scene(kinematic_model); // planning_scene::PlanningScene planning_scene = *psm.getPlanningScene(); /* The planning scene contains a RobotState *representation of the robot configuration We can get a reference to it.*/ robot_state::RobotState& current_state = planning_scene.getCurrentStateNonConst(); /* COLLISION CHECKING */ /* Let's check if the current state is in self-collision. All self-collision checks use an unpadded version of the robot collision model, i.e. no extra padding is applied to the robot.*/ collision_detection::CollisionRequest collision_request; collision_detection::CollisionResult collision_result; planning_scene.checkSelfCollision(collision_request, collision_result); ROS_INFO_STREAM("Test 1 : Current(joint_state_values.front() > 0.0 state is " << (collision_result.collision ? "in" : "not in") << " self collision"); /* Let's change the current state that the planning scene has and check if that is in self-collision*/ current_state.setToRandomValues(); collision_result.clear(); planning_scene.checkSelfCollision(collision_request, collision_result); ROS_INFO_STREAM("Test 2 : Current state is " << (collision_result.collision ? "in" : "not in") << " self collision"); /* Now, we will do collision checking only for the right_arm of the PR2, i.e. we will check whether there are any collisions between the right arm and other parts of the body of the robot.*/ collision_request.group_name = "manipulator"; current_state.setToRandomValues(); collision_result.clear(); planning_scene.checkSelfCollision(collision_request, collision_result); ROS_INFO_STREAM("Test 3: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision"); /* We will first manually set the right arm to a position where we know internal (self) collisions do happen.*/ std::vector<double> joint_values; robot_state::JointStateGroup* joint_state_group = current_state.getJointStateGroup("manipulator"); joint_state_group->getVariableValues(joint_values); joint_values[0] = 1.57; //hard-coded since we know collisions will happen here joint_state_group->setVariableValues(joint_values); /* Note that this state is now actually outside the joint limits of the PR2, which we can also check for directly.*/ ROS_INFO_STREAM("Current state is " << (current_state.satisfiesBounds() ? "valid" : "not valid")); /* Now, we will try and get contact information for any collisions that might have happened at a given configuration of the right arm. */ collision_request.contacts = true; collision_request.max_contacts = 1000; collision_result.clear(); planning_scene.checkSelfCollision(collision_request, collision_result); ROS_INFO_STREAM("Test 4: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision"); for(collision_detection::CollisionResult::ContactMap::const_iterator it = collision_result.contacts.begin(); it != collision_result.contacts.end(); ++it) { ROS_INFO("Contact between: %s and %s", it->first.first.c_str(), it->first.second.c_str()); } /* We will pass in a changed collision matrix to allow the particular set of contacts that just happened*/ collision_detection::AllowedCollisionMatrix acm = planning_scene.getAllowedCollisionMatrix(); robot_state::RobotState copied_state = planning_scene.getCurrentState(); for(collision_detection::CollisionResult::ContactMap::const_iterator it = collision_result.contacts.begin(); it != collision_result.contacts.end(); ++it) { acm.setEntry(it->first.first, it->first.second, true); } collision_result.clear(); planning_scene.checkSelfCollision(collision_request, collision_result, copied_state, acm); ROS_INFO_STREAM("Test 5: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision"); /* While we have been checking for self-collisions, we can use the checkCollision functions instead which will check for both self-collisions and for collisions with the environment (which is currently empty). This is the set of collision checking functions that you will use most often in a planner. Note that collision checks with the environment will use the padded version of the robot. Padding helps in keeping the robot further away from int_state_values.front() > 0.0 obstacles in the environment.*/ collision_result.clear(); planning_scene.checkCollision(collision_request, collision_result, copied_state, acm); ROS_INFO_STREAM("Test 6: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision"); /* CONSTRAINT CHECKING*/ /* Let's first create a constraint for the end-effector of the right arm of the PR2 */ const robot_model::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("manipulator"); std::string end_effector_name = joint_model_group->getLinkModelNames().back(); geometry_msgs::PoseStamped desired_pose; desired_pose.pose.orientation.w = 1.0; desired_pose.pose.position.x = 0.75; desired_pose.pose.position.y = -0.185; desired_pose.pose.position.z = 1.3; desired_pose.header.frame_id = "base_link"; moveit_msgs::Constraints goal_constraint = kinematic_constraints::constructGoalConstraints(end_effector_name, desired_pose); /* Now, we can directly check it for a state using the PlanningScene class*/ copied_state.setToRandomValues(); bool state_constrained = planning_scene.isStateConstrained(copied_state, goal_constraint); ROS_INFO_STREAM("Test 7: Random state is " << (state_constrained ? "constrained" : "not constrained")); /* There's a more efficient way of checking constraints (when you want to check the same constraint over and over again, e.g. inside a planner). We first construct a KinematicConstraintSet which pre-processes the ROS Constraints messages and sets it up for quick processing. */ kinematic_constraints::KinematicConstraintSet kinematic_constraint_set(kinematic_model); kinematic_constraint_set.add(goal_constraint, planning_scene.getTransforms()); bool state_constrained_2 = planning_scene.isStateConstrained(copied_state, kinematic_constraint_set); ROS_INFO_STREAM("Test 8: Random state is " << (state_constrained_2 ? "constrained" : "not constrained")); /* There's an even more efficient way to do this using the KinematicConstraintSet class directly.*/ kinematic_constraints::ConstraintEvaluationResult constraint_eval_result = kinematic_constraint_set.decide(copied_state); ROS_INFO_STREAM("Test 9: Random state is " << (constraint_eval_result.satisfied ? "constrained" : "not constrained")); /* Now, lets try a user-defined callback. This is done by specifying the callback using the setStateFeasibilityPredicate function to which you pass the desired callback. Now, whenever anyone calls isStateFeasible, the callback will be checked.*/ planning_scene.setStateFeasibilityPredicate(userCallback); bool state_feasible = planning_scene.isStateFeasible(copied_state); ROS_INFO_STREAM("Test 10: Random state is " << (state_feasible ? "feasible" : "not feasible")); /* Whenever anyone calls isStateValid, three checks are conducted: (a) collision checking (b) constraint checking and (c) feasibility checking using the user-defined callback */ bool state_valid = planning_scene.isStateValid(copied_state, kinematic_constraint_set, "manipulator"); ROS_INFO_STREAM("Test 10: Random state is " << (state_valid ? "valid" : "not valid")); ros::shutdown(); return 0; }
int main(int argc, char **argv) { ros::init (argc, argv, "move_group_tutorial"); ros::AsyncSpinner spinner(1); spinner.start(); ros::NodeHandle node_handle("motion_planner_api"); robot_model_loader::RobotModelLoader robot_model_loader("robot_description"); robot_model::RobotModelPtr robot_model = robot_model_loader.getModel(); planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model)); boost::scoped_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager> > planner_plugin_loader; planning_interface::PlannerManagerPtr planner_instance; std::string planner_plugin_name; if (!node_handle.getParam("/move_group/planning_plugin", planner_plugin_name)) ROS_FATAL_STREAM("Could not find planner plugin name"); try { planner_plugin_loader.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>("moveit_core", "planning_interface::PlannerManager")); } catch(pluginlib::PluginlibException& ex) { ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what()); } try { planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name)); if (!planner_instance->initialize(robot_model, node_handle.getNamespace())) ROS_FATAL_STREAM("Could not initialize planner instance"); ROS_INFO_STREAM("Using planning interface '" << planner_instance->getDescription() << "'"); } catch(pluginlib::PluginlibException& ex) { const std::vector<std::string> &classes = planner_plugin_loader->getDeclaredClasses(); std::stringstream ss; for (std::size_t i = 0 ; i < classes.size() ; ++i) ss << classes[i] << " "; ROS_ERROR_STREAM("Exception while loading planner '" << planner_plugin_name << "': " << ex.what() << std::endl << "Available plugins: " << ss.str()); } ros::WallDuration sleep_time(15.0); sleep_time.sleep(); planning_interface::MotionPlanRequest req; planning_interface::MotionPlanResponse res; geometry_msgs::PoseStamped pose; pose.header.frame_id = "odom"; pose.pose.position.x = -0.0119214; pose.pose.position.y = 0.0972478; pose.pose.position.z = 0.636616; pose.pose.orientation.x = 0.273055; pose.pose.orientation.y = -0.891262; pose.pose.orientation.z = -0.346185; pose.pose.orientation.w = 0.106058; group.setPoseTarget(target_pose1); moveit::planning_interface::MoveGroup::Plan my_plan; bool success = group.plan(my_plan); if (1) { ROS_INFO("Visualizing plan 1 (again)"); display_trajectory.trajectory_start = my_plan.start_state_; display_trajectory.trajectory.push_back(my_plan.trajectory_); display_publisher.publish(display_trajectory); /* Sleep to give Rviz time to visualize the plan. */ sleep(5.0); } std::vector<double> group_variable_values; group.getCurrentState()->copyJointGroupPositions(group.getCurrentState()->getRobotModel()->getJointModelGroup(group.getName()), group_variable_values); group_variable_values[4] = -1.0; group.setJointValueTarget(group_variable_values); success = group.plan(my_plan); ROS_INFO("Visualizing plan 2 (joint space goal) %s",success?"":"FAILED"); sleep(5.0); return 0; }
int main(int argc, char **argv) { /*Initialise Variables*/ CAPTURE_MOVEMENT=false;//know when you have reach the maximum of points to handle //Creating the joint_msg_leap joint_msg_leap.name.resize(8); joint_msg_leap.position.resize(8); joint_msg_leap.name[0]="arm_1_joint"; joint_msg_leap.name[1]="arm_2_joint"; joint_msg_leap.name[2]="arm_3_joint"; joint_msg_leap.name[3]="arm_4_joint"; joint_msg_leap.name[4]="arm_5_joint"; joint_msg_leap.name[5]="arm_6_joint"; joint_msg_leap.name[6]="base_joint_gripper_left"; joint_msg_leap.name[7]="base_joint_gripper_right"; aux_enter=1; FIRST_VALUE=true;//Help knowing Initial Position of the hand int arm_trajectory_point=1; collision_detection::CollisionRequest collision_request; collision_detection::CollisionResult collision_result; /*Finish Variables Initialitation*/ //ROS DECLARATION ros::init(argc, argv,"listener"); if (argc != 2) { ROS_WARN("WARNING: you should specify number of points to capture"); } else { count=atoi(argv[1]); ROS_INFO ("Number of points /n%d", count); } //Create an object of the LeapMotionListener Class LeapMotionListener leapmotionlistener; leapmotionlistener.Configure(count); //ros::NodeHandle node_handle; ros::NodeHandle node_handle("~"); // start a ROS spinning thread ros::AsyncSpinner spinner(1); spinner.start(); //we need this for leap ros::Rate r(1); //robo_pub = n.advertise<sensor_msgs::JointState>("joint_leap", 100); //Creating a Robot Model robot_model_loader::RobotModelLoader robot_model_loader("robot_description"); robot_model::RobotModelPtr robot_model = robot_model_loader.getModel(); //Initialise PlanningSceneMonitorPtr /* MOVEIT Setup*/ // ^^^^^ moveit::planning_interface::MoveGroup group("arm"); group.setPlanningTime(0.5); moveit::planning_interface::MoveGroup::Plan my_plan; // We will use the :planning_scene_interface:`PlanningSceneInterface` // class to deal directly with the world. moveit::planning_interface::PlanningSceneInterface planning_scene_interface; /* Adding/Removing Objects and Attaching/Detaching Objects*/ ROS_INFO("CREATING PLANNING_SCENE PUBLISHER"); ros::Publisher planning_scene_diff_publisher = node_handle.advertise<moveit_msgs::PlanningScene>("/planning_scene", 1); while(planning_scene_diff_publisher.getNumSubscribers() < 1) { ros::WallDuration sleep_t(0.5); sleep_t.sleep(); } ROS_INFO("CREATING COLLISION OBJECT"); moveit_msgs::AttachedCollisionObject attached_object; attached_object.link_name = ""; /* The header must contain a valid TF frame*/ attached_object.object.header.frame_id = "gripper_left"; /* The id of the object */ attached_object.object.id = "box"; /* A default pose */ geometry_msgs::Pose posebox; posebox.orientation.w = 1.0; /* Define a box to be attached */ shape_msgs::SolidPrimitive primitive; primitive.type = primitive.BOX; primitive.dimensions.resize(3); primitive.dimensions[0] = 0.1; primitive.dimensions[1] = 0.1; primitive.dimensions[2] = 0.1; attached_object.object.primitives.push_back(primitive); attached_object.object.primitive_poses.push_back(posebox); attached_object.object.operation = attached_object.object.ADD; ROS_INFO("ADDING COLLISION OBJECT TO THE WORLD"); moveit_msgs::PlanningScene planning_scene_msg; planning_scene_msg.world.collision_objects.push_back(attached_object.object); planning_scene_msg.is_diff = true; planning_scene_diff_publisher.publish(planning_scene_msg); //sleep_time.sleep(); /* First, define the REMOVE object message*/ moveit_msgs::CollisionObject remove_object; remove_object.id = "box"; remove_object.header.frame_id = "odom_combined"; remove_object.operation = remove_object.REMOVE; /* Carry out the REMOVE + ATTACH operation */ ROS_INFO("Attaching the object to the right wrist and removing it from the world."); planning_scene_msg.world.collision_objects.clear(); planning_scene_msg.world.collision_objects.push_back(remove_object); planning_scene_msg.robot_state.attached_collision_objects.push_back(attached_object); planning_scene_diff_publisher.publish(planning_scene_msg); //sleep_time.sleep(); // Create a publisher for visualizing plans in Rviz. ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true); planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model,collision_detection::WorldPtr(new collision_detection::World()))); //Creating planning_scene_monitor boost::shared_ptr<tf::TransformListener> tf(new tf::TransformListener(ros::Duration(2.0))); planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor(new planning_scene_monitor::PlanningSceneMonitor(planning_scene,"robot_description", tf)); planning_scene_monitor->startSceneMonitor(); //planning_scene_monitor->initialize(planning_scene); planning_pipeline::PlanningPipeline *planning_pipeline= new planning_pipeline::PlanningPipeline(robot_model,node_handle,"planning_plugin", "request_adapters"); /* Sleep a little to allow time to startup rviz, etc. */ ros::WallDuration sleep_time(20.0); sleep_time.sleep(); /*end of MOVEIT Setup*/ // We can print the name of the reference frame for this robot. ROS_INFO("Reference frame: %s", group.getPlanningFrame().c_str()); // We can also print the name of the end-effector link for this group. ROS_INFO("Reference frame: %s", group.getEndEffectorLink().c_str()); // Planning to a Pose goal 1 // ^^^^^^^^^^^^^^^^^^^^^^^ // We can plan a motion for this group to a desired pose for the // end-effector ROS_INFO("Planning to INITIAL POSE"); planning_interface::MotionPlanRequest req; planning_interface::MotionPlanResponse res; geometry_msgs::PoseStamped pose; pose.header.frame_id = "/odom_combined"; pose.pose.position.x = 0; pose.pose.position.y = 0; pose.pose.position.z = 1.15; pose.pose.orientation.w = 1.0; std::vector<double> tolerance_pose(3, 0.01); std::vector<double> tolerance_angle(3, 0.01); old_pose=pose; // We will create the request as a constraint using a helper function available req.group_name = "arm"; moveit_msgs::Constraints pose_goal = kinematic_constraints::constructGoalConstraints("gripper_base_link", pose, tolerance_pose, tolerance_angle); req.goal_constraints.push_back(pose_goal); // Now, call the pipeline and check whether planning was successful. planning_pipeline->generatePlan(planning_scene, req, res); /* Check that the planning was successful */ if(res.error_code_.val != res.error_code_.SUCCESS) { ROS_ERROR("Could not compute plan successfully"); return 0; } // Visualize the result // ^^^^^^^^^^^^^^^^^^^^ /* Visualize the trajectory */ moveit_msgs::DisplayTrajectory display_trajectory; ROS_INFO("Visualizing the trajectory 1"); moveit_msgs::MotionPlanResponse response; res.getMessage(response); display_trajectory.trajectory_start = response.trajectory_start; display_trajectory.trajectory.push_back(response.trajectory); display_publisher.publish(display_trajectory); //sleep_time.sleep(); /* End Planning to a Pose goal 1*/ // First, set the state in the planning scene to the final state of the last plan robot_state::RobotState& robot_state = planning_scene->getCurrentStateNonConst(); //planning_scene->setCurrentState(response.trajectory_start); joint_model_group = robot_state.getJointModelGroup("arm"); robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions); spinner.stop(); /*Capturing Stage*/ /*****************/ ROS_INFO("PRESH ENTER TO START CAPTURING POINTS"); while (getline(std::cin,s)) { if ('\n' == getchar()) break; } /* SENSOR SUBSCRIBING */ //LEAP MOTION ROS_INFO("SUBSCRIBING LEAPMOTION"); ros::Subscriber leapsub = node_handle.subscribe("/leapmotion/data", 1000, &LeapMotionListener::leapmotionCallback, &leapmotionlistener); ros::Subscriber trajectorysub = node_handle.subscribe("/move_group/", 1000, &LeapMotionListener::leapmotionCallback, &leapmotionlistener); while(!CAPTURE_MOVEMENT==true) { ros::spinOnce(); } leapsub.shutdown(); ROS_INFO("CAPTURING POINTS FINISH...PROCESSING POINTS"); // End of Capturing Stage /* Start Creating Arm Trajectory*/ /**********************************/ ROS_INFO("START CREATING ARM TRAJECTORY"); for (unsigned i=0; i<trajectory_hand.size(); i++) { //First we set the initial Position of the Hand if (FIRST_VALUE) { dataLastHand_.palmpos.x=trajectory_hand.at(i).palmpos.x; dataLastHand_.palmpos.y=trajectory_hand.at(i).palmpos.y; dataLastHand_.palmpos.z=trajectory_hand.at(i).palmpos.z; FIRST_VALUE=0; ROS_INFO("ORIGINAL POSITION OF THE HAND SET TO \n X: %f\n Y: %f\n Z: %f\n ",trajectory_hand.at(i).palmpos.x,trajectory_hand.at(i).palmpos.y,trajectory_hand.at(i).palmpos.z); sleep(2); } else { // Both limits for x,y,z to avoid small changes Updifferencex=dataLastHand_.palmpos.x+10; Downdifferencex=dataLastHand_.palmpos.x-10; Updifferencez=dataLastHand_.palmpos.z+10; Downdifferencez=dataLastHand_.palmpos.z-20; Updifferencey=dataLastHand_.palmpos.y+20; Downdifferencey=dataLastHand_.palmpos.y-20; if ((trajectory_hand.at(i).palmpos.x<Downdifferencex)||(trajectory_hand.at(i).palmpos.x>Updifferencex)||(trajectory_hand.at(i).palmpos.y<Downdifferencey)||(trajectory_hand.at(i).palmpos.y>Updifferencey)||(trajectory_hand.at(i).palmpos.z<Downdifferencez)||(trajectory_hand.at(i).palmpos.z>Updifferencez)) { ros::AsyncSpinner spinner(1); spinner.start(); ROS_INFO("TRYING TO ADD POINT %d TO TRAJECTORY",arm_trajectory_point); // Cartesian Paths // ^^^^^^^^^^^^^^^ // You can plan a cartesian path directly by specifying a list of waypoints // for the end-effector to go through. Note that we are starting // from the new start state above. The initial pose (start state) does not // need to be added to the waypoint list. pose.header.frame_id = "/odom_combined"; pose.pose.orientation.w = 1.0; pose.pose.position.y +=(trajectory_hand.at(i).palmpos.x-dataLastHand_.palmpos.x)/500 ; pose.pose.position.z +=(trajectory_hand.at(i).palmpos.y-dataLastHand_.palmpos.y)/1000 ; if(pose.pose.position.z>Uplimitez) pose.pose.position.z=Uplimitez; pose.pose.position.x +=-(trajectory_hand.at(i).palmpos.z-dataLastHand_.palmpos.z)/500 ; ROS_INFO("END EFFECTOR POSITION \n X: %f\n Y: %f\n Z: %f\n", pose.pose.position.x,pose.pose.position.y,pose.pose.position.z); ROS_INFO("Palmpos \n X: %f\n Y: %f\n Z: %f\n ",trajectory_hand.at(i).palmpos.x,trajectory_hand.at(i).palmpos.y,trajectory_hand.at(i).palmpos.z); // We will create the request as a constraint using a helper function available ROS_INFO("1"); pose_goal= kinematic_constraints::constructGoalConstraints("gripper_base_link", pose, tolerance_pose, tolerance_angle); ROS_INFO("2"); req.goal_constraints.push_back(pose_goal); // Now, call the pipeline and check whether planning was successful. planning_pipeline->generatePlan(planning_scene, req, res); ROS_INFO("3"); if(res.error_code_.val != res.error_code_.SUCCESS) { ROS_ERROR("Could not compute plan successfully"); pose=old_pose; } else { // Now when we plan a trajectory it will avoid the obstacle res.getMessage(response); collision_result.clear(); collision_detection::AllowedCollisionMatrix acm = planning_scene->getAllowedCollisionMatrix(); robot_state::RobotState copied_state = planning_scene->getCurrentState(); planning_scene->checkCollision(collision_request, collision_result, copied_state, acm); ROS_INFO_STREAM("Collision Test "<< (collision_result.collision ? "in" : "not in")<< " collision"); arm_trajectory_point++; // Visualize the trajectory ROS_INFO("VISUALIZING NEW POINT"); //req.goal_constraints.clear(); display_trajectory.trajectory_start = response.trajectory_start; ROS_INFO("AXIS 1 NEXT TRAJECTORY IS %f\n",response.trajectory_start.joint_state.position[1] ); display_trajectory.trajectory.push_back(response.trajectory); // Now you should see two planned trajectories in series display_publisher.publish(display_trajectory); planning_scene->setCurrentState(response.trajectory_start); if (planning_scene_monitor->updatesScene(planning_scene)) { ROS_INFO("CHANGING STATE"); planning_scene_monitor->updateSceneWithCurrentState(); } else { ROS_ERROR("NOT POSSIBLE TO CHANGE THE SCENE"); } robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions); req.goal_constraints.clear(); old_pose=pose; dataLastHand_.palmpos.x=trajectory_hand.at(i).palmpos.x; dataLastHand_.palmpos.y=trajectory_hand.at(i).palmpos.y; dataLastHand_.palmpos.z=trajectory_hand.at(i).palmpos.z; } //sleep(2); spinner.stop(); } } } //ros::Subscriber myogestsub = n.subscribe("/myo_gest", 1000, myogestCallback); return 0; }
int main(int argc, char **argv) { ros::init (argc, argv, "move_group_tutorial"); ros::AsyncSpinner spinner(1); spinner.start(); ros::NodeHandle node_handle("move_group"); // BEGIN_TUTORIAL // Start // ^^^^^ // Setting up to start using a planner is pretty easy. Planners are // setup as plugins in MoveIt! and you can use the ROS pluginlib // interface to load any planner that you want to use. Before we // can load the planner, we need two objects, a RobotModel // and a PlanningScene. // We will start by instantiating a // `RobotModelLoader`_ // object, which will look up // the robot description on the ROS parameter server and construct a // :moveit_core:`RobotModel` for us to use. // // .. _RobotModelLoader: http://docs.ros.org/api/moveit_ros_planning/html/classrobot__model__loader_1_1RobotModelLoader.html robot_model_loader::RobotModelLoader robot_model_loader("robot_description"); robot_model::RobotModelPtr robot_model = robot_model_loader.getModel(); // Using the :moveit_core:`RobotModel`, we can construct a // :planning_scene:`PlanningScene` that maintains the state of // the world (including the robot). planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model)); // We will now construct a loader to load a planner, by name. // Note that we are using the ROS pluginlib library here. boost::scoped_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager> > planner_plugin_loader; planning_interface::PlannerManagerPtr planner_instance; std::string planner_plugin_name; // We will get the name of planning plugin we want to load // from the ROS param server, and then load the planner // making sure to catch all exceptions. if (!node_handle.getParam("planning_plugin", planner_plugin_name)) ROS_FATAL_STREAM("Could not find planner plugin name"); try { planner_plugin_loader.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>("moveit_core", "planning_interface::PlannerManager")); } catch(pluginlib::PluginlibException& ex) { ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what()); } try { planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name)); if (!planner_instance->initialize(robot_model, node_handle.getNamespace())) ROS_FATAL_STREAM("Could not initialize planner instance"); ROS_INFO_STREAM("Using planning interface '" << planner_instance->getDescription() << "'"); } catch(pluginlib::PluginlibException& ex) { const std::vector<std::string> &classes = planner_plugin_loader->getDeclaredClasses(); std::stringstream ss; for (std::size_t i = 0 ; i < classes.size() ; ++i) ss << classes[i] << " "; ROS_ERROR_STREAM("Exception while loading planner '" << planner_plugin_name << "': " << ex.what() << std::endl << "Available plugins: " << ss.str()); } /* Sleep a little to allow time to startup rviz, etc. */ // ros::WallDuration sleep_time(15.0); ros::WallDuration sleep_time(1); sleep_time.sleep(); // Pose Goal // ^^^^^^^^^ // We will now create a motion plan request for the right arm of the PR2 // specifying the desired pose of the end-effector as input. planning_interface::MotionPlanRequest req; planning_interface::MotionPlanResponse res; geometry_msgs::PoseStamped pose; pose.header.frame_id = "base"; pose.pose.position.x = 0.3; pose.pose.position.y = 0.0; pose.pose.position.z = 0.3; pose.pose.orientation.w = 1.0; // A tolerance of 0.01 m is specified in position // and 0.01 radians in orientation std::vector<double> tolerance_pose(3, 0.01); std::vector<double> tolerance_angle(3, 0.01); // We will create the request as a constraint using a helper function available // from the // `kinematic_constraints`_ // package. // // .. _kinematic_constraints: http://docs.ros.org/api/moveit_core/html/namespacekinematic__constraints.html#a88becba14be9ced36fefc7980271e132 req.group_name = "manipulator"; moveit_msgs::Constraints pose_goal = kinematic_constraints::constructGoalConstraints("tool0", pose, tolerance_pose, tolerance_angle); req.goal_constraints.push_back(pose_goal); // We now construct a planning context that encapsulate the scene, // the request and the response. We call the planner using this // planning context planning_interface::PlanningContextPtr context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_); context->solve(res); if(res.error_code_.val != res.error_code_.SUCCESS) { ROS_ERROR("Could not compute plan successfully: %d", (int) res.error_code_.val); return 0; } // Visualize the result // ^^^^^^^^^^^^^^^^^^^^ ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true); moveit_msgs::DisplayTrajectory display_trajectory; /* Visualize the trajectory */ ROS_INFO("Visualizing the trajectory"); moveit_msgs::MotionPlanResponse response; res.getMessage(response); display_trajectory.trajectory_start = response.trajectory_start; display_trajectory.trajectory.push_back(response.trajectory); display_publisher.publish(display_trajectory); sleep_time.sleep(); // Joint Space Goals // ^^^^^^^^^^^^^^^^^ /* First, set the state in the planning scene to the final state of the last plan */ robot_state::RobotState& robot_state = planning_scene->getCurrentStateNonConst(); planning_scene->setCurrentState(response.trajectory_start); #if 0 const robot_state::JointModelGroup* joint_model_group = robot_state.getJointModelGroup("manipulator"); robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions); // Now, setup a joint space goal robot_state::RobotState goal_state(robot_model); std::vector<double> joint_values(7, 0.0); joint_values[0] = -2.0; joint_values[3] = -0.2; joint_values[5] = -0.15; goal_state.setJointGroupPositions(joint_model_group, joint_values); moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group); req.goal_constraints.clear(); req.goal_constraints.push_back(joint_goal); // Call the planner and visualize the trajectory /* Re-construct the planning context */ context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_); /* Call the Planner */ context->solve(res); /* Check that the planning was successful */ if(res.error_code_.val != res.error_code_.SUCCESS) { ROS_ERROR("Could not compute plan successfully"); return 0; } /* Visualize the trajectory */ ROS_INFO("Visualizing the trajectory"); res.getMessage(response); display_trajectory.trajectory_start = response.trajectory_start; display_trajectory.trajectory.push_back(response.trajectory); /* Now you should see two planned trajectories in series*/ display_publisher.publish(display_trajectory); /* We will add more goals. But first, set the state in the planning scene to the final state of the last plan */ robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions); /* Now, we go back to the first goal*/ req.goal_constraints.clear(); req.goal_constraints.push_back(pose_goal); context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_); context->solve(res); res.getMessage(response); display_trajectory.trajectory.push_back(response.trajectory); display_publisher.publish(display_trajectory); // Adding Path Constraints // ^^^^^^^^^^^^^^^^^^^^^^^ // Let's add a new pose goal again. This time we will also add a path constraint to the motion. /* Let's create a new pose goal */ pose.pose.position.x = 0.65; pose.pose.position.y = -0.2; pose.pose.position.z = -0.1; moveit_msgs::Constraints pose_goal_2 = kinematic_constraints::constructGoalConstraints("tool0", pose, tolerance_pose, tolerance_angle); /* First, set the state in the planning scene to the final state of the last plan */ robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions); /* Now, let's try to move to this new pose goal*/ req.goal_constraints.clear(); req.goal_constraints.push_back(pose_goal_2); /* But, let's impose a path constraint on the motion. Here, we are asking for the end-effector to stay level*/ geometry_msgs::QuaternionStamped quaternion; quaternion.header.frame_id = "torso_lift_link"; quaternion.quaternion.w = 1.0; req.path_constraints = kinematic_constraints::constructGoalConstraints("tool0", quaternion); // Imposing path constraints requires the planner to reason in the space of possible positions of the end-effector // (the workspace of the robot) // because of this, we need to specify a bound for the allowed planning volume as well; // Note: a default bound is automatically filled by the WorkspaceBounds request adapter (part of the OMPL pipeline, // but that is not being used in this example). // We use a bound that definitely includes the reachable space for the arm. This is fine because sampling is not done in this volume // when planning for the arm; the bounds are only used to determine if the sampled configurations are valid. req.workspace_parameters.min_corner.x = req.workspace_parameters.min_corner.y = req.workspace_parameters.min_corner.z = -2.0; req.workspace_parameters.max_corner.x = req.workspace_parameters.max_corner.y = req.workspace_parameters.max_corner.z = 2.0; // Call the planner and visualize all the plans created so far. context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_); context->solve(res); res.getMessage(response); display_trajectory.trajectory.push_back(response.trajectory); // Now you should see four planned trajectories in series display_publisher.publish(display_trajectory); #endif //END_TUTORIAL sleep_time.sleep(); ROS_INFO("Done"); planner_instance.reset(); return 0; }
int main(int argc, char **argv) { ros::init (argc, argv, "arm_kinematics"); ros::AsyncSpinner spinner(1); spinner.start(); // BEGIN_TUTORIAL // // Setup // ^^^^^ // // The :planning_scene:`PlanningScene` class can be easily setup and // configured using a :moveit_core:`RobotModel` or a URDF and // SRDF. This is, however, not the recommended way to instantiate a // PlanningScene. The :planning_scene_monitor:`PlanningSceneMonitor` // is the recommended method to create and maintain the current // planning scene (and is discussed in detail in the next tutorial) // using data from the robot's joints and the sensors on the robot. In // this tutorial, we will instantiate a PlanningScene class directly, // but this method of instantiation is only intended for illustration. robot_model_loader::RobotModelLoader robot_model_loader("robot_description"); robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel(); planning_scene::PlanningScene planning_scene(kinematic_model); // sleep(5); // Collision Checking // ^^^^^^^^^^^^^^^^^^ // // Self-collision checking // ~~~~~~~~~~~~~~~~~~~~~~~ // // The first thing we will do is check whether the robot in its // current state is in *self-collision*, i.e. whether the current // configuration of the robot would result in the robot's parts // hitting each other. To do this, we will construct a // :collision_detection_struct:`CollisionRequest` object and a // :collision_detection_struct:`CollisionResult` object and pass them // into the collision checking function. Note that the result of // whether the robot is in self-collision or not is contained within // the result. Self collision checking uses an *unpadded* version of // the robot, i.e. it directly uses the collision meshes provided in // the URDF with no extra padding added on. collision_detection::CollisionRequest collision_request; collision_detection::CollisionResult collision_result; planning_scene.checkSelfCollision(collision_request, collision_result); ROS_INFO_STREAM("Test 1: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision"); // Change the state // ~~~~~~~~~~~~~~~~ // // Now, let's change the current state of the robot. The planning // scene maintains the current state internally. We can get a // reference to it and change it and then check for collisions for the // new robot configuration. Note in particular that we need to clear // the collision_result before making a new collision checking // request. robot_state::RobotState& current_state = planning_scene.getCurrentStateNonConst(); current_state.setToRandomPositions(); collision_result.clear(); planning_scene.checkSelfCollision(collision_request, collision_result); ROS_INFO_STREAM("Test 2: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision"); // Checking for a group // ~~~~~~~~~~~~~~~~~~~~ // // Now, we will do collision checking only for the right_arm of the // PR2, i.e. we will check whether there are any collisions between // the right arm and other parts of the body of the robot. We can ask // for this specifically by adding the group name "right_arm" to the // collision request. collision_request.group_name = "arm"; current_state.setToRandomPositions(); collision_result.clear(); planning_scene.checkSelfCollision(collision_request, collision_result); ROS_INFO_STREAM("Test 3: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision"); // Getting Contact Information // ~~~~~~~~~~~~~~~~~~~~~~~~~~~ // // First, manually set the right arm to a position where we know // internal (self) collisions do happen. Note that this state is now // actually outside the joint limits of the PR2, which we can also // check for directly. std::vector<double> joint_values; const robot_model::JointModelGroup* joint_model_group = current_state.getJointModelGroup("arm"); current_state.copyJointGroupPositions(joint_model_group, joint_values); joint_values[0] = 1.57; //hard-coded since we know collisions will happen here current_state.setJointGroupPositions(joint_model_group, joint_values); ROS_INFO_STREAM("Current state is " << (current_state.satisfiesBounds(joint_model_group) ? "valid" : "not valid")); // Now, we can get contact information for any collisions that might // have happened at a given configuration of the right arm. We can ask // for contact information by filling in the appropriate field in the // collision request and specifying the maximum number of contacts to // be returned as a large number. collision_request.contacts = true; collision_request.max_contacts = 1000; // collision_result.clear(); planning_scene.checkSelfCollision(collision_request, collision_result); ROS_INFO_STREAM("Test 4: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision"); collision_detection::CollisionResult::ContactMap::const_iterator it; for(it = collision_result.contacts.begin(); it != collision_result.contacts.end(); ++it) { ROS_INFO("Contact between: %s and %s", it->first.first.c_str(), it->first.second.c_str()); } // Modifying the Allowed Collision Matrix // ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // // The :collision_detection_class:`AllowedCollisionMatrix` (ACM) // provides a mechanism to tell the collision world to ignore // collisions between certain object: both parts of the robot and // objects in the world. We can tell the collision checker to ignore // all collisions between the links reported above, i.e. even though // the links are actually in collision, the collision checker will // ignore those collisions and return not in collision for this // particular state of the robot. // // Note also in this example how we are making copies of both the // allowed collision matrix and the current state and passing them in // to the collision checking function. collision_detection::AllowedCollisionMatrix acm = planning_scene.getAllowedCollisionMatrix(); robot_state::RobotState copied_state = planning_scene.getCurrentState(); collision_detection::CollisionResult::ContactMap::const_iterator it2; for(it2 = collision_result.contacts.begin(); it2 != collision_result.contacts.end(); ++it2) { acm.setEntry(it2->first.first, it2->first.second, true); } collision_result.clear(); planning_scene.checkSelfCollision(collision_request, collision_result, copied_state, acm); ROS_INFO_STREAM("Test 5: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision"); // Full Collision Checking // ~~~~~~~~~~~~~~~~~~~~~~~ // // While we have been checking for self-collisions, we can use the // checkCollision functions instead which will check for both // self-collisions and for collisions with the environment (which is // currently empty). This is the set of collision checking // functions that you will use most often in a planner. Note that // collision checks with the environment will use the padded version // of the robot. Padding helps in keeping the robot further away // from obstacles in the environment.*/ collision_result.clear(); planning_scene.checkCollision(collision_request, collision_result, copied_state, acm); ROS_INFO_STREAM("Test 6: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision"); // Constraint Checking // ^^^^^^^^^^^^^^^^^^^ // // The PlanningScene class also includes easy to use function calls // for checking constraints. The constraints can be of two types: // (a) constraints chosen from the // :kinematic_constraints:`KinematicConstraint` set: // i.e. :kinematic_constraints:`JointConstraint`, // :kinematic_constraints:`PositionConstraint`, // :kinematic_constraints:`OrientationConstraint` and // :kinematic_constraints:`VisibilityConstraint` and (b) user // defined constraints specified through a callback. We will first // look at an example with a simple KinematicConstraint. // // Checking Kinematic Constraints // ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // // We will first define a simple position and orientation constraint // on the end-effector of the right_arm of the PR2 robot. Note the // use of convenience functions for filling up the constraints // (these functions are found in the :moveit_core_files:`utils.h<utils_8h>` file from the // kinematic_constraints directory in moveit_core). std::string end_effector_name = joint_model_group->getLinkModelNames().back(); geometry_msgs::PoseStamped desired_pose; desired_pose.pose.orientation.w = 1.0; desired_pose.pose.position.x = 0.75; desired_pose.pose.position.y = -0.185; desired_pose.pose.position.z = 1.3; desired_pose.header.frame_id = "base_link"; moveit_msgs::Constraints goal_constraint = kinematic_constraints::constructGoalConstraints(end_effector_name, desired_pose); // Now, we can check a state against this constraint using the // isStateConstrained functions in the PlanningScene class. copied_state.setToRandomPositions(); copied_state.update(); bool constrained = planning_scene.isStateConstrained(copied_state, goal_constraint); ROS_INFO_STREAM("Test 7: Random state is " << (constrained ? "constrained" : "not constrained")); // There's a more efficient way of checking constraints (when you want // to check the same constraint over and over again, e.g. inside a // planner). We first construct a KinematicConstraintSet which // pre-processes the ROS Constraints messages and sets it up for quick // processing. kinematic_constraints::KinematicConstraintSet kinematic_constraint_set(kinematic_model); kinematic_constraint_set.add(goal_constraint, planning_scene.getTransforms()); bool constrained_2 = planning_scene.isStateConstrained(copied_state, kinematic_constraint_set); ROS_INFO_STREAM("Test 8: Random state is " << (constrained_2 ? "constrained" : "not constrained")); // There's a direct way to do this using the KinematicConstraintSet // class. kinematic_constraints::ConstraintEvaluationResult constraint_eval_result = kinematic_constraint_set.decide(copied_state); ROS_INFO_STREAM("Test 9: Random state is " << (constraint_eval_result.satisfied ? "constrained" : "not constrained")); // User-defined constraints // ~~~~~~~~~~~~~~~~~~~~~~~~ // // CALL_SUB_TUTORIAL userCallback // Now, whenever isStateFeasible is called, this user-defined callback // will be called. planning_scene.setStateFeasibilityPredicate(userCallback); bool state_feasible = planning_scene.isStateFeasible(copied_state); ROS_INFO_STREAM("Test 10: Random state is " << (state_feasible ? "feasible" : "not feasible")); // Whenever isStateValid is called, three checks are conducted: (a) // collision checking (b) constraint checking and (c) feasibility // checking using the user-defined callback. bool state_valid = planning_scene.isStateValid(copied_state, kinematic_constraint_set, "right_arm"); ROS_INFO_STREAM("Test 10: Random state is " << (state_valid ? "valid" : "not valid")); // Note that all the planners available through MoveIt! and OMPL will // currently perform collision checking, constraint checking and // feasibility checking using user-defined callbacks. // END_TUTORIAL ros::shutdown(); return 0; }
int main(int argc, char **argv) { ros::init (argc, argv, "motion_planning"); ros::AsyncSpinner spinner(1); spinner.start(); ros::NodeHandle node_handle("/move_group"); // ros::NodeHandle node_handle("~"); /* SETUP A PLANNING SCENE*/ /* Load the robot model */ robot_model_loader::RobotModelLoader robot_model_loader("robot_description"); /* Get a shared pointer to the model */ robot_model::RobotModelPtr robot_model = robot_model_loader.getModel(); /* Construct a planning scene - NOTE: this is for illustration purposes only. The recommended way to construct a planning scene is to use the planning_scene_monitor to construct it for you.*/ planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model)); /* SETUP THE PLANNER*/ boost::scoped_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager> > planner_plugin_loader; planning_interface::PlannerManagerPtr planner_instance; std::string planner_plugin_name; /* Get the name of the planner we want to use */ if (!node_handle.getParam("planning_plugin", planner_plugin_name)) ROS_FATAL_STREAM("Could not find planner plugin name"); /* Make sure to catch all exceptions */ try { planner_plugin_loader.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>("moveit_core", "planning_interface::PlannerManager")); } catch(pluginlib::PluginlibException& ex) { ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what()); } try { planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name)); if (!planner_instance->initialize(robot_model, node_handle.getNamespace())) ROS_FATAL_STREAM("Could not initialize planner instance"); ROS_INFO_STREAM("Using planning interface '" << planner_instance->getDescription() << "'"); } catch(pluginlib::PluginlibException& ex) { const std::vector<std::string> &classes = planner_plugin_loader->getDeclaredClasses(); std::stringstream ss; for (std::size_t i = 0 ; i < classes.size() ; ++i) ss << classes[i] << " "; ROS_ERROR_STREAM("Exception while loading planner '" << planner_plugin_name << "': " << ex.what() << std::endl << "Available plugins: " << ss.str()); } /* Sleep a little to allow time to startup rviz, etc. */ ros::WallDuration sleep_time(1.0); sleep_time.sleep(); /* CREATE A MOTION PLAN REQUEST FOR THE RIGHT ARM OF THE PR2 */ /* We will ask the end-effector of the PR2 to go to a desired location */ planning_interface::MotionPlanRequest req; planning_interface::MotionPlanResponse res; /* A desired pose */ geometry_msgs::PoseStamped pose; pose.header.frame_id = "base_link"; pose.pose.position.x = 0.3; pose.pose.position.y = -0.3; pose.pose.position.z = 0.7; pose.pose.orientation.x = 0.62478; pose.pose.orientation.y = 0.210184; pose.pose.orientation.z = -0.7107 ; pose.pose.orientation.w = 0.245722; /* A desired tolerance */ std::vector<double> tolerance_pose(3, 0.1); std::vector<double> tolerance_angle(3, 0.1); // std::vector<double> tolerance_pose(3, 0.01); // std::vector<double> tolerance_angle(3, 0.01); ROS_INFO("marker4"); /*Create the request */ req.group_name = "manipulator"; moveit_msgs::Constraints pose_goal = kinematic_constraints::constructGoalConstraints("wrist_3_link", pose, tolerance_pose, tolerance_angle); req.goal_constraints.push_back(pose_goal); ROS_INFO("marker5"); /* Construct the planning context */ planning_interface::PlanningContextPtr context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_); ROS_INFO("marker6"); /* CALL THE PLANNER */ // context->solve(res); // ROS_INFO("marker7"); /* Check that the planning was successful */ if(res.error_code_.val != res.error_code_.SUCCESS) { ROS_ERROR("Could not compute plan successfully"); return 0; } /* Visualize the generated plan */ /* Publisher for display */ ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true); moveit_msgs::DisplayTrajectory display_trajectory; /* Visualize the trajectory */ ROS_INFO("Visualizing the trajectory"); moveit_msgs::MotionPlanResponse response; res.getMessage(response); display_trajectory.trajectory_start = response.trajectory_start; display_trajectory.trajectory.push_back(response.trajectory); display_publisher.publish(display_trajectory); sleep_time.sleep(); /* NOW TRY A JOINT SPACE GOAL */ /* First, set the state in the planning scene to the final state of the last plan */ robot_state::RobotState& robot_state = planning_scene->getCurrentStateNonConst(); planning_scene->setCurrentState(response.trajectory_start); robot_state::JointStateGroup* joint_state_group = robot_state.getJointStateGroup("manipulator"); joint_state_group->setVariableValues(response.trajectory.joint_trajectory.points.back().positions); /* Now, setup a joint space goal*/ robot_state::RobotState goal_state(robot_model); robot_state::JointStateGroup* goal_group = goal_state.getJointStateGroup("manipulator"); std::vector<double> joint_values(7, 0.0); // joint_values[0] = 2.0; joint_values[2] = 1.6; // joint_values[5] = -0.15; goal_group->setVariableValues(joint_values); moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(goal_group); req.goal_constraints.clear(); req.goal_constraints.push_back(joint_goal); /* Construct the planning context */ context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_); /* Call the Planner */ context->solve(res); /* Check that the planning was successful */ if(res.error_code_.val != res.error_code_.SUCCESS) { ROS_ERROR("Could not compute plan successfully"); return 0; } /* Visualize the trajectory */ ROS_INFO("Visualizing the trajectory"); res.getMessage(response); display_trajectory.trajectory_start = response.trajectory_start; display_trajectory.trajectory.push_back(response.trajectory); //Now you should see two planned trajectories in series display_publisher.publish(display_trajectory); /* Now, let's try to go back to the first goal*/ /* First, set the state in the planning scene to the final state of the last plan */ joint_state_group->setVariableValues(response.trajectory.joint_trajectory.points.back().positions); /* Now, we go back to the first goal*/ req.goal_constraints.clear(); req.goal_constraints.push_back(pose_goal); context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_); context->solve(res); res.getMessage(response); display_trajectory.trajectory.push_back(response.trajectory); display_publisher.publish(display_trajectory); /* Let's create a new pose goal */ pose.pose.position.x = 0.65; pose.pose.position.y = -0.2; pose.pose.position.z = -0.1; moveit_msgs::Constraints pose_goal_2 = kinematic_constraints::constructGoalConstraints("wrist_3_link", pose, tolerance_pose, tolerance_angle); /* First, set the state in the planning scene to the final state of the last plan */ joint_state_group->setVariableValues(response.trajectory.joint_trajectory.points.back().positions); /* Now, let's try to move to this new pose goal*/ req.goal_constraints.clear(); req.goal_constraints.push_back(pose_goal_2); /* But, let's impose a path constraint on the motion. Here, we are asking for the end-effector to stay level*/ geometry_msgs::QuaternionStamped quaternion; quaternion.header.frame_id = "base_link"; quaternion.quaternion.w = 1.0; req.path_constraints = kinematic_constraints::constructGoalConstraints("wrist_3_link", quaternion); // imposing path constraints requires the planner to reason in the space of possible positions of the end-effector // (the workspace of the robot) // because of this, we need to specify a bound for the allowed planning volume as well; // Note: a default bound is automatically filled by the WorkspaceBounds request adapter (part of the OMPL pipeline, // but that is not being used in this example). // We use a bound that definitely includes the reachable space for the arm. This is fine because sampling is not done in this volume // when planning for the arm; the bounds are only used to determine if the sampled configurations are valid. req.workspace_parameters.min_corner.x = req.workspace_parameters.min_corner.y = -2.0; req.workspace_parameters.min_corner.z = 0.2; req.workspace_parameters.max_corner.x = req.workspace_parameters.max_corner.y = req.workspace_parameters.max_corner.z = 2.0; context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_); context->solve(res); res.getMessage(response); display_trajectory.trajectory.push_back(response.trajectory); //Now you should see four planned trajectories in series display_publisher.publish(display_trajectory); sleep_time.sleep(); ROS_INFO("Done"); planner_instance.reset(); return 0; }
int main(int argc, char **argv) { int motion = 0; std::string initialpath = ""; if (argc >= 2) { motion = atoi(argv[1]); if(argc >=3) { initialpath = argv[2]; } } printf("%d Motion %d\n", argc, motion); ros::init(argc, argv, "move_itomp"); ros::AsyncSpinner spinner(1); spinner.start(); ros::NodeHandle node_handle("~"); robot_model_loader::RobotModelLoader robot_model_loader( "robot_description"); robot_model::RobotModelPtr robot_model = robot_model_loader.getModel(); planning_scene::PlanningScenePtr planning_scene( new planning_scene::PlanningScene(robot_model)); boost::scoped_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager> > planner_plugin_loader; planning_interface::PlannerManagerPtr planner_instance; std::string planner_plugin_name; if (!node_handle.getParam("planning_plugin", planner_plugin_name)) ROS_FATAL_STREAM("Could not find planner plugin name"); try { planner_plugin_loader.reset( new pluginlib::ClassLoader<planning_interface::PlannerManager>( "moveit_core", "planning_interface::PlannerManager")); } catch (pluginlib::PluginlibException& ex) { ROS_FATAL_STREAM( "Exception while creating planning plugin loader " << ex.what()); } try { planner_instance.reset( planner_plugin_loader->createUnmanagedInstance( planner_plugin_name)); if (!planner_instance->initialize(robot_model, node_handle.getNamespace())) ROS_FATAL_STREAM("Could not initialize planner instance"); ROS_INFO_STREAM( "Using planning interface '" << planner_instance->getDescription() << "'"); } catch (pluginlib::PluginlibException& ex) { const std::vector<std::string> &classes = planner_plugin_loader->getDeclaredClasses(); std::stringstream ss; for (std::size_t i = 0; i < classes.size(); ++i) ss << classes[i] << " "; ROS_ERROR_STREAM( "Exception while loading planner '" << planner_plugin_name << "': " << ex.what() << std::endl << "Available plugins: " << ss.str()); } loadStaticScene(node_handle, planning_scene, robot_model); /* Sleep a little to allow time to startup rviz, etc. */ ros::WallDuration sleep_time(1.0); sleep_time.sleep(); renderStaticScene(node_handle, planning_scene, robot_model); // We will now create a motion plan request // specifying the desired pose of the end-effector as input. planning_interface::MotionPlanRequest req; planning_interface::MotionPlanResponse res; std::vector<robot_state::RobotState> robot_states; int state_index = 0; robot_states.push_back(planning_scene->getCurrentStateNonConst()); robot_states.push_back(robot_states.back()); Eigen::VectorXd start_trans, goal_trans; // set trajectory constraints std::vector<Eigen::VectorXd> waypoints; std::vector<std::string> hierarchy; // internal waypoints between start and goal if(initialpath.empty()) { hierarchy.push_back("base_prismatic_joint_x"); hierarchy.push_back("base_prismatic_joint_y"); hierarchy.push_back("base_prismatic_joint_z"); hierarchy.push_back("base_revolute_joint_z"); hierarchy.push_back("base_revolute_joint_y"); hierarchy.push_back("base_revolute_joint_x"); Eigen::VectorXd vec1; start_trans = Eigen::VectorXd(6); start_trans << 0.0, 1.0, 0.0,0,0,0; goal_trans = Eigen::VectorXd(6); goal_trans << 0.0, 2.5, 0.0,0,0,0; vec1 = Eigen::VectorXd(6); vec1 << 0.0, 1.5, 1.0,0,0,0; waypoints.push_back(vec1); vec1 = Eigen::VectorXd(6); vec1 << 0.0, 2.0, 2.0,0,0,0; waypoints.push_back(vec1); vec1 = Eigen::VectorXd(6); vec1 << 0.0, 2.5, 1.0,0,0,0; waypoints.push_back(vec1); } else { hierarchy = InitTrajectoryFromFile(waypoints, initialpath); start_trans = waypoints.front(); goal_trans = waypoints.back(); } setWalkingStates(robot_states[state_index], robot_states[state_index + 1], start_trans, goal_trans, hierarchy); for (int i = 0; i < waypoints.size(); ++i) { moveit_msgs::Constraints configuration_constraint = setRootJointConstraint(hierarchy, waypoints[i]); req.trajectory_constraints.constraints.push_back( configuration_constraint); } displayStates(robot_states[state_index], robot_states[state_index + 1], node_handle, robot_model); doPlan("whole_body", req, res, robot_states[state_index], robot_states[state_index + 1], planning_scene, planner_instance); visualizeResult(res, node_handle, 0, 1.0); ROS_INFO("Done"); planner_instance.reset(); return 0; }
int main(int argc, char **argv) { ros::init (argc, argv, "workspace_analysis"); ros::AsyncSpinner spinner(1); spinner.start(); // wait some time for everything to be loaded correctly... ROS_INFO_STREAM("Waiting a few seconds to load the robot description correctly..."); sleep(3); ROS_INFO_STREAM("Hope this was enough!"); /*Get some ROS params */ ros::NodeHandle node_handle("~"); double res_x, res_y, res_z; double min_x, min_y, min_z; double max_x, max_y, max_z; double roll, pitch, yaw; int max_attempts; double joint_limits_penalty_multiplier; std::string group_name; if (!node_handle.getParam("min_x", min_x)) min_x = 0.0; if (!node_handle.getParam("max_x", max_x)) max_x = 0.0; if (!node_handle.getParam("res_x", res_x)) res_x = 0.1; if (!node_handle.getParam("min_y", min_y)) min_y = 0.0; if (!node_handle.getParam("max_y", max_y)) max_y = 0.0; if (!node_handle.getParam("res_y", res_y)) res_y = 0.1; if (!node_handle.getParam("min_z", min_z)) min_z = 0.0; if (!node_handle.getParam("max_z", max_z)) max_z = 0.0; if (!node_handle.getParam("res_z", res_z)) res_z = 0.1; if (!node_handle.getParam("max_attempts", max_attempts)) max_attempts = 10000; if (!node_handle.getParam("roll", roll)) roll = 0.0; if (!node_handle.getParam("pitch", pitch)) pitch = 0.0; if (!node_handle.getParam("yaw", yaw)) yaw = 0.0; if (!node_handle.getParam("joint_limits_penalty_multiplier", joint_limits_penalty_multiplier)) joint_limits_penalty_multiplier = 0.0; std::string filename; if (!node_handle.getParam("filename", filename)) ROS_ERROR("Will not write to file"); std::string quat_filename; if (!node_handle.getParam("quat_filename", quat_filename)) ROS_ERROR("Will not write to file"); if (!node_handle.getParam("group_name", group_name)) ROS_FATAL("Must have valid group name"); /* Load the robot model */ robot_model_loader::RobotModelLoader robot_model_loader("robot_description"); /* Get a shared pointer to the model */ robot_model::RobotModelPtr robot_model = robot_model_loader.getModel(); /* Create a robot state*/ robot_state::RobotStatePtr robot_state(new robot_state::RobotState(robot_model)); if(!robot_model->hasJointModelGroup(group_name)) ROS_FATAL("Invalid group name: %s", group_name.c_str()); const robot_model::JointModelGroup* joint_model_group = robot_model->getJointModelGroup(group_name); /* Construct a planning scene - NOTE: this is for illustration purposes only. The recommended way to construct a planning scene is to use the planning_scene_monitor to construct it for you.*/ planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model)); moveit_msgs::WorkspaceParameters workspace; workspace.min_corner.x = min_x; workspace.min_corner.y = min_y; workspace.min_corner.z = min_z; workspace.max_corner.x = max_x; workspace.max_corner.y = max_y; workspace.max_corner.z = max_z; /* Load the workspace analysis */ moveit_workspace_analysis::WorkspaceAnalysis workspace_analysis(planning_scene, true, joint_limits_penalty_multiplier); ros::Time init_time; moveit_workspace_analysis::WorkspaceMetrics metrics; /* Compute the metrics */ bool use_vigir_rpy = true; if (use_vigir_rpy) { std::vector<geometry_msgs::Quaternion> orientations; geometry_msgs::Quaternion quaternion; //yaw = -M_PI*0.0; //roll = M_PI*0.49; // translate roll, pitch and yaw into a Quaternion tf::Quaternion q; q.setRPY(roll, pitch, yaw); geometry_msgs::Quaternion odom_quat; tf::quaternionTFToMsg(q, quaternion); orientations.push_back(quaternion); metrics = workspace_analysis.computeMetrics(workspace, orientations, robot_state.get(), joint_model_group, res_x, res_y, res_z); } else { // load the set of quaternions std::vector<geometry_msgs::Quaternion> orientations; std::ifstream quat_file; quat_file.open(quat_filename.c_str()); while(!quat_file.eof()) { geometry_msgs::Quaternion temp_quat; orientations.push_back(temp_quat); } init_time = ros::Time::now(); metrics = workspace_analysis.computeMetrics(workspace, orientations, robot_state.get(), joint_model_group, res_x, res_y, res_z); if(metrics.points_.empty()) ROS_WARN_STREAM("No point to be written to file: consider changing the workspace, or recompiling moveit_workspace_analysis with a longer sleeping time at the beginning (if this could be the cause)"); } //ros::WallDuration duration(100.0); //moveit_workspace_analysis::WorkspaceMetrics metrics = workspace_analysis.computeMetricsFK(&(*robot_state), joint_model_group, max_attempts, duration); if(!filename.empty()) if(!metrics.writeToFile(filename,",",false)) ROS_INFO("Could not write to file"); ros::Duration total_duration = ros::Time::now() - init_time; ROS_INFO_STREAM("Total duration: " << total_duration.toSec() << "s"); ros::shutdown(); return 0; }