42 std::vector<moveit_msgs::Grasp> grasps;
43 for (std::size_t i = 0; i < 20; ++i)
46 p.pose.orientation.x = 0;
47 p.pose.orientation.y = 0;
48 p.pose.orientation.z = 0;
49 p.pose.orientation.w = 1;
52 g.pre_grasp_approach.direction.vector.x = 1.0;
53 g.post_grasp_retreat.direction.vector.z = 1.0;
54 g.post_grasp_retreat.direction.header = p.header;
55 g.pre_grasp_approach.min_distance = 0.2;
56 g.pre_grasp_approach.desired_distance = 0.4;
57 g.post_grasp_retreat.min_distance = 0.1;
58 g.post_grasp_retreat.desired_distance = 0.27;
59 g.pre_grasp_posture.joint_names.resize(1,
"r_gripper_joint");
60 g.pre_grasp_posture.points.resize(1);
61 g.pre_grasp_posture.points[0].positions.resize(1);
62 g.pre_grasp_posture.points[0].positions[0] = 1;
64 g.grasp_posture.joint_names.resize(1,
"r_gripper_joint");
65 g.grasp_posture.points.resize(1);
66 g.grasp_posture.points[0].positions.resize(1);
67 g.grasp_posture.points[0].positions[0] = 0;
71 group.
pick(
"bubu", grasps);
76 std::vector<moveit_msgs::PlaceLocation> loc;
77 for (std::size_t i = 0; i < 20; ++i)
80 p.pose.orientation.x = 0;
81 p.pose.orientation.y = 0;
82 p.pose.orientation.z = 0;
83 p.pose.orientation.w = 1;
84 moveit_msgs::PlaceLocation g;
86 g.pre_place_approach.direction.vector.x = 1.0;
87 g.post_place_retreat.direction.vector.z = 1.0;
88 g.post_place_retreat.direction.header = p.header;
89 g.pre_place_approach.min_distance = 0.2;
90 g.pre_place_approach.desired_distance = 0.4;
91 g.post_place_retreat.min_distance = 0.1;
92 g.post_place_retreat.desired_distance = 0.27;
94 g.post_place_posture.joint_names.resize(1,
"r_gripper_joint");
95 g.post_place_posture.points.resize(1);
96 g.post_place_posture.points[0].positions.resize(1);
97 g.post_place_posture.points[0].positions[0] = 0;
101 group.
place(
"bubu", loc);
108 int main(
int argc,
char** argv)
void demoPick(moveit::planning_interface::MoveGroupInterface &group)
MoveItErrorCode pick(const std::string &object, bool plan_only=false)
Pick up an object.
int main(int argc, char **argv)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
geometry_msgs::PoseStamped getRandomPose(const std::string &end_effector_link="")
Get a random reachable pose for the end-effector end_effector_link. If end_effector_link is empty (th...
MoveItErrorCode place(const std::string &object, bool plan_only=false)
Place an object somewhere safe in the world (a safe location will be detected)
Client class to conveniently use the ROS interfaces provided by the move_group node.
void demoPlace(moveit::planning_interface::MoveGroupInterface &group)