MoveIt node for "both arms" of YuMi IRB14000

asked 2022-10-23 19:57:46 -0500

nemunatu gravatar image

updated 2022-10-25 05:08:15 -0500

ravijoshi gravatar image

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
edit retag flag offensive close merge delete

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.

ravijoshi gravatar image ravijoshi  ( 2022-10-25 05:04:04 -0500 )edit