//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; }
//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; }
//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; }