Robotics StackExchange | Archived questions

Control multiple manipulators ur3e simultaneously

ROS2 HUMBLE Ubuntu 22.04

I am trying to

Control multiple manipulators ur3e simultaneously

in rviz with moveit interface class I have made urdf file with two robots ur3e in it and created package with moveit setup assistant In package I have defined two planning groups for each manipulator(ur1_group and ur2_group and a third planning group which includes both groups (cobotcontrol) now the problem i am facing is when i control individual groups from cpp node it works fine. when i control both groups simultaneously it did not work. I am not getting it. Please help here is my srdf file i created for moveit setup assistant

<?xml version="1.0" encoding="UTF-8"?>
<!--This does not replace URDF, and is not an extension of URDF.
    This is a format for representing semantic information about the robot structure.
    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
-->
<robot name="ur3e">
    <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
    <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
    <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
    <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
    <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
    <group name="ur1_group">
        <joint name="shoulder_pan_joint"/>
        <joint name="shoulder_lift_joint"/>
        <joint name="elbow_joint"/>
        <joint name="wrist_1_joint"/>
        <joint name="wrist_2_joint"/>
        <joint name="wrist_3_joint"/>
        <chain base_link="base_link" tip_link="tool0"/>
    </group>
    <group name="ur2_group">
        <joint name="shoulder_pan_joint_1"/>
        <joint name="shoulder_lift_joint_1"/>
        <joint name="elbow_joint_1"/>
        <joint name="wrist_1_joint_1"/>
        <joint name="wrist_2_joint_1"/>
        <joint name="wrist_3_joint_1"/>
        <chain base_link="base_link_1" tip_link="tool1"/>
    </group>
    <group name="co_bot_control">
        <group name="ur1_group"/>
        <group name="ur2_group"/>
    </group>
    <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
    <end_effector name="tool0" parent_link="tool0" group="ur1_group"/>
    <end_effector name="tool1" parent_link="tool1" group="ur2_group"/>
    <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
    <virtual_joint name="virtual_joint" type="fixed" parent_frame="world" child_link="base_link"/>
    <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
    <disable_collisions link1="base_link" link2="base_link_1" reason="Adjacent"/>
    <disable_collisions link1="base_link" link2="shoulder_link" reason="Adjacent"/>
    <disable_collisions link1="base_link" link2="shoulder_link_1" reason="Never"/>
    <disable_collisions link1="base_link" link2="upper_arm_link_1" reason="Never"/>
    <disable_collisions link1="base_link_1" link2="shoulder_link" reason="Never"/>
    <disable_collisions link1="base_link_1" link2="shoulder_link_1" reason="Adjacent"/>
    <disable_collisions link1="base_link_1" link2="upper_arm_link" reason="Never"/>
    <disable_collisions link1="ee_link" link2="ee_link_1" reason="Never"/>
    <disable_collisions link1="ee_link" link2="wrist_1_link" reason="Never"/>
    <disable_collisions link1="ee_link" link2="wrist_2_link" reason="Never"/>
    <disable_collisions link1="ee_link" link2="wrist_3_link" reason="Adjacent"/>
    <disable_collisions link1="ee_link_1" link2="wrist_1_link_1" reason="Never"/>
    <disable_collisions link1="ee_link_1" link2="wrist_2_link_1" reason="Never"/>
    <disable_collisions link1="ee_link_1" link2="wrist_3_link_1" reason="Adjacent"/>
    <disable_collisions link1="forearm_link" link2="upper_arm_link" reason="Adjacent"/>
    <disable_collisions link1="forearm_link" link2="wrist_1_link" reason="Adjacent"/>
    <disable_collisions link1="forearm_link" link2="wrist_2_link" reason="Never"/>
    <disable_collisions link1="forearm_link_1" link2="upper_arm_link_1" reason="Adjacent"/>
    <disable_collisions link1="forearm_link_1" link2="wrist_1_link_1" reason="Adjacent"/>
    <disable_collisions link1="forearm_link_1" link2="wrist_2_link_1" reason="Never"/>
    <disable_collisions link1="shoulder_link" link2="shoulder_link_1" reason="Never"/>
    <disable_collisions link1="shoulder_link" link2="upper_arm_link" reason="Adjacent"/>
    <disable_collisions link1="shoulder_link" link2="upper_arm_link_1" reason="Never"/>
    <disable_collisions link1="shoulder_link" link2="wrist_1_link" reason="Never"/>
    <disable_collisions link1="shoulder_link" link2="wrist_2_link" reason="Never"/>
    <disable_collisions link1="shoulder_link_1" link2="upper_arm_link" reason="Never"/>
    <disable_collisions link1="shoulder_link_1" link2="upper_arm_link_1" reason="Adjacent"/>
    <disable_collisions link1="shoulder_link_1" link2="wrist_1_link_1" reason="Never"/>
    <disable_collisions link1="shoulder_link_1" link2="wrist_2_link_1" reason="Never"/>
    <disable_collisions link1="wrist_1_link" link2="wrist_2_link" reason="Adjacent"/>
    <disable_collisions link1="wrist_1_link" link2="wrist_3_link" reason="Never"/>
    <disable_collisions link1="wrist_1_link_1" link2="wrist_2_link_1" reason="Adjacent"/>
    <disable_collisions link1="wrist_1_link_1" link2="wrist_3_link_1" reason="Never"/>
    <disable_collisions link1="wrist_2_link" link2="wrist_3_link" reason="Adjacent"/>
    <disable_collisions link1="wrist_2_link_1" link2="wrist_3_link_1" reason="Adjacent"/>
</robot>

working cpp code

with ur1_group its working fine when i replace ur1_group with ur2_group it will also work fine but i need to change coordinated in Cartesian frame but it works

auto move_group_interface_ur1 = moveit::planning_interface::MoveGroupInterface(node,"ur1_group");
  auto const target_pose_1 = [] {
    geometry_msgs::msg::Pose msg_1;
    msg_1.orientation.x = 0.993434;
    msg_1.orientation.y = -1.54803e-06;
    msg_1.orientation.z = 0.114403;
    msg_1.orientation.w = 1.0;
    msg_1.position.x = 0.28;
    msg_1.position.y = -0.3;
    msg_1.position.z = 0.2;
    return msg_1;
  }();
  move_group_interface_ur1.setPoseTarget(target_pose_1,"tool0");
  auto const [success_1, plan_1] = [&move_group_interface_ur1] {
    moveit::planning_interface::MoveGroupInterface::Plan plan_msg_1;
    auto const ok = static_cast<bool>(move_group_interface_ur1.plan(plan_msg_1));
    return std::make_pair(ok, plan_msg_1);
    }();
  if(success_1)
  {
    moveit::planning_interface::MoveItErrorCode err=move_group_interface_ur1.asyncExecute(plan_1);

  }
  else
  {
    RCLCPP_ERROR(logger, "Planning failed!");
  }

but when i try to control with with third planning group (cobotcontrol)it did not work saying planning failed i am not getting it not working code

  auto move_group_interface_co = moveit::planning_interface::MoveGroupInterface(node,"co_bot_control");

  auto const target_pose_1 = [] {
    geometry_msgs::msg::Pose msg_1;
    msg_1.orientation.x = 0.993434;
    msg_1.orientation.y = -1.54803e-06;
    msg_1.orientation.z = 0.114403;
    msg_1.orientation.w = 1.0;
    msg_1.position.x = 0.28;
    msg_1.position.y = -0.3;
    msg_1.position.z = 0.2;
    return msg_1;
  }();
  move_group_interface_co.setPoseTarget(target_pose_1,"tool0");
  auto const [success_1, plan_1] = [&move_group_interface_co] {
    moveit::planning_interface::MoveGroupInterface::Plan plan_msg_1;
    auto const ok = static_cast<bool>(move_group_interface_co.plan(plan_msg_1));
    return std::make_pair(ok, plan_msg_1);
    }();
  if(success_1)
  {
    moveit::planning_interface::MoveItErrorCode err=move_group_interface_co.asyncExecute(plan_1);

  }
  else
  {
    RCLCPP_ERROR(logger, "Planning failed!");
  }

what i am not doing right here Please help

Asked by Osal22 on 2022-10-30 02:39:46 UTC

Comments

Answers