launch 2 independently controlled ur5 arms in gazebo

asked 2018-02-14 13:06:14 -0500

I am running ROS Kinetic on Ubuntu 16.04. I am able to launch a single ur5 arm in Gazebo and I am also able to control it from a simple ROS node.

My launch_one_arm.launch file looks like this. I actually just pulled it directly from the ros-industrial/universal_robot repository ( https://github.com/ros-industrial/uni... ).

<?xml version="1.0"?>
<launch>
  <arg name="limited" default="false"/>
  <arg name="paused" default="false"/>
  <arg name="gui" default="true"/>

  <!-- startup simulated world -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" default="worlds/empty.world"/>
    <arg name="paused" value="$(arg paused)"/>
    <arg name="gui" value="$(arg gui)"/>
  </include>

  <!-- send robot urdf to param server -->
  <include file="$(find ur_description)/launch/ur5_upload.launch">
    <arg name="limited" value="$(arg limited)"/>
  </include>

  <!-- push robot_description to factory and spawn robot in gazebo -->
  <node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot -z 0.1" respawn="false" output="screen" />

  <include file="$(find ur_gazebo)/launch/controller_utils.launch"/>

  <rosparam file="$(find ur_gazebo)/controller/arm_controller_ur5.yaml" command="load"/>
  <node name="arm_controller_spawner" pkg="controller_manager" type="controller_manager" args="spawn arm_controller" respawn="false" output="screen"/>

</launch>

And here is my simple move_arm.cpp file that successfully makes the arm move in Gazebo. It creates a JointTrajectory Message and publishes that message to the "/arm_controller/command" Topic.

#include <ros/ros.h>
#include <trajectory_msgs/JointTrajectory.h>
#include <cstdlib>

using namespace std;
float PI = 3.14159;

int main(int argc, char** argv) {

    ros::init(argc, argv, "move_arm");  
        ros::NodeHandle nh;
    ros::Publisher publisher_arm_command = nh.advertise<trajectory_msgs::JointTrajectory>(  "/arm_controller/command", 
                                                10, 
                                                true    );
    // create arm command message
    float joint_velocity = 0.001;
    float joint_acceleration = 0.001;
    float joint_effort = 1.0;
    trajectory_msgs::JointTrajectory message_arm_command;
    message_arm_command.joint_names.resize(6);
    message_arm_command.joint_names[0] = "shoulder_pan_joint";
    message_arm_command.joint_names[1] = "shoulder_lift_joint";
    message_arm_command.joint_names[2] = "elbow_joint";
    message_arm_command.joint_names[3] = "wrist_1_joint";
    message_arm_command.joint_names[4] = "wrist_2_joint";
    message_arm_command.joint_names[5] = "wrist_3_joint";
    trajectory_msgs::JointTrajectoryPoint point1, point2;
    point1.positions.resize(6);
    point1.positions[0] = 0.0 * (PI / 180.0);
    point1.positions[1] = -90.0 * (PI / 180.0);
    point1.positions[2] = 0.0 * (PI / 180.0);
    point1.positions[3] = 0.0 * (PI / 180.0);
    point1.positions[4] = 0.0 * (PI / 180.0);
    point1.positions[5] = 0.0 * (PI / 180.0);
    point1.velocities.resize(6);
    point1.velocities[0] = joint_velocity;
    point1.velocities[1] = joint_velocity;
    point1.velocities[2] = joint_velocity;
    point1.velocities[3] = joint_velocity;
    point1.velocities[4] = joint_velocity;
    point1.velocities[5] = joint_velocity;
    point1.accelerations.resize(6);
    point1.accelerations[0] = joint_acceleration;
    point1.accelerations[1] = joint_acceleration;
    point1.accelerations[2] = joint_acceleration;
    point1.accelerations[3] = joint_acceleration;
    point1.accelerations[4] = joint_acceleration;
    point1.accelerations[5] = joint_acceleration;
    point1.effort.resize(6);
    point1.effort[0] = joint_effort;
    point1.effort[1] = joint_effort;
    point1.effort[2] = joint_effort;
    point1.effort[3] = joint_effort;
    point1.effort[4] = joint_effort;
    point1.effort[5] = joint_effort;
    ros::Duration five_seconds(5.0);
    point1.time_from_start = five_seconds;
    point2.positions.resize(6);
    point2.positions[0] = -90.0 * (PI / 180.0);
    point2.positions[1] = -90.0 * (PI / 180.0);
    point2.positions[2] = 90.0 * (PI / 180.0);
    point2.positions[3] = -90.0 * (PI / 180.0);
    point2.positions[4] = -90.0 * (PI / 180.0);
    point2.positions[5 ...
(more)
edit retag flag offensive close merge delete

Comments

Many of the xacro files seem to have a ${prefix} variable before most of the joint names and such, so that may be the answer I'm looking for. But I can't figure out how I'm supposed to configure and start up 2 separate arms with 2 separate controllers without overlap and conflicts.

Kurt Leucht gravatar imageKurt Leucht ( 2018-02-16 07:14:13 -0500 )edit