How to call pick function using both arms?
I am trying to call pick function using right arm first and then left arm but it is not working.
Here is my code:
moveit::planning_interface::MoveGroup group("right_arm");
group.setPlanningTime(45.0);
ros::WallDuration(1.0).sleep();
geometry_msgs::PoseStamped p;
p.header.frame_id = "odom_combined";
p.pose.position.x = (float)req.a;
p.pose.position.y = (float)req.b;
p.pose.position.z = (float)req.c;
p.pose.orientation.x = (float)req.d;
p.pose.orientation.y = (float)req.e;
p.pose.orientation.z = (float)req.f;
p.pose.orientation.w = (float)req.g;
geometry_msgs::PoseStamped q;
q.header.frame_id = "odom_combined";
q.pose.position.x = (float)req.h;
q.pose.position.y = (float)req.i;
q.pose.position.z = (float)req.j;
q.pose.orientation.x = (float)req.k;
q.pose.orientation.y = (float)req.l;
q.pose.orientation.z = (float)req.m;
q.pose.orientation.w = (float)req.n;
pick_right(group, p);
ros::WallDuration(1.0).sleep();
moveit::planning_interface::MoveGroup group_l("left_arm");
group_l.setPlanningTime(45.0);
ros::WallDuration(1.0).sleep();
pick_left(group_l, q);
I have the corresponding pick_left functions. The right arm picks but nothing happens with the left arm.