MoveIt node for "both arms" of YuMi IRB14000
Hello. I am trying to move a YuMi IRB14000 dual arm from a program using ros's moveit library. I was able to move both the right arm and the left arm independently, one arm at a time, but I get an error when I try to move both arms at the same time. I would appreciate it if someone could help me even the most trivial details.
Here is the node and Error message
int main(int argc, char **argv) {
ros::init(argc, argv, "pick_and_placer_both");
ros::NodeHandle nh;
ros::AsyncSpinner spinner(2);
spinner.start();
moveit::planning_interface::MoveGroupInterface arm_l("both_arms");
moveit::planning_interface::MoveGroupInterface arm_r("both_arms");
arm_l.setPoseReferenceFrame("yumi_base_link");
arm_r.setPoseReferenceFrame("yumi_base_link");
ros::Duration(0.5).sleep();
while (ros::ok()){
geometry_msgs::PoseStamped pose_l,pose_r;
const std::string left = "gripper_l_base";
const std::string right = "gripper_r_base";
pose_l.header.frame_id = "yumi_base_link";
double x,y,z,roll,pitch,yaw,q1,q2,q3,q4;
x=0.2;
y=0.2;
z=0.2;
roll=-82;
pitch=0;
yaw=-90;
pose_l.pose.position.x = 0.2;
pose_l.pose.position.y = 0.2;
pose_l.pose.position.z = 0.2;
q(roll,pitch,yaw,q1,q2,q3,q4);
pose_r.pose.orientation.x = q1;
pose_r.pose.orientation.y = q2;
pose_r.pose.orientation.z = q3;
pose_r.pose.orientation.w = q4;
arm_l.setPoseTarget(pose_l,left);
arm_l.move() ;
pose_r.header.frame_id = "yumi_base_link";
pose_r.pose.position.x = 0.2;
pose_r.pose.position.y = -0.2;
pose_r.pose.position.z = 0.2;
pose_r.pose.orientation.x = q1;
pose_r.pose.orientation.y = q2;
pose_r.pose.orientation.z = q3;
pose_r.pose.orientation.w = q4;
arm_r.setPoseTarget(pose_r,right);
arm_r.move();
}
}
Error message
[ INFO] [1666572340.053687349]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1666572340.053904658]: Planning attempt 1 of at most 1
[ERROR] [1666572340.055182213]: Unable to construct goal representation
Asked by nemunatu on 2022-10-23 19:57:46 UTC
Comments
I am worried by seeing two instances of
moveit::planning_interface::MoveGroupInterface
having the same group, i.e., "both_arms". I think one instance should be sufficient. You should check MoveIt documentation. Furthermore, An example of the "both_arms" group for the Baxter robot in Python language can be seen at ravijo/baxter_moveit_tutorial.Asked by ravijoshi on 2022-10-25 05:04:04 UTC