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:
and this cofiguration for the connection with the microcontroll :
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