Exemplo n.º 1
0
  //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 ***");
}
Exemplo n.º 3
0
/**
 * 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;
}
Exemplo n.º 4
0
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;
}
Exemplo n.º 5
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;
}
Exemplo n.º 6
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;
  
}
Exemplo n.º 7
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();
}
Exemplo n.º 8
0
int main(int argc, char **argv)
{
	ros::init (argc, argv, "move_group_tutorial");
	ros::AsyncSpinner spinner(1);
	spinner.start();
	ros::NodeHandle node_handle("motion_planner_api");

	robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
	robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();

	planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model));

	boost::scoped_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager> > planner_plugin_loader;
	planning_interface::PlannerManagerPtr planner_instance;
	std::string planner_plugin_name;

	if (!node_handle.getParam("/move_group/planning_plugin", planner_plugin_name))
	ROS_FATAL_STREAM("Could not find planner plugin name");
	try
	{
		planner_plugin_loader.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>("moveit_core", "planning_interface::PlannerManager"));
	}
	catch(pluginlib::PluginlibException& ex)
	{
		ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what());
	}
	try
	{
		planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name));
		if (!planner_instance->initialize(robot_model, node_handle.getNamespace()))
		ROS_FATAL_STREAM("Could not initialize planner instance");
		ROS_INFO_STREAM("Using planning interface '" << planner_instance->getDescription() << "'");
	}
	catch(pluginlib::PluginlibException& ex)
	{
		const std::vector<std::string> &classes = planner_plugin_loader->getDeclaredClasses();
		std::stringstream ss;
		for (std::size_t i = 0 ; i < classes.size() ; ++i)
		ss << classes[i] << " ";
		ROS_ERROR_STREAM("Exception while loading planner '" << planner_plugin_name << "': " << ex.what() << std::endl << "Available plugins: " << ss.str());
	}

	ros::WallDuration sleep_time(15.0);
	sleep_time.sleep(); 

	planning_interface::MotionPlanRequest req;
	planning_interface::MotionPlanResponse res;

	geometry_msgs::PoseStamped pose;
	
	pose.header.frame_id = "odom";
	pose.pose.position.x = -0.0119214;
	pose.pose.position.y = 0.0972478;
	pose.pose.position.z = 0.636616;
	pose.pose.orientation.x = 0.273055;
	pose.pose.orientation.y = -0.891262;
	pose.pose.orientation.z = -0.346185;
	pose.pose.orientation.w = 0.106058;

	group.setPoseTarget(target_pose1);

	moveit::planning_interface::MoveGroup::Plan my_plan;
	bool success = group.plan(my_plan);

	if (1)
	{
		ROS_INFO("Visualizing plan 1 (again)");
		display_trajectory.trajectory_start = my_plan.start_state_;
		display_trajectory.trajectory.push_back(my_plan.trajectory_);
		display_publisher.publish(display_trajectory);
		/* Sleep to give Rviz time to visualize the plan. */
		sleep(5.0);
	}

	std::vector<double> group_variable_values;
	group.getCurrentState()->copyJointGroupPositions(group.getCurrentState()->getRobotModel()->getJointModelGroup(group.getName()), group_variable_values);

	group_variable_values[4] = -1.0;
	group.setJointValueTarget(group_variable_values);
	success = group.plan(my_plan);

	ROS_INFO("Visualizing plan 2 (joint space goal) %s",success?"":"FAILED");

	sleep(5.0);

	

	return 0;
}
int main(int argc, char **argv)
{     
      /*Initialise Variables*/
      
      CAPTURE_MOVEMENT=false;//know when you have reach the maximum of points to handle
      //Creating the joint_msg_leap
      joint_msg_leap.name.resize(8);
      joint_msg_leap.position.resize(8);
      joint_msg_leap.name[0]="arm_1_joint";
      joint_msg_leap.name[1]="arm_2_joint";
      joint_msg_leap.name[2]="arm_3_joint";
      joint_msg_leap.name[3]="arm_4_joint";
      joint_msg_leap.name[4]="arm_5_joint";
      joint_msg_leap.name[5]="arm_6_joint";
      joint_msg_leap.name[6]="base_joint_gripper_left";
      joint_msg_leap.name[7]="base_joint_gripper_right";
      aux_enter=1;
      FIRST_VALUE=true;//Help knowing Initial Position of the hand
      int arm_trajectory_point=1;
      
      collision_detection::CollisionRequest collision_request;
      collision_detection::CollisionResult collision_result;
      
      /*Finish Variables Initialitation*/

      //ROS DECLARATION
      ros::init(argc, argv,"listener");
      if (argc != 2)
      {
        ROS_WARN("WARNING: you should specify number of points to capture");
      }
      else
      {
        count=atoi(argv[1]);
        ROS_INFO ("Number of points /n%d", count);
      }
      
      //Create an object of the LeapMotionListener Class
      LeapMotionListener leapmotionlistener;
      leapmotionlistener.Configure(count);
      
      //ros::NodeHandle node_handle;
      ros::NodeHandle node_handle("~");
      // start a ROS spinning thread
      ros::AsyncSpinner spinner(1);
      spinner.start();
      //we need this for leap
      ros::Rate r(1);
      //robo_pub = n.advertise<sensor_msgs::JointState>("joint_leap", 100);
      
      //Creating a Robot Model
      robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
      robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();
      //Initialise PlanningSceneMonitorPtr

      
      /* MOVEIT Setup*/
      // ^^^^^
      moveit::planning_interface::MoveGroup group("arm");
      group.setPlanningTime(0.5);
      moveit::planning_interface::MoveGroup::Plan my_plan;
      // We will use the :planning_scene_interface:`PlanningSceneInterface`
      // class to deal directly with the world.
      moveit::planning_interface::PlanningSceneInterface planning_scene_interface;  
      
      /* Adding/Removing Objects and Attaching/Detaching Objects*/
      ROS_INFO("CREATING PLANNING_SCENE PUBLISHER");
      ros::Publisher planning_scene_diff_publisher = node_handle.advertise<moveit_msgs::PlanningScene>("/planning_scene", 1);
      while(planning_scene_diff_publisher.getNumSubscribers() < 1)
      {
        ros::WallDuration sleep_t(0.5);
        sleep_t.sleep();
      }
      ROS_INFO("CREATING COLLISION OBJECT");
      
      moveit_msgs::AttachedCollisionObject attached_object;
      attached_object.link_name = "";
      /* The header must contain a valid TF frame*/
      attached_object.object.header.frame_id = "gripper_left";
      /* The id of the object */
      attached_object.object.id = "box";

      /* A default pose */
      geometry_msgs::Pose posebox;
      posebox.orientation.w = 1.0;

      /* Define a box to be attached */
      shape_msgs::SolidPrimitive primitive;
      primitive.type = primitive.BOX;
      primitive.dimensions.resize(3);
      primitive.dimensions[0] = 0.1;
      primitive.dimensions[1] = 0.1;
      primitive.dimensions[2] = 0.1;

      attached_object.object.primitives.push_back(primitive);
      attached_object.object.primitive_poses.push_back(posebox);
      attached_object.object.operation = attached_object.object.ADD;

      
      ROS_INFO("ADDING COLLISION OBJECT TO THE WORLD");
      
moveit_msgs::PlanningScene planning_scene_msg;
planning_scene_msg.world.collision_objects.push_back(attached_object.object);
planning_scene_msg.is_diff = true;
planning_scene_diff_publisher.publish(planning_scene_msg);
//sleep_time.sleep();

/* First, define the REMOVE object message*/
moveit_msgs::CollisionObject remove_object;
remove_object.id = "box";
remove_object.header.frame_id = "odom_combined";
remove_object.operation = remove_object.REMOVE;

/* Carry out the REMOVE + ATTACH operation */
ROS_INFO("Attaching the object to the right wrist and removing it from the world.");
planning_scene_msg.world.collision_objects.clear();
planning_scene_msg.world.collision_objects.push_back(remove_object);
planning_scene_msg.robot_state.attached_collision_objects.push_back(attached_object);
planning_scene_diff_publisher.publish(planning_scene_msg);

//sleep_time.sleep();
     
      // Create a publisher for visualizing plans in Rviz.
      ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
      planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model,collision_detection::WorldPtr(new collision_detection::World())));
      
      //Creating planning_scene_monitor
      boost::shared_ptr<tf::TransformListener> tf(new tf::TransformListener(ros::Duration(2.0)));
      planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor(new planning_scene_monitor::PlanningSceneMonitor(planning_scene,"robot_description", tf));
      planning_scene_monitor->startSceneMonitor();
      //planning_scene_monitor->initialize(planning_scene);
      planning_pipeline::PlanningPipeline *planning_pipeline= new planning_pipeline::PlanningPipeline(robot_model,node_handle,"planning_plugin", "request_adapters");

      /* Sleep a little to allow time to startup rviz, etc. */
      ros::WallDuration sleep_time(20.0);
      sleep_time.sleep();  
      /*end of MOVEIT Setup*/

      // We can print the name of the reference frame for this robot.
      ROS_INFO("Reference frame: %s", group.getPlanningFrame().c_str());
      // We can also print the name of the end-effector link for this group.
      ROS_INFO("Reference frame: %s", group.getEndEffectorLink().c_str());
      
      // Planning to a Pose goal 1
      // ^^^^^^^^^^^^^^^^^^^^^^^
      // We can plan a motion for this group to a desired pose for the 
      // end-effector  
      ROS_INFO("Planning to INITIAL POSE");
      planning_interface::MotionPlanRequest req;
      planning_interface::MotionPlanResponse res;
      geometry_msgs::PoseStamped pose;
      pose.header.frame_id = "/odom_combined";
      pose.pose.position.x = 0;
      pose.pose.position.y = 0;
      pose.pose.position.z = 1.15;
      pose.pose.orientation.w = 1.0;
      std::vector<double> tolerance_pose(3, 0.01);
      std::vector<double> tolerance_angle(3, 0.01);
      old_pose=pose;
      
      // We will create the request as a constraint using a helper function available
      req.group_name = "arm";
      moveit_msgs::Constraints pose_goal = kinematic_constraints::constructGoalConstraints("gripper_base_link", pose, tolerance_pose, tolerance_angle);
      req.goal_constraints.push_back(pose_goal);
      // Now, call the pipeline and check whether planning was successful.
      planning_pipeline->generatePlan(planning_scene, req, res);
      /* Check that the planning was successful */
      if(res.error_code_.val != res.error_code_.SUCCESS)
      {
      ROS_ERROR("Could not compute plan successfully");
      return 0;
      }
      // Visualize the result
      // ^^^^^^^^^^^^^^^^^^^^
      /* Visualize the trajectory */
      moveit_msgs::DisplayTrajectory display_trajectory;
      ROS_INFO("Visualizing the trajectory 1");
      moveit_msgs::MotionPlanResponse response;
      res.getMessage(response);
      display_trajectory.trajectory_start = response.trajectory_start;
      display_trajectory.trajectory.push_back(response.trajectory);
      display_publisher.publish(display_trajectory);
      //sleep_time.sleep();
      /* End Planning to a Pose goal 1*/

     // First, set the state in the planning scene to the final state of the last plan 
      robot_state::RobotState& robot_state = planning_scene->getCurrentStateNonConst();
      //planning_scene->setCurrentState(response.trajectory_start);
      joint_model_group = robot_state.getJointModelGroup("arm");
      robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
      spinner.stop();
      
      /*Capturing Stage*/
      /*****************/
      ROS_INFO("PRESH ENTER TO START CAPTURING POINTS");
      while (getline(std::cin,s))
      {
        if ('\n' == getchar())
          break;
      }
      
      /* SENSOR SUBSCRIBING */
      //LEAP MOTION
      ROS_INFO("SUBSCRIBING LEAPMOTION");
      ros::Subscriber leapsub = node_handle.subscribe("/leapmotion/data", 1000, &LeapMotionListener::leapmotionCallback, &leapmotionlistener);
      ros::Subscriber trajectorysub = node_handle.subscribe("/move_group/", 1000, &LeapMotionListener::leapmotionCallback, &leapmotionlistener);
      while(!CAPTURE_MOVEMENT==true)
      {
       
       ros::spinOnce();
       
      }
      leapsub.shutdown();
      ROS_INFO("CAPTURING POINTS FINISH...PROCESSING POINTS");
      // End of Capturing Stage
      

      /* Start Creating Arm Trajectory*/
      /**********************************/
      
      ROS_INFO("START CREATING ARM TRAJECTORY");
      for (unsigned i=0; i<trajectory_hand.size(); i++)
      {
        //First we set the initial Position of the Hand
        if (FIRST_VALUE)
        {
        dataLastHand_.palmpos.x=trajectory_hand.at(i).palmpos.x;
        dataLastHand_.palmpos.y=trajectory_hand.at(i).palmpos.y;
        dataLastHand_.palmpos.z=trajectory_hand.at(i).palmpos.z;
        FIRST_VALUE=0;
        ROS_INFO("ORIGINAL POSITION OF THE HAND SET TO \n X: %f\n  Y: %f\n Z: %f\n ",trajectory_hand.at(i).palmpos.x,trajectory_hand.at(i).palmpos.y,trajectory_hand.at(i).palmpos.z);
        sleep(2);
        }
        else
        {
        // Both limits for x,y,z to avoid small changes
        Updifferencex=dataLastHand_.palmpos.x+10;
        Downdifferencex=dataLastHand_.palmpos.x-10;
        Updifferencez=dataLastHand_.palmpos.z+10;
        Downdifferencez=dataLastHand_.palmpos.z-20;
        Updifferencey=dataLastHand_.palmpos.y+20;
        Downdifferencey=dataLastHand_.palmpos.y-20;
        if ((trajectory_hand.at(i).palmpos.x<Downdifferencex)||(trajectory_hand.at(i).palmpos.x>Updifferencex)||(trajectory_hand.at(i).palmpos.y<Downdifferencey)||(trajectory_hand.at(i).palmpos.y>Updifferencey)||(trajectory_hand.at(i).palmpos.z<Downdifferencez)||(trajectory_hand.at(i).palmpos.z>Updifferencez))
          {
            
            ros::AsyncSpinner spinner(1);
            spinner.start();
            ROS_INFO("TRYING TO ADD POINT %d TO TRAJECTORY",arm_trajectory_point);
            // Cartesian Paths
            // ^^^^^^^^^^^^^^^
            // You can plan a cartesian path directly by specifying a list of waypoints
            // for the end-effector to go through. Note that we are starting
            // from the new start state above. The initial pose (start state) does not
            // need to be added to the waypoint list.
            pose.header.frame_id = "/odom_combined";
            pose.pose.orientation.w = 1.0;
            pose.pose.position.y +=(trajectory_hand.at(i).palmpos.x-dataLastHand_.palmpos.x)/500 ;
            pose.pose.position.z +=(trajectory_hand.at(i).palmpos.y-dataLastHand_.palmpos.y)/1000 ;
            if(pose.pose.position.z>Uplimitez)
            pose.pose.position.z=Uplimitez;
            pose.pose.position.x +=-(trajectory_hand.at(i).palmpos.z-dataLastHand_.palmpos.z)/500 ;
            ROS_INFO("END EFFECTOR POSITION \n X: %f\n  Y: %f\n Z: %f\n", pose.pose.position.x,pose.pose.position.y,pose.pose.position.z);
            ROS_INFO("Palmpos \n X: %f\n  Y: %f\n Z: %f\n ",trajectory_hand.at(i).palmpos.x,trajectory_hand.at(i).palmpos.y,trajectory_hand.at(i).palmpos.z);
            // We will create the request as a constraint using a helper function available
            ROS_INFO("1");
            pose_goal= kinematic_constraints::constructGoalConstraints("gripper_base_link", pose, tolerance_pose, tolerance_angle);
            ROS_INFO("2");
            req.goal_constraints.push_back(pose_goal);
            
            // Now, call the pipeline and check whether planning was successful.
            planning_pipeline->generatePlan(planning_scene, req, res);
            ROS_INFO("3");
            if(res.error_code_.val != res.error_code_.SUCCESS)
            {
            ROS_ERROR("Could not compute plan successfully");
            pose=old_pose;
            }
            else
            {
            // Now when we plan a trajectory it will avoid the obstacle
            
            
            res.getMessage(response);
            
            collision_result.clear();
            collision_detection::AllowedCollisionMatrix acm = planning_scene->getAllowedCollisionMatrix();
            robot_state::RobotState copied_state = planning_scene->getCurrentState();
            planning_scene->checkCollision(collision_request, collision_result, copied_state, acm);
            ROS_INFO_STREAM("Collision Test "<< (collision_result.collision ? "in" : "not in")<< " collision");
            arm_trajectory_point++;
            // Visualize the trajectory 
            ROS_INFO("VISUALIZING NEW POINT");
            //req.goal_constraints.clear();
            display_trajectory.trajectory_start = response.trajectory_start;
            ROS_INFO("AXIS 1 NEXT TRAJECTORY IS %f\n",response.trajectory_start.joint_state.position[1] );
            display_trajectory.trajectory.push_back(response.trajectory); 
            // Now you should see two planned trajectories in series
            display_publisher.publish(display_trajectory);
            planning_scene->setCurrentState(response.trajectory_start);
            if (planning_scene_monitor->updatesScene(planning_scene))
            {
            ROS_INFO("CHANGING STATE");
            planning_scene_monitor->updateSceneWithCurrentState();
            }
            else
            {
            ROS_ERROR("NOT POSSIBLE TO CHANGE THE SCENE");
            }
            robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
            req.goal_constraints.clear();
            old_pose=pose;
            dataLastHand_.palmpos.x=trajectory_hand.at(i).palmpos.x;
            dataLastHand_.palmpos.y=trajectory_hand.at(i).palmpos.y;
            dataLastHand_.palmpos.z=trajectory_hand.at(i).palmpos.z;
            }
            //sleep(2);
            spinner.stop();     
          }
          
        }
      
      }  
   //ros::Subscriber myogestsub = n.subscribe("/myo_gest", 1000, myogestCallback);
        
      return 0;
}
int main(int argc, char **argv)
{
  ros::init (argc, argv, "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();


}
Exemplo n.º 13
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 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;
}
Exemplo n.º 15
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;
}
Exemplo n.º 16
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;
}
Exemplo n.º 17
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();
  }
}
Exemplo n.º 18
0
int main(int argc, char **argv)
{
	int motion = 0;
    std::string initialpath = "";
	if (argc >= 2)
	{
		motion = atoi(argv[1]);
        if(argc >=3)
        {
            initialpath = argv[2];
        }
	}
	printf("%d Motion %d\n", argc, motion);

	ros::init(argc, argv, "move_itomp");
	ros::AsyncSpinner spinner(1);
	spinner.start();
	ros::NodeHandle node_handle("~");

	robot_model_loader::RobotModelLoader robot_model_loader(
			"robot_description");
	robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();

	planning_scene::PlanningScenePtr planning_scene(
			new planning_scene::PlanningScene(robot_model));

	boost::scoped_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager> > planner_plugin_loader;
	planning_interface::PlannerManagerPtr planner_instance;
	std::string planner_plugin_name;

	if (!node_handle.getParam("planning_plugin", planner_plugin_name))
		ROS_FATAL_STREAM("Could not find planner plugin name");
	try
	{
		planner_plugin_loader.reset(
				new pluginlib::ClassLoader<planning_interface::PlannerManager>(
						"moveit_core", "planning_interface::PlannerManager"));
	} catch (pluginlib::PluginlibException& ex)
	{
		ROS_FATAL_STREAM(
				"Exception while creating planning plugin loader " << ex.what());
	}
	try
	{
		planner_instance.reset(
				planner_plugin_loader->createUnmanagedInstance(
						planner_plugin_name));
		if (!planner_instance->initialize(robot_model,
				node_handle.getNamespace()))
			ROS_FATAL_STREAM("Could not initialize planner instance");
		ROS_INFO_STREAM(
				"Using planning interface '" << planner_instance->getDescription() << "'");
	} catch (pluginlib::PluginlibException& ex)
	{
		const std::vector<std::string> &classes =
				planner_plugin_loader->getDeclaredClasses();
		std::stringstream ss;
		for (std::size_t i = 0; i < classes.size(); ++i)
			ss << classes[i] << " ";
		ROS_ERROR_STREAM(
				"Exception while loading planner '" << planner_plugin_name << "': " << ex.what() << std::endl << "Available plugins: " << ss.str());
	}

	loadStaticScene(node_handle, planning_scene, robot_model);

	/* Sleep a little to allow time to startup rviz, etc. */
	ros::WallDuration sleep_time(1.0);
	sleep_time.sleep();

	renderStaticScene(node_handle, planning_scene, robot_model);

	// We will now create a motion plan request
	// specifying the desired pose of the end-effector as input.
	planning_interface::MotionPlanRequest req;
	planning_interface::MotionPlanResponse res;

	std::vector<robot_state::RobotState> robot_states;

	int state_index = 0;

	robot_states.push_back(planning_scene->getCurrentStateNonConst());
    robot_states.push_back(robot_states.back());
    Eigen::VectorXd start_trans, goal_trans;

	// set trajectory constraints
    std::vector<Eigen::VectorXd> waypoints;
    std::vector<std::string> hierarchy;
	// internal waypoints between start and goal

    if(initialpath.empty())
    {
        hierarchy.push_back("base_prismatic_joint_x");
        hierarchy.push_back("base_prismatic_joint_y");
        hierarchy.push_back("base_prismatic_joint_z");
        hierarchy.push_back("base_revolute_joint_z");
        hierarchy.push_back("base_revolute_joint_y");
        hierarchy.push_back("base_revolute_joint_x");
        Eigen::VectorXd vec1;
        start_trans = Eigen::VectorXd(6); start_trans << 0.0, 1.0, 0.0,0,0,0;
        goal_trans =  Eigen::VectorXd(6); goal_trans << 0.0, 2.5, 0.0,0,0,0;
        vec1 = Eigen::VectorXd(6); vec1 << 0.0, 1.5, 1.0,0,0,0;
        waypoints.push_back(vec1);
        vec1 = Eigen::VectorXd(6); vec1 << 0.0, 2.0, 2.0,0,0,0;
        waypoints.push_back(vec1);
        vec1 = Eigen::VectorXd(6); vec1 << 0.0, 2.5, 1.0,0,0,0;
        waypoints.push_back(vec1);
    }
    else
    {
        hierarchy = InitTrajectoryFromFile(waypoints, initialpath);
        start_trans = waypoints.front();
        goal_trans =  waypoints.back();
    }
    setWalkingStates(robot_states[state_index], robot_states[state_index + 1],
            start_trans, goal_trans, hierarchy);
	for (int i = 0; i < waypoints.size(); ++i)
	{
		moveit_msgs::Constraints configuration_constraint =
                setRootJointConstraint(hierarchy, waypoints[i]);
        req.trajectory_constraints.constraints.push_back(
				configuration_constraint);
	}

	displayStates(robot_states[state_index], robot_states[state_index + 1],
				node_handle, robot_model);

	doPlan("whole_body", req, res, robot_states[state_index],
			robot_states[state_index + 1], planning_scene, planner_instance);


	visualizeResult(res, node_handle, 0, 1.0);

	ROS_INFO("Done");
	planner_instance.reset();

	return 0;
}
Exemplo n.º 19
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;
}
Exemplo n.º 20
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;
}
Exemplo n.º 21
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;
}
Exemplo n.º 23
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;
}