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