Пример #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;
  }
Пример #2
0
  //service function for execute_cartesian_ik_trajectory
  bool execute_cartesian_ik_trajectory(
         ik_trajectory_tutorial::ExecuteCartesianIKTrajectory::Request &req,
         ik_trajectory_tutorial::ExecuteCartesianIKTrajectory::Response &res){

    int trajectory_length = req.poses.size();
    int i, j;
    
    //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[7];    
    get_current_joint_angles(last_angles);
    double *trajectory_point = new double[7];
    //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];
      success = run_ik(stamped_pose, last_angles, trajectory_point, "l_wrist_roll_link");
      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<7; j++){ 
		last_angles[j] = trajectory_point[j];
		if(j == 6)
			ROS_INFO("Trajectory point %f \n", trajectory_point[j]);
	}
    }        

    //run the resulting joint trajectory
    ROS_INFO("executing joint trajectory");
    ros::ServiceClient client = node.serviceClient<simple_trajectory::MoveMultipleJoints>("move_multiple_joints");
    simple_trajectory::MoveMultipleJoints srv;
    srv.request.arm = 1;
    for(int i= 0; i < 7; i++)
	srv.request.jointAngles.push_back(trajectory_point[i]);
    if(client.call(srv))
	ROS_INFO("It worked");
    else
	ROS_INFO("It f****d");
    //success = execute_joint_trajectory(joint_trajectory);
    res.success = success;
    
    return success;
  }
  //service function for execute_cartesian_ik_trajectory
  bool execute_cartesian_ik_trajectory(
         ik_trajectory::ExecuteCartesianIKTrajectory::Request &req,
         ik_trajectory::ExecuteCartesianIKTrajectory::Response &res)
  {

    int trajectory_length = req.poses.size();
    int i, j;
    
    //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, last_angles, trajectory_point, "wrist_3_link");
      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;
  }
Пример #4
0
//send a desired joint trajectory to the joint trajectory action
  //and wait for it to finish
  bool execute_joint_trajectory(std::vector<double *> joint_trajectory)
  {
    int i, j; 
    int trajectorylength = joint_trajectory.size();

    //get the current joint angles
    double current_angles[6];    
    get_current_joint_angles(current_angles);

    //fill the goal message with the desired joint trajectory
    goal.trajectory.points.resize(trajectorylength+1);

    //set the first trajectory point to the current position
    goal.trajectory.points[0].positions.resize(6);
    goal.trajectory.points[0].velocities.resize(6);
    for(j=0; j<6; j++)
    {
      goal.trajectory.points[0].positions[j] = current_angles[j];
      goal.trajectory.points[0].velocities[j] = 0.0; 
    }

   //make the first trajectory point start 0.25 seconds from when we run
    goal.trajectory.points[0].time_from_start = ros::Duration(0.25);     

    //fill in the rest of the trajectory
    double time_from_start = 0.25;
    for(i=0; i<trajectorylength; i++)
    {
      goal.trajectory.points[i+1].positions.resize(6);
      goal.trajectory.points[i+1].velocities.resize(6);

      //fill in the joint positions (velocities of 0 mean that the arm
      //will try to stop briefly at each waypoint)
      for(j=0; j<6; j++)
      {
        goal.trajectory.points[i+1].positions[j] = joint_trajectory[i][j];
        goal.trajectory.points[i+1].velocities[j] = 0.0;
      }

      //compute a desired time for this trajectory point using a max 
      //joint velocity
      double max_joint_move = 0;
      for(j=0; j<6; j++)
      {
        double joint_move = fabs(goal.trajectory.points[i+1].positions[j] 
                                 - goal.trajectory.points[i].positions[j]);
        if(joint_move > max_joint_move) max_joint_move = joint_move;
      }
      double seconds = max_joint_move/MAX_JOINT_VEL;
      ROS_INFO("max_joint_move: %0.3f, seconds: %0.3f", max_joint_move, seconds);
      time_from_start += seconds;
      goal.trajectory.points[i+1].time_from_start = 
        ros::Duration(time_from_start);
    }

    //when to start the trajectory
    goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(0.25);

    ROS_INFO("Sending goal to joint_trajectory_action");
    action_client->sendGoal(goal);

    action_client->waitForResult();

    //get the current joint angles for debugging
    get_current_joint_angles(current_angles);
//    ROS_INFO("joint angles after trajectory: %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f\n",current_angles[0],current_angles[1],current_angles[2],current_angles[3],current_angles[4],current_angles[5]);

    if(action_client->getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
      ROS_INFO("Hooray, the arm finished the trajectory!");
      return 1;
    }
    ROS_INFO("The arm failed to execute the trajectory.");
    return 0;
  }
// up and down the scanner
pr2_controllers_msgs::JointTrajectoryGoal PR2MoveArmJoint::armScanTrajectory(int flag, float down_value, float up_value)
{
    pr2_controllers_msgs::JointTrajectoryGoal goal;

    if(flag==0){
        goal.trajectory.joint_names.push_back("l_shoulder_pan_joint");
        goal.trajectory.joint_names.push_back("l_shoulder_lift_joint");
        goal.trajectory.joint_names.push_back("l_upper_arm_roll_joint");
        goal.trajectory.joint_names.push_back("l_elbow_flex_joint");
        goal.trajectory.joint_names.push_back("l_forearm_roll_joint");
        goal.trajectory.joint_names.push_back("l_wrist_flex_joint");
        goal.trajectory.joint_names.push_back("l_wrist_roll_joint");
        goal.trajectory.points.resize(2);

        double current_angles[7];
        get_current_joint_angles(current_angles);

        // double initalRightWristFlex = current_angles[5];

        // First trajectory point: up
        int ind = 0;
        goal.trajectory.points[ind].positions.resize(7);
        goal.trajectory.points[ind].positions[0] = current_angles[0];
        goal.trajectory.points[ind].positions[1] = current_angles[1];
        goal.trajectory.points[ind].positions[2] = current_angles[2];
        goal.trajectory.points[ind].positions[3] = current_angles[3];
        goal.trajectory.points[ind].positions[4] = current_angles[4];
        goal.trajectory.points[ind].positions[5] = up_value;  // down
        //goal.trajectory.points[ind].positions[5] = -1.697;  // down
        //goal.trajectory.points[ind].positions[5] = -1.7;  // down
        //goal.trajectory.points[ind].positions[5] = -2.0;  // down
        //goal.trajectory.points[ind].positions[5] = -2.05;  // down
        goal.trajectory.points[ind].positions[6] = current_angles[6];
        goal.trajectory.points[ind].velocities.resize(7);
        for (size_t j = 0; j < 7; ++j)
        {
            goal.trajectory.points[ind].velocities[j] = 0.0;
        }
        // goal.trajectory.points[ind].velocities[5] = 0.01;
        // goal.trajectory.points[ind].velocities[6] = 0.0;
        //goal.trajectory.points[ind].time_from_start = ros::Duration(8);   // 5 is great
        goal.trajectory.points[ind].time_from_start = ros::Duration(3);   // 5 is great


        // Second traj point: down
        ind += 1;
        goal.trajectory.points[ind].positions.resize(7);
        goal.trajectory.points[ind].positions[0] = current_angles[0];
        goal.trajectory.points[ind].positions[1] = current_angles[1];
        goal.trajectory.points[ind].positions[2] = current_angles[2];
        goal.trajectory.points[ind].positions[3] = current_angles[3];
        goal.trajectory.points[ind].positions[4] = current_angles[4];
        goal.trajectory.points[ind].positions[5] = down_value;  // up
        //goal.trajectory.points[ind].positions[5] = -1.547;  // up
        //goal.trajectory.points[ind].positions[5] = -1.2;  // up
        //goal.trajectory.points[ind].positions[5] = -1.6;  // up
        // goal.trajectory.points[ind].positions[5] = -1.95;  // up
        goal.trajectory.points[ind].positions[6] = current_angles[6];
        goal.trajectory.points[ind].velocities.resize(7);
        for (size_t j = 0; j < 7; ++j)
        {
            goal.trajectory.points[ind].velocities[j] = 0.0;
        }
        //goal.trajectory.points[ind].velocities[5] = 0.01;
        //goal.trajectory.points[ind].velocities[6] = 0.0;
        //goal.trajectory.points[ind].time_from_start = ros::Duration(16);  // 10 is great
        goal.trajectory.points[ind].time_from_start = ros::Duration(6);  // 10 is great
    }
    else if(flag==1){
        goal.trajectory.joint_names.push_back("r_shoulder_pan_joint");
        goal.trajectory.joint_names.push_back("r_shoulder_lift_joint");
        goal.trajectory.joint_names.push_back("r_upper_arm_roll_joint");
        goal.trajectory.joint_names.push_back("r_elbow_flex_joint");
        goal.trajectory.joint_names.push_back("r_forearm_roll_joint");
        goal.trajectory.joint_names.push_back("r_wrist_flex_joint");
        goal.trajectory.joint_names.push_back("r_wrist_roll_joint");
        goal.trajectory.points.resize(2);

        double current_angles[7];
        get_current_joint_angles(current_angles);

        // double initalRightWristFlex = current_angles[5];

        // First trajectory point: up
        int ind = 0;
        goal.trajectory.points[ind].positions.resize(7);
        goal.trajectory.points[ind].positions[0] = current_angles[0];
        goal.trajectory.points[ind].positions[1] = current_angles[1];
        goal.trajectory.points[ind].positions[2] = current_angles[2];
        goal.trajectory.points[ind].positions[3] = current_angles[3];
        goal.trajectory.points[ind].positions[4] = current_angles[4];
        goal.trajectory.points[ind].positions[5] = up_value;  // down
        //goal.trajectory.points[ind].positions[5] = -1.697;  // down
        //goal.trajectory.points[ind].positions[5] = -1.7;  // down
        //goal.trajectory.points[ind].positions[5] = -2.0;  // down
        //goal.trajectory.points[ind].positions[5] = -2.05;  // down
        goal.trajectory.points[ind].positions[6] = current_angles[6];
        goal.trajectory.points[ind].velocities.resize(7);
        for (size_t j = 0; j < 7; ++j)
        {
            goal.trajectory.points[ind].velocities[j] = 0.0;
        }
        // goal.trajectory.points[ind].velocities[5] = 0.01;
        // goal.trajectory.points[ind].velocities[6] = 0.0;
        //goal.trajectory.points[ind].time_from_start = ros::Duration(8);   // 5 is great
        goal.trajectory.points[ind].time_from_start = ros::Duration(4);   // 5 is great


        // Second traj point: down
        ind += 1;
        goal.trajectory.points[ind].positions.resize(7);
        goal.trajectory.points[ind].positions[0] = current_angles[0];
        goal.trajectory.points[ind].positions[1] = current_angles[1];
        goal.trajectory.points[ind].positions[2] = current_angles[2];
        goal.trajectory.points[ind].positions[3] = current_angles[3];
        goal.trajectory.points[ind].positions[4] = current_angles[4];
        goal.trajectory.points[ind].positions[5] = down_value;  // up
        //goal.trajectory.points[ind].positions[5] = -1.547;  // up
        //goal.trajectory.points[ind].positions[5] = -1.2;  // up
        //goal.trajectory.points[ind].positions[5] = -1.6;  // up
        // goal.trajectory.points[ind].positions[5] = -1.95;  // up
        goal.trajectory.points[ind].positions[6] = current_angles[6];
        goal.trajectory.points[ind].velocities.resize(7);
        for (size_t j = 0; j < 7; ++j)
        {
            goal.trajectory.points[ind].velocities[j] = 0.0;
        }
        //goal.trajectory.points[ind].velocities[5] = 0.01;
        //goal.trajectory.points[ind].velocities[6] = 0.0;
        //goal.trajectory.points[ind].time_from_start = ros::Duration(16);  // 10 is great
        goal.trajectory.points[ind].time_from_start = ros::Duration(6);  // 10 is great
    }

    return goal;
}