Robotics StackExchange | Archived questions

Control 2 phantom x pincher robots

I have 2 robots phatom x pincher. I used the configuration privided by turlebot to controll each robots seperatly:

https://github.com/turtlebot/turtlebot_arm.git

and this cofiguration for the connection with the microcontroll :

https://github.com/corb555/arbotix_ros

This is the code that i used to control one robot:

#include "moveit/move_group_interface/move_group_interface.h"
#include <moveit/planning_scene_interface/planning_scene_interface.h>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "move_group_interface_tutorial");
  ros::NodeHandle n;

  // ROS spinning must be running for the MoveGroupInterface to get information
  // about the robot's state. One way to do this is to start an AsyncSpinner
  // beforehand.
  ros::AsyncSpinner spinner(1);



 spinner.start();

    // MoveIt operates on sets of joints called "planning groups" and stores them in an object called
  // the `JointModelGroup`. Throughout MoveIt the terms "planning group" and "joint model group"
  // are used interchangably.
    static const std::string PLANNING_GROUP_ARM = "arm";
    static const std::string PLANNING_GROUP_GRIPPER = "gripper";

    // The :planning_interface:`MoveGroupInterface` class can be easily
    // setup using just the name of the planning group you would like to control and plan for.
    moveit::planning_interface::MoveGroupInterface move_group_interface_arm(PLANNING_GROUP_ARM);
    moveit::planning_interface::MoveGroupInterface move_group_interface_gripper(PLANNING_GROUP_GRIPPER);
  // We can get a list of all the groups in the robot:
    ROS_INFO_NAMED("tutorial", "Available Planning Groups:");
    std::copy(move_group_interface_arm.getJointModelGroupNames().begin(),
        move_group_interface_arm.getJointModelGroupNames().end(), std::ostream_iterator<std::string>(std::cout, ", "));

   moveit::planning_interface::MoveGroupInterface::Plan my_plan_arm;
   moveit::planning_interface::MoveGroupInterface::Plan my_plan_gripper;



// 1. Initialisation (move to home position )
    move_group_interface_arm.setJointValueTarget(move_group_interface_arm.getNamedTargetValues("result"));

    bool success = (move_group_interface_arm.plan(my_plan_arm) == 
    moveit::planning_interface::MoveItErrorCode::SUCCESS);

    ROS_INFO_NAMED("tutorial", "Home Position %s", success ? "" : "FAILED");

    move_group_interface_arm.move();
    move_group_interface_arm.setPlanningTime(40);
    move_group_interface_gripper.setJointValueTarget(move_group_interface_gripper.getNamedTargetValues("grip_mid"));

    success = (move_group_interface_gripper.plan(my_plan_gripper) == 
   moveit::planning_interface::MoveItErrorCode::SUCCESS);
   ROS_INFO_NAMED("tutorial", "gripper mid close %s", success ? "" : "FAILED");
   move_group_interface_gripper.setPlanningTime(40);
   move_group_interface_gripper.move();

   move_group_interface_gripper.setJointValueTarget(move_group_interface_gripper.getNamedTargetValues("grip_open"));

   success = (move_group_interface_gripper.plan(my_plan_gripper) == 
   moveit::planning_interface::MoveItErrorCode::SUCCESS);

   ROS_INFO_NAMED("tutorial", "gripper open and robot ready to play %s", success ? "" : "FAILED");
   move_group_interface_gripper.setPlanningTime(40);
   move_group_interface_gripper.move();


// 1. Move to target position (Place the TCP (Tool Center Point, the tip of the robot) above the chess piece)
   move_group_interface_arm.setJointValueTarget(move_group_interface_arm.getNamedTargetValues("A8"));

   success = (move_group_interface_arm.plan(my_plan_arm) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

   ROS_INFO_NAMED("tutorial", "Gripper above the chess piece %s", success ? "" : "FAILED");
   move_group_interface_arm.setPlanningTime(40);
   move_group_interface_arm.move();


// 2. Open the gripper

  move_group_interface_gripper.setJointValueTarget(move_group_interface_gripper.getNamedTargetValues("grip_mid"));

  success = (move_group_interface_gripper.plan(my_plan_gripper) == 
  moveit::planning_interface::MoveItErrorCode::SUCCESS);

  ROS_INFO_NAMED("tutorial", "Gripper open and Robot ready to pick %s", success ? "" : "FAILED");
  move_group_interface_gripper.setPlanningTime(40);
  move_group_interface_gripper.move();


//3. Move the gripper close to the object

  move_group_interface_arm.setJointValueTarget(move_group_interface_arm.getNamedTargetValues("A8_1"));
  success = (move_group_interface_arm.plan(my_plan_arm) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

  ROS_INFO_NAMED("tutorial", "Move close to the object %s", success ? "" : "FAILED");
  move_group_interface_arm.setPlanningTime(40);
  move_group_interface_arm.move();

// 4.close the gripper
  move_group_interface_gripper.setJointValueTarget(move_group_interface_gripper.getNamedTargetValues("grip_closed"));

  success = (move_group_interface_gripper.plan(my_plan_gripper) == 
  moveit::planning_interface::MoveItErrorCode::SUCCESS);

  ROS_INFO_NAMED("tutorial", "Gripper closed %s", success ? "" : "FAILED");
  move_group_interface_gripper.setPlanningTime(40);
  move_group_interface_gripper.move();

// 5. Place the TCP (Tool Center Point, the tip of the robot) above the chess piece

  move_group_interface_arm.setJointValueTarget(move_group_interface_arm.getNamedTargetValues("A8"));

  success = (move_group_interface_arm.plan(my_plan_arm) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

  ROS_INFO_NAMED("tutorial", "Gripper above the Target  %s", success ? "" : "FAILED");

  move_group_interface_arm.move();
  move_group_interface_arm.setPlanningTime(40);

   // 6. Move to target position (Place the TCP (Tool Center Point, the tip of the robot) above the chess piece)

  move_group_interface_arm.setJointValueTarget(move_group_interface_arm.getNamedTargetValues("B1"));
  success = (move_group_interface_arm.plan(my_plan_arm) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

  ROS_INFO_NAMED("tutorial", "Gripper above the Target %s", success ? "" : "FAILED");
  move_group_interface_arm.setPlanningTime(40);
  move_group_interface_arm.move();

//7. Move the gripper close to the object

  move_group_interface_arm.setJointValueTarget(move_group_interface_arm.getNamedTargetValues("B1_1"));
  success = (move_group_interface_arm.plan(my_plan_arm) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

  ROS_INFO_NAMED("tutorial", "Move close to the object %s", success ? "" : "FAILED");
  move_group_interface_arm.setPlanningTime(40);
  move_group_interface_arm.move();


// 8. Open the gripper

  move_group_interface_gripper.setJointValueTarget(move_group_interface_gripper.getNamedTargetValues("grip_mid"));

  success = (move_group_interface_gripper.plan(my_plan_gripper) == 
  moveit::planning_interface::MoveItErrorCode::SUCCESS);

  ROS_INFO_NAMED("tutorial", "Gripper open %s", success ? "" : "FAILED");
  move_group_interface_gripper.setPlanningTime(40);
  move_group_interface_gripper.move();


// 9. Place the TCP (Tool Center Point, the tip of the robot) above the chess piece

   move_group_interface_arm.setJointValueTarget(move_group_interface_arm.getNamedTargetValues("B1"));

   success = (move_group_interface_arm.plan(my_plan_arm) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

   ROS_INFO_NAMED("tutorial", "Gripper above the Target %s", success ? "" : "FAILED");
   move_group_interface_arm.setPlanningTime(40);
   move_group_interface_arm.move();


   // 10. Move to Home position

   move_group_interface_arm.setJointValueTarget(move_group_interface_arm.getNamedTargetValues("result"));

   success = (move_group_interface_arm.plan(my_plan_arm) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

   ROS_INFO_NAMED("tutorial", "move to home position  %s", success ? "" : "FAILED");
   move_group_interface_arm.setPlanningTime(40);
    move_group_interface_arm.move();
       ros::shutdown();
       return 0;
}

This is the code for controlling one robot. How can I control both robots at the same time. I mean, the first robot has to do something, then the second one has to do something else. Can you please help me ?

Asked by atef008 on 2023-03-03 19:03:39 UTC

Comments

Answers