Simulator

README.md

@@ -41,16 +41,30 @@ All parameters use SI units.
 
 ## Usage
 
-Currently this repository offers the `view_tracebot_mockup.launch` file, inside `tracebot_mockup_description` to visualize the mockup model using rviz.
+Currently this repository offers the `tracebot_mockup_simulator.launch` file, inside `tracebot_mockup_simulator` to start the simulated mockup model and visualize it using rviz.
 
 This launchfile exposes the parameters listed in [Model Parameters](#model-parameters) as arguments, providing reasonable defaults.
 For instance, to visualize the model with a 30 degree tilt of the robot bases, run:
 
 ```bash
-roslaunch tracebot_mockup_description view_tracebot_mockup.launch robot_base_tilt:=0.5236
+roslaunch tracebot_mockup_simulator tracebot_mockup_simulator.launch robot_base_tilt:=0.5236
 ```
 
-A camera view can be added to rviz:
+The suggested way to interact with this simulator at this point is to use `rqt_controller_manager` and `rqt_joint_trajectory_controller` to interact with the default controllers in the simulation.
+
+Alternatively, a simpler visualization can be started by launching a `joint_state_publisher_gui`-based setup with:
+
+```bash
+roslaunch tracebot_mockup_description view_tracebot_mockup.launch interactive:=true
+```
+
+The simple visualization allows the initial position of each of the joints to be configured using roslaunch arguments, such as:
+
+```bash
+roslaunch tracebot_mockup_description view_tracebot_mockup.launch interactive:=true left_shoulder_pan_position:=0.0
+```
+
+A camera view can be added to rviz when viewing either the simulator or the simple visualization with:
 
 ```bash
 roslaunch tracebot_mockup_description camera_display.launch
@@ -112,13 +126,20 @@ This is a small wrapper tool around the docker CLI which facilitates tasks such
 The mockup with default parameters can be run using rocker with:
 
 ```bash
-rocker --x11 -- miguelprada/tracebot_mockup:noetic roslaunch tracebot_mockup_description view_tracebot_mockup.launch
+rocker --x11 --name tracebot -- miguelprada/tracebot_mockup:noetic roslaunch tracebot_mockup_simulator tracebot_mockup_simulator.launch
 ```
 
 Note that if your system runs NVidia graphics, you may need to use:
 
 ```bash
-rocker --x11 --nvidia -- miguelprada/tracebot_mockup:noetic roslaunch tracebot_mockup_description view_tracebot_mockup.launch
+rocker --x11 --nvidia --name tracebot -- miguelprada/tracebot_mockup:noetic roslaunch tracebot_mockup_description view_tracebot_mockup.launch
 ```
 
 Alternatively, instructions to enable X11 forwarding in docker in different ways can be found in [this page](http://wiki.ros.org/docker/Tutorials/GUI).
+
+Additional commands may be run in the same container running the simulator using `docker exec`.
+E.g. add the camera visualization with:
+
+```bash
+docker exec -it tracebot bash -c 'source /opt/ros/tracebot_mockup/setup.bash; roslaunch tracebot_mockup_description camera_display.launch
+```

tracebot_mockup_description/launch/load_model.launch

@@ -0,0 +1,39 @@
+<launch>
+
+  <arg name="table_length" default="2.0"/>
+  <arg name="table_width" default="2.0"/>
+  <arg name="table_height" default="1.0"/>
+  <arg name="robot_mount_offset_x" default="0.0"/>
+  <arg name="robot_mount_offset_y" default="-0.2"/>
+  <arg name="robot_mount_offset_theta" default="0.0"/>
+  <arg name="robot_mount_length" default="0.2"/>
+  <arg name="robot_mount_width" default="0.2"/>
+  <arg name="robot_mount_height" default="0.6"/>
+  <arg name="robot_base_tilt" default="0.0"/>
+  <arg name="pump_offset_x" default="0.0"/>
+  <arg name="pump_offset_y" default="0.2"/>
+  <arg name="pump_offset_theta" default="0.0"/>
+  <arg name="pump_length" default="0.2"/>
+  <arg name="pump_width" default="0.3"/>
+  <arg name="pump_height" default="0.4"/>
+
+  <param name="robot_description" command="$(find xacro)/xacro $(find tracebot_mockup_description)/urdf/tracebot_mockup.urdf.xacro
+    table_length:=$(arg table_length)
+    table_width:=$(arg table_width)
+    table_height:=$(arg table_height)
+    robot_mount_offset_x:=$(arg robot_mount_offset_x)
+    robot_mount_offset_y:=$(arg robot_mount_offset_y)
+    robot_mount_offset_theta:=$(arg robot_mount_offset_theta)
+    robot_mount_length:=$(arg robot_mount_length)
+    robot_mount_width:=$(arg robot_mount_width)
+    robot_mount_height:=$(arg robot_mount_height)
+    robot_base_tilt:=$(arg robot_base_tilt)
+    pump_offset_x:=$(arg pump_offset_x)
+    pump_offset_y:=$(arg pump_offset_y)
+    pump_offset_theta:=$(arg pump_offset_theta)
+    pump_length:=$(arg pump_length)
+    pump_width:=$(arg pump_width)
+    pump_height:=$(arg pump_height)
+    "/>
+
+</launch>

tracebot_mockup_description/launch/view_tracebot_mockup.launch

@@ -1,5 +1,7 @@
 <launch>
 
+  <arg name="interactive" default="false"/>
+
   <arg name="table_length" default="2.0"/>
   <arg name="table_width" default="2.0"/>
   <arg name="table_height" default="1.0"/>
@@ -17,26 +19,38 @@
   <arg name="pump_width" default="0.3"/>
   <arg name="pump_height" default="0.4"/>
 
-  <param name="robot_description" command="$(find xacro)/xacro $(find tracebot_mockup_description)/urdf/tracebot_mockup.urdf.xacro
-    table_length:=$(arg table_length)
-    table_width:=$(arg table_width)
-    table_height:=$(arg table_height)
-    robot_mount_offset_x:=$(arg robot_mount_offset_x)
-    robot_mount_offset_y:=$(arg robot_mount_offset_y)
-    robot_mount_offset_theta:=$(arg robot_mount_offset_theta)
-    robot_mount_length:=$(arg robot_mount_length)
-    robot_mount_width:=$(arg robot_mount_width)
-    robot_mount_height:=$(arg robot_mount_height)
-    robot_base_tilt:=$(arg robot_base_tilt)
-    pump_offset_x:=$(arg pump_offset_x)
-    pump_offset_y:=$(arg pump_offset_y)
-    pump_offset_theta:=$(arg pump_offset_theta)
-    pump_length:=$(arg pump_length)
-    pump_width:=$(arg pump_width)
-    pump_height:=$(arg pump_height)
-    "/>
+  <arg name="left_shoulder_pan_position" default="$(eval pi/2)"/>
+  <arg name="left_shoulder_lift_position" default="$(eval -pi/4)"/>
+  <arg name="left_elbow_position" default="$(eval pi/2)"/>
+  <arg name="left_wrist_1_position" default="$(eval -3*pi/4)"/>
+  <arg name="left_wrist_2_position" default="$(eval -pi/2)"/>
+  <arg name="left_wrist_3_position" default="0.0"/>
+  <arg name="right_shoulder_pan_position" default="$(eval -pi/2)"/>
+  <arg name="right_shoulder_lift_position" default="$(eval -3*pi/4)"/>
+  <arg name="right_elbow_position" default="$(eval -pi/2)"/>
+  <arg name="right_wrist_1_position" default="$(eval -pi/4)"/>
+  <arg name="right_wrist_2_position" default="$(eval pi/2)"/>
+  <arg name="right_wrist_3_position" default="0.0"/>
+
+  <include file="$(find tracebot_mockup_description)/launch/load_model.launch" pass_all_args="True"/>
+
+  <node if="$(arg interactive)" name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui">
+    <rosparam param="zeros" subst_value="True">
+      left_shoulder_pan_joint: $(arg left_shoulder_pan_position)
+      left_shoulder_lift_joint: $(arg left_shoulder_lift_position)
+      left_elbow_joint: $(arg left_elbow_position)
+      left_wrist_1_joint: $(arg left_wrist_1_position)
+      left_wrist_2_joint: $(arg left_wrist_2_position)
+      left_wrist_3_joint: $(arg left_wrist_3_position)
+      right_shoulder_pan_joint: $(arg right_shoulder_pan_position)
+      right_shoulder_lift_joint: $(arg right_shoulder_lift_position)
+      right_elbow_joint: $(arg right_elbow_position)
+      right_wrist_1_joint: $(arg right_wrist_1_position)
+      right_wrist_2_joint: $(arg right_wrist_2_position)
+      right_wrist_3_joint: $(arg right_wrist_3_position)
+    </rosparam>
+  </node>
 
-  <node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui"/>
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
   <node name="rviz" pkg="rviz" type="rviz" args="-d $(find tracebot_mockup_description)/config/tracebot_mockup.rviz"/>
 

tracebot_mockup_simulator/CMakeLists.txt

@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(tracebot_mockup_simulator)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+if (CATKIN_ENABLE_TESTING)
+  find_package(roslaunch REQUIRED)
+  roslaunch_add_file_check(launch)
+endif()
+
+install(DIRECTORY config launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/
+)

tracebot_mockup_simulator/config/core_controllers.yaml

@@ -0,0 +1,129 @@
+joint_state_controller:
+  type:         joint_state_controller/JointStateController
+  publish_rate: 50
+
+joint_trajectory_controller:
+  type: velocity_controllers/JointTrajectoryController
+  joints:
+    - left_shoulder_pan_joint
+    - left_shoulder_lift_joint
+    - left_elbow_joint
+    - left_wrist_1_joint
+    - left_wrist_2_joint
+    - left_wrist_3_joint
+    - right_shoulder_pan_joint
+    - right_shoulder_lift_joint
+    - right_elbow_joint
+    - right_wrist_1_joint
+    - right_wrist_2_joint
+    - right_wrist_3_joint
+  constraints:
+    goal_time: &goal_time 5.0
+    left_shoulder_pan_joint: &joint_constraints
+      trajectory: 0.60
+      goal:       0.15
+    left_shoulder_lift_joint: *joint_constraints
+    left_elbow_joint: *joint_constraints
+    left_wrist_1_joint: *joint_constraints
+    left_wrist_2_joint: *joint_constraints
+    left_wrist_3_joint: *joint_constraints
+    right_shoulder_pan_joint: *joint_constraints
+    right_shoulder_lift_joint: *joint_constraints
+    right_elbow_joint: *joint_constraints
+    right_wrist_1_joint: *joint_constraints
+    right_wrist_2_joint: *joint_constraints
+    right_wrist_3_joint: *joint_constraints
+  gains:
+    left_shoulder_pan_joint: &joint_pid
+      p: 2.0
+      i: 0.0
+      d: 0.0
+      i_clamp: 1.0
+    left_shoulder_lift_joint: *joint_pid
+    left_elbow_joint: *joint_pid
+    left_wrist_1_joint: *joint_pid
+    left_wrist_2_joint: *joint_pid
+    left_wrist_3_joint: *joint_pid
+    right_shoulder_pan_joint: *joint_pid
+    right_shoulder_lift_joint: *joint_pid
+    right_elbow_joint: *joint_pid
+    right_wrist_1_joint: *joint_pid
+    right_wrist_2_joint: *joint_pid
+    right_wrist_3_joint: *joint_pid
+  velocity_ff:
+    left_shoulder_pan_joint: &joint_vel_ff 1.0
+    left_shoulder_lift_joint: *joint_vel_ff
+    left_elbow_joint: *joint_vel_ff
+    left_wrist_1_joint: *joint_vel_ff
+    left_wrist_2_joint: *joint_vel_ff
+    left_wrist_3_joint: *joint_vel_ff
+    right_shoulder_pan_joint: *joint_vel_ff
+    right_shoulder_lift_joint: *joint_vel_ff
+    right_elbow_joint: *joint_vel_ff
+    right_wrist_1_joint: *joint_vel_ff
+    right_wrist_2_joint: *joint_vel_ff
+    right_wrist_3_joint: *joint_vel_ff
+
+left_joint_trajectory_controller:
+  type: velocity_controllers/JointTrajectoryController
+  joints:
+    - left_shoulder_pan_joint
+    - left_shoulder_lift_joint
+    - left_elbow_joint
+    - left_wrist_1_joint
+    - left_wrist_2_joint
+    - left_wrist_3_joint
+  constraints:
+    goal_time: *goal_time
+    left_shoulder_pan_joint: *joint_constraints
+    left_shoulder_lift_joint: *joint_constraints
+    left_elbow_joint: *joint_constraints
+    left_wrist_1_joint: *joint_constraints
+    left_wrist_2_joint: *joint_constraints
+    left_wrist_3_joint: *joint_constraints
+  gains:
+    left_shoulder_pan_joint: *joint_pid
+    left_shoulder_lift_joint: *joint_pid
+    left_elbow_joint: *joint_pid
+    left_wrist_1_joint: *joint_pid
+    left_wrist_2_joint: *joint_pid
+    left_wrist_3_joint: *joint_pid
+  velocity_ff:
+    left_shoulder_pan_joint: *joint_vel_ff
+    left_shoulder_lift_joint: *joint_vel_ff
+    left_elbow_joint: *joint_vel_ff
+    left_wrist_1_joint: *joint_vel_ff
+    left_wrist_2_joint: *joint_vel_ff
+    left_wrist_3_joint: *joint_vel_ff
+
+right_joint_trajectory_controller:
+  type: velocity_controllers/JointTrajectoryController
+  joints:
+    - right_shoulder_pan_joint
+    - right_shoulder_lift_joint
+    - right_elbow_joint
+    - right_wrist_1_joint
+    - right_wrist_2_joint
+    - right_wrist_3_joint
+  constraints:
+    goal_time: *goal_time
+    right_shoulder_pan_joint: *joint_constraints
+    right_shoulder_lift_joint: *joint_constraints
+    right_elbow_joint: *joint_constraints
+    right_wrist_1_joint: *joint_constraints
+    right_wrist_2_joint: *joint_constraints
+    right_wrist_3_joint: *joint_constraints
+  gains:
+    right_shoulder_pan_joint: *joint_pid
+    right_shoulder_lift_joint: *joint_pid
+    right_elbow_joint: *joint_pid
+    right_wrist_1_joint: *joint_pid
+    right_wrist_2_joint: *joint_pid
+    right_wrist_3_joint: *joint_pid
+  velocity_ff:
+    right_shoulder_pan_joint: *joint_vel_ff
+    right_shoulder_lift_joint: *joint_vel_ff
+    right_elbow_joint: *joint_vel_ff
+    right_wrist_1_joint: *joint_vel_ff
+    right_wrist_2_joint: *joint_vel_ff
+    right_wrist_3_joint: *joint_vel_ff

tracebot_mockup_simulator/config/tracebot_mockup_hardware.yaml

@@ -0,0 +1,34 @@
+combined_hardware_interface:
+  robot_hardware:
+    - left_ur_hw
+    - right_ur_hw
+
+  left_ur_hw:
+    type: ros_control_boilerplate/SimHWInterface
+    joints: &left_robot_joints
+      - left_shoulder_pan_joint
+      - left_shoulder_lift_joint
+      - left_elbow_joint
+      - left_wrist_1_joint
+      - left_wrist_2_joint
+      - left_wrist_3_joint
+    hardware_interface:
+      joints: *left_robot_joints
+      sim_control_mode: 1      # 0: position, 1: velocity
+
+  right_ur_hw:
+    type: ros_control_boilerplate/SimHWInterface
+    joints: &right_robot_joints
+      - right_shoulder_pan_joint
+      - right_shoulder_lift_joint
+      - right_elbow_joint
+      - right_wrist_1_joint
+      - right_wrist_2_joint
+      - right_wrist_3_joint
+    hardware_interface:
+      joints: *right_robot_joints
+      sim_control_mode: 1      # 0: position, 1: velocity
+
+generic_hw_control_loop:
+  loop_hz: 500
+  cycle_time_error_threshold: 0.002

tracebot_mockup_simulator/launch/tracebot_mockup_simulator.launch

@@ -0,0 +1,45 @@
+<?xml version="1.0"?>
+<launch>
+
+  <arg name="debug" default="false" />
+  <arg unless="$(arg debug)" name="launch_prefix" value="" />
+  <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
+
+  <arg name="table_length" default="2.0"/>
+  <arg name="table_width" default="2.0"/>
+  <arg name="table_height" default="1.0"/>
+  <arg name="robot_mount_offset_x" default="0.0"/>
+  <arg name="robot_mount_offset_y" default="-0.2"/>
+  <arg name="robot_mount_offset_theta" default="0.0"/>
+  <arg name="robot_mount_length" default="0.2"/>
+  <arg name="robot_mount_width" default="0.2"/>
+  <arg name="robot_mount_height" default="0.6"/>
+  <arg name="robot_base_tilt" default="0.0"/>
+  <arg name="pump_offset_x" default="0.0"/>
+  <arg name="pump_offset_y" default="0.2"/>
+  <arg name="pump_offset_theta" default="0.0"/>
+  <arg name="pump_length" default="0.2"/>
+  <arg name="pump_width" default="0.3"/>
+  <arg name="pump_height" default="0.4"/>
+
+  <include file="$(find tracebot_mockup_description)/launch/view_tracebot_mockup.launch" pass_all_args="True"/>
+  <include file="$(find tracebot_mockup_description)/launch/camera_display.launch"/>
+
+  <group ns="tracebot">
+
+    <rosparam file="$(find tracebot_mockup_simulator)/config/tracebot_mockup_hardware.yaml" />
+
+    <node name="combined_hardware_interface" pkg="ros_control_boilerplate" type="rrbot_combined_hw_main" output="screen" launch-prefix="$(arg launch_prefix)"/>
+
+    <rosparam file="$(find tracebot_mockup_simulator)/config/core_controllers.yaml"/>
+
+    <node name="controller_spawner" pkg="controller_manager" type="controller_manager" args="spawn joint_state_controller" />
+    <node name="controller_loader" pkg="controller_manager" type="controller_manager" args="load joint_trajectory_controller left_joint_trajectory_controller right_joint_trajectory_controller" />
+
+  </group>
+
+  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
+    <rosparam param="source_list">[/tracebot/joint_states]</rosparam>
+  </node>
+
+</launch>