void execute_cartesian_ik_trajectory( const ur5_control_moveit::CartesianTrajectory &req) { double time0 = ros::Time::now().toSec(); geometry_msgs::PoseStamped stamped_pose; // stamped_pose.header = req.header; stamped_pose.header.stamp = ros::Time::now(); bool success; std::vector<double *> joint_trajectory; stamped_pose.pose = req.pose; double *trajectory_point = new double[6]; success = run_ik(stamped_pose, current_angles, trajectory_point, "ee_pin_link"); joint_trajectory.push_back(trajectory_point); if(!success){ ROS_ERROR("IK solution not found for trajectory point!\n"); } //run the resulting joint trajectory // ROS_INFO("executing joint trajectory"); success = execute_joint_trajectory(joint_trajectory, req.move_time, time0); // res.success = success; }
//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; }