Select Git revision
pick_and_place_sample.cpp
pick_and_place_sample.cpp 12.37 KiB
// ROS
#include <ros/ros.h>
// MoveIt!
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/move_group_interface/move_group_interface.h>
// TF2
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
geometry_msgs::Quaternion q_pick_object;
geometry_msgs::Pose getTargetPickPose(){
std::vector<double> co_dim{0.02, 0.02, 0.2};
std::vector<double> co_pose{0.5, 0.0, 0.2};
geometry_msgs::Pose target_pose;
tf2::Quaternion orientation_gripper(-0.924, 0.383, 0, 0);
tf2::Quaternion orientation_object(q_pick_object.x, q_pick_object.y, q_pick_object.z, q_pick_object.w);
tf2::Quaternion orientation = orientation_gripper * orientation_object;
orientation.setZ(orientation.getZ() * -1);
orientation.setY(orientation.getY() * -1);
orientation.setX(orientation.getX() * -1);
target_pose.orientation = tf2::toMsg(orientation);
target_pose.position.x = co_pose[0];
target_pose.position.y = co_pose[1];
target_pose.position.z = co_pose[2] + 0.2;
return target_pose;
}
void openGripper(trajectory_msgs::JointTrajectory& posture)
{
// BEGIN_SUB_TUTORIAL open_gripper
/* Add both finger joints of panda robot. */
posture.joint_names.resize(2);
posture.joint_names[0] = "panda_finger_joint1";
posture.joint_names[1] = "panda_finger_joint2";
/* Set them as open, wide enough for the object to fit. */
posture.points.resize(1);
posture.points[0].positions.resize(2);
posture.points[0].positions[0] = 0.04;
posture.points[0].positions[1] = 0.04;
posture.points[0].time_from_start = ros::Duration(0.5);
// END_SUB_TUTORIAL
}
void closedGripper(trajectory_msgs::JointTrajectory& posture)
{
// BEGIN_SUB_TUTORIAL closed_gripper
/* Add both finger joints of panda robot. */
posture.joint_names.resize(2);
posture.joint_names[0] = "panda_finger_joint1";
posture.joint_names[1] = "panda_finger_joint2";
/* Set them as closed. */
posture.points.resize(1);
posture.points[0].positions.resize(2);
posture.points[0].positions[0] = 0.00;
posture.points[0].positions[1] = 0.00;
posture.points[0].time_from_start = ros::Duration(0.5);
// END_SUB_TUTORIAL
}
void pick(moveit::planning_interface::MoveGroupInterface& move_group)