//service function for execute_cartesian_ik_trajectory bool execute_cartesian_ik_trajectory( ur5_control::ExecuteCartesianIKTrajectory::Request &req, ur5_control::ExecuteCartesianIKTrajectory::Response &res) { int trajectory_length = req.poses.size(); int i, j; /* 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(); /* Create a kinematic state - this represents the configuration for the robot represented by kinematic_model */ robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model)); /* Get the configuration for the joints in the right arm of the PR2*/ robot_state::JointStateGroup* joint_state_group = kinematic_state->getJointStateGroup("manipulator"); /* Get the names of the joints in the right_arm*/ const std::vector<std::string> &joint_names = joint_state_group->getJointModelGroup()->getJointModelNames(); //IK takes in Cartesian poses stamped with the frame they belong to geometry_msgs::PoseStamped stamped_pose; stamped_pose.header = req.header; stamped_pose.header.stamp = ros::Time::now(); bool success; std::vector<double *> joint_trajectory; //get the current joint angles (to find ik solutions close to) double last_angles[6]; get_current_joint_angles(last_angles); //find IK solutions for each point along the trajectory //and stick them into joint_trajectory for(i=0; i<trajectory_length; i++){ stamped_pose.pose = req.poses[i]; double *trajectory_point = new double[6]; success = run_ik(stamped_pose.pose, last_angles, trajectory_point, "ee_link", joint_state_group, joint_names); joint_trajectory.push_back(trajectory_point); if(!success){ ROS_ERROR("IK solution not found for trajectory point number %d!\n", i); return 0; } for(j=0; j<6; j++) last_angles[j] = trajectory_point[j]; } //run the resulting joint trajectory ROS_INFO("executing joint trajectory"); success = execute_joint_trajectory(joint_trajectory); res.success = success; return success; }
RobotMoveit::RobotMoveit() { // Kinematics ROS_INFO("RobotMoveit - kinematic_state init"); robot_model_loader::RobotModelLoader robot_model_loader("robot_description"); kinematic_model = robot_model::RobotModelPtr(robot_model_loader.getModel()); kinematic_state = robot_state::RobotStatePtr(new robot_state::RobotState(kinematic_model)); kinematic_state->setToDefaultValues(); // Indigo //joint_names_right_arm = kinematic_state->getJointModelGroup("right_arm")->getActiveJointModelNames(); //joint_names_left_arm = kinematic_state->getJointModelGroup("left_arm")->getActiveJointModelNames(); // Groovy joint_state_group_right_arm = kinematic_state->getJointStateGroup("right_arm"); joint_state_group_left_arm = kinematic_state->getJointStateGroup("left_arm"); joint_names_right_arm = joint_state_group_right_arm->getJointModelGroup()->getJointModelNames(); joint_names_left_arm = joint_state_group_left_arm->getJointModelGroup()->getJointModelNames(); // Move Group Interface ROS_INFO("RobotMoveit - planning_interface init"); group_left_arm = new moveit::planning_interface::MoveGroup("left_arm"); // TODO Gets stuck here without move_group.launch group_right_arm = new moveit::planning_interface::MoveGroup("right_arm"); // Set planners group_left_arm->setPlannerId("RRTConnectkConfigDefault"); group_left_arm->setPlanningTime(15); group_right_arm->setPlannerId("RRTConnectkConfigDefault"); group_right_arm->setPlanningTime(15); // group_left_arm->setGoalOrientationTolerance(0.1); // group_right_arm->setGoalOrientationTolerance(0.1); group_right_arm->setGoalJointTolerance(0.1); group_right_arm->setGoalPositionTolerance(0.1); group_right_arm->setGoalTolerance(0.1); joint_pos_right_arm.resize(joint_names_right_arm.size()); joint_pos_left_arm.resize(joint_names_left_arm.size()); leftMotionInProgress = false; rightMotionInProgress = false; // Check if /joint_states is remapped to /robot/joint_states ROS_INFO("RobotMoveit - checking if /joint_states is publishing"); sensor_msgs::JointStateConstPtr msg = ros::topic::waitForMessage<sensor_msgs::JointState>("/joint_states",ros::Duration(1,0)); if (msg == NULL) { ROS_ERROR("Remap /joint_states to /robot/joint_states for getCurrentJointValues() to work!"); ROS_ERROR("Do this inside a launch file or pass '/joint_states:=/robot/joint_states' as a function argument."); } ROS_INFO("*** Initialized RobotMoveit class ***"); }
/** * intialize() handles retrieving the planner name from the parameter server and * initializing the PlannerManager. * The code is largely copied from the moveit tutorials. * @returns success */ bool PlannerStoppable::initialize() { robot_model_loader::RobotModelLoaderPtr robot_model_loader( new robot_model_loader::RobotModelLoader("/robot_description")); robot_model::RobotModelPtr robot_model = robot_model_loader->getModel(); ps_.reset(new planning_scene::PlanningScene(robot_model)); monitor_.reset(new planning_scene_monitor::PlanningSceneMonitor(ps_, robot_model_loader)); // TODO: allow overriding of default service and topic subscriptions for // monitor_. // Get initial planning scene state from /get_planning_scene service. monitor_->requestPlanningSceneState("/get_planning_scene"); monitor_->startSceneMonitor("/planning_scene"); // 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; 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("default_planner_config", 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()); return false; // Although I assume we never reach this... } 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) { // Just debugging/logging info. 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()); return false; } return true; }
int main(int argc, char **argv) { ros::init(argc, argv, "ik"); ros::NodeHandle n; robot_model_loader::RobotModelLoader robot_model_loader("robot_description"); robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel(); ROS_INFO("Model frame: %s", kinematic_model->getModelFrame().c_str()); robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model)); kinematic_state->setToDefaultValues(); geometry_msgs::Point pos; pos.x = 0.5; pos.y = 0; pos.z = 0.5; geometry_msgs::Quaternion qua; qua.x = 0; qua.y = 0; qua.z = 0; qua.w = 1; geometry_msgs::Pose end_effector_state; end_effector_state.position = pos; end_effector_state.orientation = qua; std::vector<double> joint_values; std::vector<std::string> joint_names; joint_names.push_back("joint_1"); joint_names.push_back("joint_2"); joint_names.push_back("joint_3"); joint_names.push_back("joint_4"); joint_names.push_back("joint_5"); joint_names.push_back("joint_6"); const moveit::core::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("manipulator"); bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, 10, 0.1); if(found_ik) { kinematic_state->copyJointGroupPositions(joint_model_group, joint_values); ROS_INFO("asd\n"); for(std::size_t i=0; i < joint_names.size(); ++i) { ROS_INFO("Joint %s: %f", joint_names[i].c_str(), joint_values[i]); } } else { ROS_INFO("Did not find IK solution"); } return 0; }
int main(int argc, char **argv) { ros::init (argc, argv, "state_display_tutorial"); ros::NodeHandle nh; /* Needed for ROS_INFO commands to work */ ros::AsyncSpinner spinner(1); spinner.start(); /* 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(); ROS_INFO("Model frame: %s", kinematic_model->getModelFrame().c_str()); /* Create a kinematic state - this represents the configuration for the robot represented by kinematic_model */ robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model)); /* Get the configuration for the joints in the right arm of the PR2*/ const robot_model::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("base"); const std::vector<std::string> &joint_names = joint_model_group->getJointModelNames(); std::vector<double> joint_values; kinematic_state->copyJointGroupPositions(joint_model_group, joint_values); for(std::size_t i = 0; i < joint_names.size(); ++i) { ROS_INFO("Joint %s: %f", joint_names[i].c_str(), joint_values[i]); } ros::Publisher robot_state_publisher = nh.advertise<pr2_moveit_ltl::baseState>( "base_robot_state", 1 ); ros::Rate loop_rate(1); while(ros::ok()){ kinematic_state->copyJointGroupPositions(joint_model_group, joint_values); pr2_moveit_ltl::baseState msg; msg.joints = joint_values; msg.names = joint_names; robot_state_publisher.publish( msg ); joint_values[0] = joint_values[0] + 0.2; kinematic_state->setJointGroupPositions("base", joint_values); ros::spinOnce(); loop_rate.sleep(); } /* loop at 1 Hz */ return 0; }
int main(int argc, char **argv) { ros::init (argc, argv, "state_display_tutorial"); /* Needed for ROS_INFO commands to work */ ros::AsyncSpinner spinner(1); spinner.start(); /* 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(); /* Create a kinematic state - this represents the configuration for the robot represented by kinematic_model */ robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model)); const robot_model::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("base"); ros::NodeHandle nh; ros::Publisher robot_state_publisher = nh.advertise<moveit_msgs::DisplayRobotState>( "tutorial_robot_state", 1 ); /* loop at 1 Hz */ ros::Rate loop_rate(1); for (int cnt=0; cnt<5 && ros::ok(); cnt++) { kinematic_state->setToRandomPositions(joint_model_group); /* get a robot state message describing the pose in kinematic_state */ moveit_msgs::DisplayRobotState msg; robot_state::robotStateToRobotStateMsg(*kinematic_state, msg.state); /* send the message to the RobotState display */ std::cout << "Num:" << cnt << std::endl; robot_state_publisher.publish( msg ); /* let ROS send the message, then wait a while */ ros::spinOnce(); loop_rate.sleep(); } ros::shutdown(); return 0; }
ARM_manager::ARM_manager() { // Load the robot model robot_model_loader::RobotModelLoader robot_model_loader("robot_description"); my_robot_model_loader = robot_model_loader; // Get a shared pointer to the model my_kinematic_model = my_robot_model_loader.getModel(); // Get and print the name of the coordinate frame in which the transforms for this model are computed ROS_INFO("Model frame: %s", my_kinematic_model->getModelFrame().c_str()); // WORKING WITH THE KINEMATIC STATE // Create a kinematic state - this represents the configuration for the robot represented by kinematic_model robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(my_kinematic_model)); my_kinematic_state = kinematic_state; // Set all joints in this state to their default values my_kinematic_state->setToDefaultValues(); RA_joint_model_group = my_kinematic_model->getJointModelGroup("right_arm"); RA_group_ = my_kinematic_state->getJointStateGroup("right_arm"); // Get the names of the joints in the right_arm RA_joint_names = RA_joint_model_group->getJointModelNames(); LA_joint_model_group = my_kinematic_model->getJointModelGroup("left_arm"); LA_group_ = my_kinematic_state->getJointStateGroup("left_arm"); // Get the names of the joints in the left_arm LA_joint_names = LA_joint_model_group->getJointModelNames(); action_sub_ = nh.subscribe < std_msgs::Int32 > ("/ROBOT/action", 5, &ARM_manager::actionCallback, this); firepose_sub_ = nh.subscribe < geometry_msgs::PoseStamped > ("/ROBOT/firepose", 5, &ARM_manager::fireposeCallback, this); // inclure la couleur dans le twist fruitcolor_sub_ = nh.subscribe < std_msgs::Int32 > ("/ROBOT/fruitcolor", 5, &ARM_manager::fruitcolorCallback, this); start_game_sub_ = nh.subscribe < std_msgs::Empty > ("/GENERAL/start_game", 5, &ARM_manager::startgameCallback, this); Rdebug_sub_ = nh.subscribe < geometry_msgs::PoseStamped > ("/DEBUG/right_arm_pose", 5, &ARM_manager::RdebugCallback, this); // inclure la couleur dans le twist Ldebug_sub_ = nh.subscribe < geometry_msgs::PoseStamped > ("/DEBUG/left_arm_pose", 5, &ARM_manager::LdebugCallback, this); // inclure la couleur dans le twist alpha_pub = nh.advertise < std_msgs::Int32 > ("/ROBOT/alpha_ros", 5); delta_pub = nh.advertise < std_msgs::Int32 > ("/ROBOT/delta_ros", 5); grip_pub = nh.advertise < std_msgs::Int8 > ("/ROBOT/grip", 5); rpump_pub = nh.advertise < std_msgs::Int8 > ("pump_ros", 5); lpump_pub = nh.advertise < std_msgs::Int8 > ("/ROBOT/lpump", 5); done_pub = nh.advertise < std_msgs::Empty > ("/ROBOT/done", 5); Rjoint1_pub = nh.advertise < std_msgs::Float64 > ("/Rlink1_controller/command", 5); Rjoint2_pub = nh.advertise < std_msgs::Float64 > ("/Rlink2_controller/command", 5); Rjoint3_pub = nh.advertise < std_msgs::Float64 > ("/Rlink3_controller/command", 5); Rjoint4_pub = nh.advertise < std_msgs::Float64 > ("/Rlink4_controller/command", 5); Rjoint5_pub = nh.advertise < std_msgs::Float64 > ("/Rlink5_controller/command", 5); Ljoint1_pub = nh.advertise < std_msgs::Float64 > ("/Llink1_controller/command", 5); Ljoint2_pub = nh.advertise < std_msgs::Float64 > ("/Llink2_controller/command", 5); Ljoint3_pub = nh.advertise < std_msgs::Float64 > ("/Llink3_controller/command", 5); Ljoint4_pub = nh.advertise < std_msgs::Float64 > ("/Llink4_controller/command", 5); Ljoint5_pub = nh.advertise < std_msgs::Float64 > ("/Llink5_controller/command", 5); speed_Rjoint1 = nh.serviceClient<dynamixel_controllers::SetSpeed>("/Rlink1_controller/set_speed", true); speed_Rjoint2 = nh.serviceClient<dynamixel_controllers::SetSpeed>("/Rlink2_controller/set_speed", true); speed_Rjoint3 = nh.serviceClient<dynamixel_controllers::SetSpeed>("/Rlink3_controller/set_speed", true); speed_Rjoint4 = nh.serviceClient<dynamixel_controllers::SetSpeed>("/Rlink4_controller/set_speed", true); speed_Rjoint5 = nh.serviceClient<dynamixel_controllers::SetSpeed>("/Rlink5_controller/set_speed", true); slope_Rjoint1 = nh.serviceClient<dynamixel_controllers::SetComplianceSlope>("/Rlink1_controller/set_compliance_slope", true); slope_Rjoint2 = nh.serviceClient<dynamixel_controllers::SetComplianceSlope>("/Rlink2_controller/set_compliance_slope", true); slope_Rjoint3 = nh.serviceClient<dynamixel_controllers::SetComplianceSlope>("/Rlink3_controller/set_compliance_slope", true); slope_Rjoint4 = nh.serviceClient<dynamixel_controllers::SetComplianceSlope>("/Rlink4_controller/set_compliance_slope", true); slope_Rjoint5 = nh.serviceClient<dynamixel_controllers::SetComplianceSlope>("/Rlink5_controller/set_compliance_slope", true); speed_Ljoint1 = nh.serviceClient<dynamixel_controllers::SetSpeed>("/Llink1_controller/set_speed", true); speed_Ljoint2 = nh.serviceClient<dynamixel_controllers::SetSpeed>("/Llink2_controller/set_speed", true); speed_Ljoint3 = nh.serviceClient<dynamixel_controllers::SetSpeed>("/Llink3_controller/set_speed", true); speed_Ljoint4 = nh.serviceClient<dynamixel_controllers::SetSpeed>("/Llink4_controller/set_speed", true); speed_Ljoint5 = nh.serviceClient<dynamixel_controllers::SetSpeed>("/Llink5_controller/set_speed", true); slope_Ljoint1 = nh.serviceClient<dynamixel_controllers::SetComplianceSlope>("/Llink1_controller/set_compliance_slope", true); slope_Ljoint2 = nh.serviceClient<dynamixel_controllers::SetComplianceSlope>("/Llink2_controller/set_compliance_slope", true); slope_Ljoint3 = nh.serviceClient<dynamixel_controllers::SetComplianceSlope>("/Llink3_controller/set_compliance_slope", true); slope_Ljoint4 = nh.serviceClient<dynamixel_controllers::SetComplianceSlope>("/Llink4_controller/set_compliance_slope", true); slope_Ljoint5 = nh.serviceClient<dynamixel_controllers::SetComplianceSlope>("/Llink5_controller/set_compliance_slope", true); prev_Rjoint1 = 0.0; prev_Rjoint2 = 0.0; prev_Rjoint3 = 0.0; prev_Rjoint4 = 0.0; prev_Rjoint5 = 0.0; max_speed = 5.0; usleep(1000000); dynamixel_controllers::SetComplianceSlope tmp_slope; tmp_slope.request.slope = 90; if (slope_Rjoint1.call(tmp_slope)) { //ROS_INFO("Sum: %ld", (long int)srv.response.sum); } else { ROS_ERROR("Failed to call service SetSlope"); } if (slope_Rjoint2.call(tmp_slope)) { //ROS_INFO("Sum: %ld", (long int)srv.response.sum); } else { ROS_ERROR("Failed to call service SetSlope"); } if (slope_Rjoint3.call(tmp_slope)) { //ROS_INFO("Sum: %ld", (long int)srv.response.sum); } else { ROS_ERROR("Failed to call service SetSlope"); } if (slope_Rjoint4.call(tmp_slope)) { //ROS_INFO("Sum: %ld", (long int)srv.response.sum); } else { ROS_ERROR("Failed to call service SetSlope"); } if (slope_Rjoint5.call(tmp_slope)) { //ROS_INFO("Sum: %ld", (long int)srv.response.sum); } else { ROS_ERROR("Failed to call service SetSlope"); } if (slope_Ljoint1.call(tmp_slope)) { //ROS_INFO("Sum: %ld", (long int)srv.response.sum); } else { ROS_ERROR("Failed to call service SetSlope"); } if (slope_Ljoint2.call(tmp_slope)) { //ROS_INFO("Sum: %ld", (long int)srv.response.sum); } else { ROS_ERROR("Failed to call service SetSlope"); } if (slope_Ljoint3.call(tmp_slope)) { //ROS_INFO("Sum: %ld", (long int)srv.response.sum); } else { ROS_ERROR("Failed to call service SetSlope"); } if (slope_Ljoint4.call(tmp_slope)) { //ROS_INFO("Sum: %ld", (long int)srv.response.sum); } else { ROS_ERROR("Failed to call service SetSlope"); } if (slope_Ljoint5.call(tmp_slope)) { //ROS_INFO("Sum: %ld", (long int)srv.response.sum); } else { ROS_ERROR("Failed to call service SetSlope"); } usleep(500000); init_pose(); //standard_pose(); }
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, "state_display_tutorial"); /* Needed for ROS_INFO commands to work */ ros::AsyncSpinner spinner(1); spinner.start(); /* Load the robot model */ robot_model_loader::RDFLoader robot_model_loader("robot_description"); /* Get a shared pointer to the model */ robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel(); /* Create a kinematic state - this represents the configuration for the robot represented by kinematic_model */ robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model)); /* Get the configuration for the joints in the right arm of the PR2*/ robot_state::JointStateGroup* joint_state_group = kinematic_state->getJointStateGroup("right_arm"); /* PUBLISH RANDOM ARM POSITIONS */ ros::NodeHandle nh; ros::Publisher robot_state_publisher = nh.advertise<moveit_msgs::DisplayRobotState>( "tutorial_robot_state", 1 ); /* loop at 1 Hz */ ros::Rate loop_rate(1); for (int cnt=0; cnt<5 && ros::ok(); cnt++) { joint_state_group->setToRandomValues(); /* get a robot state message describing the pose in kinematic_state */ moveit_msgs::DisplayRobotState msg; robot_state::robotStateToRobotStateMsg(*kinematic_state, msg.state); /* send the message to the RobotState display */ robot_state_publisher.publish( msg ); /* let ROS send the message, then wait a while */ ros::spinOnce(); loop_rate.sleep(); } /* POSITION END EFFECTOR AT SPECIFIC LOCATIONS */ /* Find the default pose for the end effector */ kinematic_state->setToDefaultValues(); const Eigen::Affine3d end_effector_default_pose = kinematic_state->getLinkState("r_wrist_roll_link")->getGlobalLinkTransform(); const double PI = boost::math::constants::pi<double>(); const double RADIUS = 0.1; for (double angle=0; angle<=2*PI && ros::ok(); angle+=2*PI/20) { /* calculate a position for the end effector */ Eigen::Affine3d end_effector_pose = Eigen::Translation3d(RADIUS * cos(angle), RADIUS * sin(angle), 0.0) * end_effector_default_pose; ROS_INFO_STREAM("End effector position:\n" << end_effector_pose.translation()); /* use IK to get joint angles satisfyuing the calculated position */ bool found_ik = joint_state_group->setFromIK(end_effector_pose, 10, 0.1); if (!found_ik) { ROS_INFO_STREAM("Could not solve IK for pose\n" << end_effector_pose.translation()); continue; } /* get a robot state message describing the pose in kinematic_state */ moveit_msgs::DisplayRobotState msg; robot_state::robotStateToRobotStateMsg(*kinematic_state, msg.state); /* send the message to the RobotState display */ robot_state_publisher.publish( msg ); /* let ROS send the message, then wait a while */ ros::spinOnce(); loop_rate.sleep(); } ros::shutdown(); return 0; }
int main(int argc, char **argv) { ros::init (argc, argv, "right_arm_kinematics"); ros::AsyncSpinner spinner(1); spinner.start(); ros::NodeHandle node_handle; // Start a service client ros::ServiceClient service_client = node_handle.serviceClient<moveit_msgs::GetPositionIK> ("compute_ik"); ros::Publisher robot_state_publisher = node_handle.advertise<moveit_msgs::DisplayRobotState>( "tutorial_robot_state", 1 ); while(!service_client.exists()) { ROS_INFO("Waiting for service"); sleep(1.0); } moveit_msgs::GetPositionIK::Request service_request; moveit_msgs::GetPositionIK::Response service_response; service_request.ik_request.group_name = "left_arm"; service_request.ik_request.pose_stamped.header.frame_id = "torso_lift_link"; service_request.ik_request.pose_stamped.pose.position.x = 0.75; service_request.ik_request.pose_stamped.pose.position.y = 0.188; service_request.ik_request.pose_stamped.pose.position.z = 0.0; service_request.ik_request.pose_stamped.pose.orientation.x = 0.0; service_request.ik_request.pose_stamped.pose.orientation.y = 0.0; service_request.ik_request.pose_stamped.pose.orientation.z = 0.0; service_request.ik_request.pose_stamped.pose.orientation.w = 1.0; /* Call the service */ service_client.call(service_request, service_response); ROS_INFO_STREAM("Result: " << ((service_response.error_code.val == service_response.error_code.SUCCESS) ? "True " : "False ") << service_response.error_code.val); /* Filling in a seed state */ robot_model_loader::RobotModelLoader robot_model_loader("robot_description"); robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel(); robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model)); robot_state::JointStateGroup* joint_state_group = kinematic_state->getJointStateGroup("left_arm"); /* Get the names of the joints in the right_arm*/ service_request.ik_request.robot_state.joint_state.name = joint_state_group->getJointModelGroup()->getJointModelNames(); /* Get the joint values and put them into the message, this is where you could put in your own set of values as well.*/ joint_state_group->setToRandomValues(); joint_state_group->getVariableValues(service_request.ik_request.robot_state.joint_state.position); /* Call the service again*/ service_client.call(service_request, service_response); ROS_INFO_STREAM("Result: " << ((service_response.error_code.val == service_response.error_code.SUCCESS) ? "True " : "False ") << service_response.error_code.val); /* Check for collisions*/ service_request.ik_request.avoid_collisions = true; /* Call the service again*/ service_client.call(service_request, service_response); ROS_INFO_STREAM("Result: " << ((service_response.error_code.val == service_response.error_code.SUCCESS) ? "True " : "False ") << service_response.error_code.val); /* Visualize the result*/ moveit_msgs::DisplayRobotState msg; joint_state_group->setVariableValues(service_response.solution.joint_state); robot_state::robotStateToRobotStateMsg(*kinematic_state, msg.state); robot_state_publisher.publish(msg); //Sleep to let the message go through ros::Duration(2.0).sleep(); ros::shutdown(); return 0; }
//Constructor NAO_PLANNER_CONTROL::NAO_PLANNER_CONTROL(): nh_("~") { //Read params from parameter server nh_.param("robot_description", ROBOT_DESCRIPTION, std::string("robot_description")); nh_.param("planning_group", PLANNING_GROUP, std::string("whole_body_rfoot")); nh_.param("scale_sp", SCALE_SP, 0.8); //Stable Configuration Generator Params nh_.param("scg_max_samples", MAX_SAMPLES, 3000); nh_.param("scg_max_ik_iterations", MAX_IK_ITERATIONS, 5); //RRT CONNECT Params nh_.param("planner_step_factor", ADVANCE_STEPS, 0.1); nh_.param("planner_max_iterations", MAX_EXPAND_ITERATIONS, 8000); //Joint weights nh_.param("r_arm_weight_approach", R_ARM_WEIGHT_APPROACH, 1.0); nh_.param("l_arm_weight_approach", L_ARM_WEIGHT_APPROACH, 1.0); nh_.param("legs_weight_approach", LEGS_WEIGHT_APPROACH, 1.0); nh_.param("r_arm_weight_manipulate", R_ARM_WEIGHT_MANIPULATE, 1.0); nh_.param("l_arm_weight_manipulate", L_ARM_WEIGHT_MANIPULATE, 1.0); nh_.param("legs_weight_manipulate", LEGS_WEIGHT_MANIPULATE, 1.0); //Path to files to read from and write into std::string ds_database; std::string goal_config_first; std::string goal_config_second; std::string solution_first; std::string solution_second; nh_.param("file_path_DS_database",ds_database,std::string("../config_database/ssc_double_support.dat")); nh_.param("file_path_first_goal_config",goal_config_first,std::string("../config_database/ssc_double_support_goal_first.dat")); nh_.param("file_path_second_goal_config",goal_config_second,std::string("../config_database/ssc_double_support_goal_second.dat")); nh_.param("file_path_solution_traj_first",solution_first,std::string("../solutions/solution_path_first.dat")); nh_.param("file_path_solution_traj_second",solution_second,std::string("../solutions/solution_path_second.dat")); SSC_DS_CONFIGS = new char[ds_database.size() + 1]; std::copy(ds_database.begin(), ds_database.end(), SSC_DS_CONFIGS); SSC_DS_CONFIGS[ds_database.size()] = '\0'; // don't forget the terminating 0 SSC_DS_GOAL_CONFIG_FIRST = new char[goal_config_first.size() + 1]; std::copy(goal_config_first.begin(), goal_config_first.end(), SSC_DS_GOAL_CONFIG_FIRST); SSC_DS_GOAL_CONFIG_FIRST[goal_config_first.size()] = '\0'; // don't forget the terminating 0 SSC_DS_GOAL_CONFIG_SECOND = new char[goal_config_second.size() + 1]; std::copy(goal_config_second.begin(), goal_config_second.end(), SSC_DS_GOAL_CONFIG_SECOND); SSC_DS_GOAL_CONFIG_SECOND[goal_config_second.size()] = '\0'; // don't forget the terminating 0 SOLUTION_FILE_PATH_FIRST = new char[solution_first.size() + 1]; std::copy(solution_first.begin(), solution_first.end(), SOLUTION_FILE_PATH_FIRST); SOLUTION_FILE_PATH_FIRST[solution_first.size()] = '\0'; // don't forget the terminating 0 SOLUTION_FILE_PATH_SECOND = new char[solution_second.size() + 1]; std::copy(solution_second.begin(), solution_second.end(), SOLUTION_FILE_PATH_SECOND); SOLUTION_FILE_PATH_SECOND[solution_second.size()] = '\0'; // don't forget the terminating 0 // //Set the file containing the statically stable configurations // SSC_DS_CONFIGS = ds_database.c_str(); // //Set the file containing the statically stable GOAL configurations (for the first motion plan) // SSC_DS_GOAL_CONFIG_FIRST = goal_config_first.c_str(); // //Set the file containing the statically stable GOAL configurations (for the second motion plan) // SSC_DS_GOAL_CONFIG_SECOND = goal_config_second.c_str(); // //Files storing the solution paths // SOLUTION_FILE_PATH_FIRST = solution_first.c_str(); // SOLUTION_FILE_PATH_SECOND = solution_second.c_str(); //Create planning scene monitor psm_ = boost::shared_ptr<planning_scene_monitor::PlanningSceneMonitor>(new planning_scene_monitor::PlanningSceneMonitor(ROBOT_DESCRIPTION)); //Create planning scene robot_model_loader::RobotModelLoader robot_model_loader(ROBOT_DESCRIPTION); robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel(); p_s_ = boost::shared_ptr<planning_scene::PlanningScene>(new planning_scene::PlanningScene(kinematic_model)); //Create Stable Config Generator //scg_ = boost::shared_ptr<nao_constraint_sampler::StableConfigGenerator>(new nao_constraint_sampler::StableConfigGenerator(psm_, PLANNING_GROUP, SCALE_SP)); scg_ = boost::shared_ptr<nao_constraint_sampler::StableConfigGenerator>(new nao_constraint_sampler::StableConfigGenerator(p_s_, PLANNING_GROUP, SCALE_SP)); //Create RRT Planner //planner_ = boost::shared_ptr<nao_planner::RRT_Planner>(new nao_planner::RRT_Planner(psm_,PLANNING_GROUP)); planner_ = boost::shared_ptr<nao_planner::RRT_Planner>(new nao_planner::RRT_Planner(p_s_,PLANNING_GROUP)); //Scale support polygon for first planner planner_->scale_support_polygon(SCALE_SP); //Load a set of statically stable configurations planner_->load_DS_database(SSC_DS_CONFIGS); //Fix Joint Names order //planning_scene_monitor::LockedPlanningSceneRO p_s(psm_); //const std::vector<std::string> joint_names = p_s->getRobotModel()->getJointModelGroup(PLANNING_GROUP)->getJointModelNames(); //const std::vector<std::string> joint_names = psm_->getPlanningScene()->getRobotModel()->getJointModelGroup(PLANNING_GROUP)->getJointModelNames(); const std::vector<std::string> joint_names = p_s_->getRobotModel()->getJointModelGroup(PLANNING_GROUP)->getJointModelNames(); joint_names_ = joint_name_order_RobModel_to_Planner(joint_names); //Start Monitors of Planning_Scene_Monitor psm_->startWorldGeometryMonitor(); psm_->startSceneMonitor(); psm_->startStateMonitor(); }
int main(int argc, char **argv) { // Iniciar nodo IK ros::init (argc, argv, "kuka_ik"); ros::NodeHandle n; ros::NodeHandle nh("~"); // Parametro rate int rate; nh.param("rate", rate, 5); ros::Rate loop_rate(rate); ROS_INFO("IK Solve: %d Hz", rate); // Parametro origin std::string origin; nh.param<std::string>("origin", origin, "world"); // Cargar robot KUKA robot_model_loader::RobotModelLoader robot_model_loader("robot_description"); robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel(); // Frame por defecto base ROS_INFO("Model frame: %s", kinematic_model->getModelFrame().c_str()); robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model)); kinematic_state->setToDefaultValues(); // Grupo de movimiento del robot const robot_state::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("arm"); // Obtener nombres de joints const std::vector<std::string> &joint_names = joint_model_group->getJointModelNames(); // Posicion actual de joints std::vector<double> joint_values; kinematic_state->copyJointGroupPositions(joint_model_group, joint_values); // Publisher para posiciones KUKA Shadow ros::Publisher kukaShadowPub = n.advertise<sensor_msgs::JointState>("joint_states", 1000); // Suscriber para posiciones objetivo para KUKA Shadow ros::Subscriber kukaShadowSub = n.subscribe("efector_pose", 50, kukaEfectorCallback); // Mensaje para KUKA Shadow sensor_msgs::JointState shadowJointMsg; shadowJointMsg.name.resize(6); shadowJointMsg.position.resize(6); shadowJointMsg.velocity.resize(6); for(std::size_t i=0; i < joint_names.size(); ++i){ shadowJointMsg.name[i] = joint_names[i].c_str(); shadowJointMsg.position[i] = joint_values[i]; } // Mensaje while (ros::ok()) { // IK kinematic_state->setJointGroupPositions(joint_model_group, joint_values); bool found_ik = kinematic_state->setFromIK(joint_model_group, goalPose, 5, 0.01); // Now, we can print out the IK solution (if found): if (found_ik) { // Actualizar joint_values kinematic_state->copyJointGroupPositions(joint_model_group, joint_values); // Publicar mensaje para KUKA Shadow shadowJointMsg.header.stamp = ros::Time::now(); for(std::size_t i=0; i < joint_names.size(); ++i){ shadowJointMsg.name[i] = joint_names[i].c_str(); shadowJointMsg.position[i] = joint_values[i]; } kukaShadowPub.publish(shadowJointMsg); /* for(std::size_t i=0; i < joint_names.size(); ++i){ ROS_INFO("Joint %s: %f", joint_names[i].c_str(), joint_values[i]); } */ } else { ROS_ERROR("Problema con IK"); } ros::spinOnce(); loop_rate.sleep(); } ros::shutdown(); 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) { 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; }
TeleopMK2::TeleopMK2() { joint_pub_ = nh_.advertise<sensor_msgs::JointState>("/command", 5); joy_sub_ = nh_.subscribe<sensor_msgs::Joy>("joy", 10, &TeleopMK2::joyCallback, this); /* Load the robot model */ robot_model_loader::RobotModelLoader robot_model_loader("robot_description"); /* Get a shared pointer to the model */ kinematic_model = robot_model_loader.getModel(); /* Get and print the name of the coordinate frame in which the transforms for this model are computed */ ROS_INFO("Model frame: %s", kinematic_model->getModelFrame().c_str()); /* WORKING WITH THE KINEMATIC STATE */ /* Create a kinematic state - this represents the configuration for the robot represented by kinematic_model */ robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model)); /* Set all joints in this state to their default values */ kinematic_state->setToDefaultValues(); /* Get the configuration for the joints in the right arm of the MK2 */ const robot_state::JointModelGroup* joint_state_group = kinematic_state->getJointModelGroup("manipulator");//"manipulator"); /* Get the names of the joints in the right_arm*/ const std::vector<std::string> &joint_names = joint_state_group->getJointModelNames(); // Get Joint Values // ^^^^^^^^^^^^^^^^ /* Get the joint states for the right arm*/ std::vector<double> joint_values; kinematic_state->copyJointGroupPositions(joint_state_group,joint_values); std::vector<double> vel; double p[] = {0.4,0.4,0.4,0.4,0.6,0.6,0.6}; std::vector<double> a(p, p+7); sensor_msgs::JointState msg; ros::Rate r(50); const Eigen::Affine3d &end_effector_state = kinematic_state->getGlobalLinkTransform("link_7"); Eigen::Affine3d new_end_effector_state; tf::poseEigenToMsg(end_effector_state, actual_end_effector_); while(nh_.ok()) { tf::poseMsgToEigen(actual_end_effector_, new_end_effector_state); bool found_ik = kinematic_state->setFromIK(joint_state_group, new_end_effector_state, 10, 0.1); if (found_ik) { kinematic_state->copyJointGroupPositions(joint_state_group,joint_values); msg.header.stamp = ros::Time::now(); msg.name = joint_names; msg.position = joint_values; msg.velocity = a; joint_pub_.publish(msg); } else { ROS_INFO("Did not find IK solution"); } ros::spinOnce(); r.sleep(); } }
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, "ptu_kinematics"); ros::AsyncSpinner spinner(1); spinner.start(); // BEGIN_TUTORIAL // Start // ^^^^^ // Setting up to start using the RobotModel class is very easy. In // general, you will find that most higher-level components will // return a shared pointer to the RobotModel. You should always use // that when possible. In this example, we will start with such a // shared pointer and discuss only the basic API. You can have a // look at the actual code API for these classes to get more // information about how to use more features provided by these // classes. // // 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 kinematic_model = robot_model_loader.getModel(); ROS_INFO("Model frame: %s", kinematic_model->getModelFrame().c_str()); // Using the :moveit_core:`RobotModel`, we can construct a // :moveit_core:`RobotState` that maintains the configuration // of the robot. We will set all joints in the state to their // default values. We can then get a // :moveit_core:`JointModelGroup`, which represents the robot // model for a particular group, e.g. the "right_arm" of the PR2 // robot. robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model)); kinematic_state->setToDefaultValues(); const robot_state::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("ptu"); const std::vector<std::string> &joint_names = joint_model_group->getJointModelNames(); // Get Joint Values // ^^^^^^^^^^^^^^^^ // We can retreive the current set of joint values stored in the state for the right arm. std::vector<double> joint_values; kinematic_state->copyJointGroupPositions(joint_model_group, joint_values); for(std::size_t i = 0; i < joint_names.size(); ++i) { ROS_INFO("Joint %s: %f", joint_names[i].c_str(), joint_values[i]); } // Joint Limits // ^^^^^^^^^^^^ // setJointGroupPositions() does not enforce joint limits by itself, but a call to enforceBounds() will do it. /* Set one joint in the right arm outside its joint limit */ joint_values[0] = 1.57; kinematic_state->setJointGroupPositions(joint_model_group, joint_values); /* Check whether any joint is outside its joint limits */ ROS_INFO_STREAM("Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid")); /* Enforce the joint limits for this state and check again*/ kinematic_state->enforceBounds(); ROS_INFO_STREAM("Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid")); // Forward Kinematics // ^^^^^^^^^^^^^^^^^^ // Now, we can compute forward kinematics for a set of random joint // values. Note that we would like to find the pose of the // "r_wrist_roll_link" which is the most distal link in the // "right_arm" of the robot. kinematic_state->setToRandomPositions(joint_model_group); double pan = 0.0; double tilt = 0.0; kinematic_state->setJointPositions("pan", &pan); kinematic_state->setJointPositions("tilt", &tilt); const Eigen::Affine3d &end_effector_state = kinematic_state->getGlobalLinkTransform("head_xtion_depth_optical_frame"); /* Print end-effector pose. Remember that this is in the model frame */ ROS_INFO_STREAM("Translation: " << end_effector_state.translation()); ROS_INFO_STREAM("Rotation: " << end_effector_state.rotation()); // Inverse Kinematics // ^^^^^^^^^^^^^^^^^^ // We can now solve inverse kinematics (IK) for the right arm of the // PR2 robot. To solve IK, we will need the following: // * The desired pose of the end-effector (by default, this is the last link in the "right_arm" chain): end_effector_state that we computed in the step above. // * The number of attempts to be made at solving IK: 5 // * The timeout for each attempt: 0.1 s bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, 10, 0.1); // Now, we can print out the IK solution (if found): if (found_ik) { kinematic_state->copyJointGroupPositions(joint_model_group, joint_values); for(std::size_t i=0; i < joint_names.size(); ++i) { ROS_INFO("Joint %s: %f", joint_names[i].c_str(), joint_values[i]); } } else { ROS_INFO("Did not find IK solution"); } // Get the Jacobian // ^^^^^^^^^^^^^^^^ // We can also get the Jacobian from the :moveit_core:`RobotState`. Eigen::Vector3d reference_point_position(0.0,0.0,0.0); Eigen::MatrixXd jacobian; kinematic_state->getJacobian(joint_model_group, kinematic_state->getLinkModel(joint_model_group->getLinkModelNames().back()), reference_point_position, jacobian); ROS_INFO_STREAM("Jacobian: " << jacobian); // END_TUTORIAL ros::shutdown(); return 0; }
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("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, "right_arm_kinematics"); ros::AsyncSpinner spinner(1); spinner.start(); /* Load the robot model */ robot_model_loader::RDFLoader robot_model_loader("robot_description"); /* Get a shared pointer to the model */ robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel(); /* Get and print the name of the coordinate frame in which the transforms for this model are computed*/ ROS_INFO("Model frame: %s", kinematic_model->getModelFrame().c_str()); /* WORKING WITH THE KINEMATIC STATE */ /* Create a kinematic state - this represents the configuration for the robot represented by kinematic_model */ robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model)); /* Set all joints in this state to their default values */ kinematic_state->setToDefaultValues(); /* Get the configuration for the joints in the right arm of the PR2*/ robot_state::JointStateGroup* joint_state_group = kinematic_state->getJointStateGroup("right_arm"); /* Get the names of the joints in the right_arm*/ const std::vector<std::string> &joint_names = joint_state_group->getJointModelGroup()->getJointModelNames(); /* Get the joint states for the right arm*/ std::vector<double> joint_values; joint_state_group->getVariableValues(joint_values); /* Print joint names and values */ for(std::size_t i = 0; i < joint_names.size(); ++i) { ROS_INFO("Joint %s: %f", joint_names[i].c_str(), joint_values[i]); } /* Set one joint in the right arm outside its joint limit */ joint_values[0] = 1.57; joint_state_group->setVariableValues(joint_values); /* Check whether any joint is outside its joint limits */ ROS_INFO_STREAM("Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid")); /* Enforce the joint limits for this state and check again*/ kinematic_state->enforceBounds(); ROS_INFO_STREAM("Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid")); /* FORWARD KINEMATICS */ /* Compute FK for a set of random joint values*/ joint_state_group->setToRandomValues(); const Eigen::Affine3d &end_effector_state = joint_state_group->getRobotState()->getLinkState("r_wrist_roll_link")->getGlobalLinkTransform(); /* Print end-effector pose. Remember that this is in the model frame */ ROS_INFO_STREAM("Translation: " << end_effector_state.translation()); ROS_INFO_STREAM("Rotation: " << end_effector_state.rotation()); /* INVERSE KINEMATICS */ /* Set joint state group to a set of random values*/ joint_state_group->setToRandomValues(); /* Do IK on the pose we just generated using forward kinematics * Here 10 is the number of random restart and 0.1 is the allowed time after * each restart */ bool found_ik = joint_state_group->setFromIK(end_effector_state, 10, 0.1); /* Get and print the joint values */ if (found_ik) { joint_state_group->getVariableValues(joint_values); for(std::size_t i=0; i < joint_names.size(); ++i) { ROS_INFO("Joint %s: %f", joint_names[i].c_str(), joint_values[i]); } } else { ROS_INFO("Did not find IK solution"); } /* DIFFERENTIAL KINEMATICS */ /* Get and print the Jacobian for the right arm*/ Eigen::Vector3d reference_point_position(0.0,0.0,0.0); Eigen::MatrixXd jacobian; joint_state_group->getJacobian(joint_state_group->getJointModelGroup()->getLinkModelNames().back(), reference_point_position, jacobian); ROS_INFO_STREAM("Jacobian: " << jacobian); ros::shutdown(); return 0; }
int main(int argc, char **argv) { // Iniciar nodo IK ros::init (argc, argv, "kuka_ik"); ros::NodeHandle n; ros::NodeHandle nh("~"); // Parametro rate int rate; nh.param("rate", rate, 5); ros::Rate loop_rate(rate); ROS_INFO("IK Solve: %d Hz", rate); // Parametro origin std::string origin; nh.param<std::string>("origin", origin, "world"); // Cargar robot KUKA robot_model_loader::RobotModelLoader robot_model_loader("robot_description"); robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel(); // Frame por defecto base ROS_INFO("Model frame: %s", kinematic_model->getModelFrame().c_str()); robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model)); kinematic_state->setToDefaultValues(); // Grupo de movimiento del robot const robot_state::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("arm"); // Obtener nombres de joints const std::vector<std::string> &joint_names = joint_model_group->getJointModelNames(); // Posicion actual de joints std::vector<double> joint_values; kinematic_state->copyJointGroupPositions(joint_model_group, joint_values); // Publisher for KUKA model joint position from IK solver ros::Publisher kukaJointPub = n.advertise<sensor_msgs::JointState>("joint_states", 1000); // Suscriber for goal position (InteractiveMarker) ros::Subscriber kukaIkGoalSub = n.subscribe("goal_pose", 50, kukaIkCallback); // JointState Msg for KUKA model from IK solver sensor_msgs::JointState jointMsg; jointMsg.name.resize(6); jointMsg.position.resize(6); jointMsg.velocity.resize(6); for(std::size_t i=0; i < joint_names.size(); ++i){ jointMsg.name[i] = joint_names[i].c_str(); jointMsg.position[i] = joint_values[i]; } // Publish init joint state ros::Duration(1).sleep(); jointMsg.header.stamp = ros::Time::now(); kukaJointPub.publish(jointMsg); // Mensaje while (ros::ok()) { if (updatePose){ // IK kinematic_state->setJointGroupPositions(joint_model_group, joint_values); bool found_ik = kinematic_state->setFromIK(joint_model_group, goalPose, 5, 0.01); // IK solution (if found): if (found_ik) { // Actualizar joint_values kinematic_state->copyJointGroupPositions(joint_model_group, joint_values); // for(std::size_t i=0; i < joint_names.size(); ++i){ // jointMsg.position[i] = joint_values[i]; // //ROS_INFO("Joint %s: %f", joint_names[i].c_str(), joint_values[i]); // } jointMsg.position[0] = joint_values[0]; jointMsg.position[1] = joint_values[1]; jointMsg.position[2] = joint_values[2]; //jointMsg.position[3] = 0.0; //GIRO KINECT jointMsg.position[3] = joint_values[3]; //GIRO KINECT jointMsg.position[4] = joint_values[4]; jointMsg.position[5] = joint_values[5]; } else { ROS_ERROR_THROTTLE(2,"INVERSE KINEMATIC IS NOT FEASIBLE!"); } updatePose=false; } // Publicar mensaje para KUKA model jointMsg.header.stamp = ros::Time::now(); kukaJointPub.publish(jointMsg); ros::spinOnce(); loop_rate.sleep(); } ros::shutdown(); return 0; }