Added voice control and joint limits

Works only in cartesian mode
Something about trajectory time being zero again

src/ar3_config/config/ar3_controllers.yaml

@@ -1,5 +1,13 @@
 moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
+
 moveit_simple_controller_manager:
+  trajectory_execution:
+    allowed_execution_duration_scaling: 1.2
+    allowed_goal_duration_margin: 0.5
+    allowed_start_tolerance: 0.01
+
+  moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
+
   controller_names:
     - joint_trajectory_controller
 

src/ar3_config/config/ar3_joy.yaml

@@ -0,0 +1,70 @@
+###############################################
+# Modify all parameters related to servoing here
+###############################################
+use_gazebo: false # Whether the robot is started in a Gazebo simulation environment
+
+## Properties of incoming commands
+command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
+scale:
+  # Scale parameters are only used if command_in_type=="unitless"
+  linear:  0.4 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands.
+  rotational: 0.8 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands.
+  # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic.
+  joint: 0.5
+
+## Properties of outgoing commands
+publish_period: 0.034  # 1/Nominal publish rate [seconds]
+low_latency_mode: true  # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored)
+
+# What type of topic does your robot driver expect?
+# Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory
+command_out_type: trajectory_msgs/JointTrajectory
+
+# What to publish? Can save some bandwidth as most robots only require positions or velocities
+publish_joint_positions: true
+publish_joint_velocities: false
+publish_joint_accelerations: false
+
+## Incoming Joint State properties
+low_pass_filter_coeff: 2.0  # Larger --> trust the filtered data more, trust the measurements less.
+
+## MoveIt properties
+move_group_name:  ar3_arm  # Often 'manipulator' or 'arm'
+planning_frame: base_link  # The MoveIt planning frame. Often 'base_link' or 'world'
+
+## Other frames
+ee_frame_name: link_6  # The name of the end effector link, used to return the EE pose
+robot_link_command_frame: base_link  # commands must be given in the frame of a robot link. Usually either the base or end effector
+
+## Stopping behaviour
+incoming_command_timeout:  10.1  # Stop servoing if X seconds elapse without a new command
+# If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish.
+# Important because ROS may drop some messages and we need the robot to halt reliably.
+num_outgoing_halt_msgs_to_publish: 4
+
+## Configure handling of singularities and joint limits
+lower_singularity_threshold: 170000.0 # Start decelerating when the condition number hits this (close to singularity)
+hard_stop_singularity_threshold: 300000.0 # Stop when the condition number hits this
+joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger.
+
+## Topic names
+cartesian_command_in_topic: ~/delta_twist_cmds  # Topic for incoming Cartesian twist commands
+joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands
+joint_topic: /joint_states
+status_topic: ~/status # Publish status to this topic
+command_out_topic: /joint_trajectory_controller/joint_trajectory # Publish outgoing commands here
+# command_out_topic: /gaycunts # Publish outgoing commands here
+
+## Collision checking for the entire robot body
+check_collisions: false # Check collisions?
+collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often.
+# Two collision check algorithms are available:
+# "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually.
+# "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits
+collision_check_type: threshold_distance
+# Parameters for "threshold_distance"-type collision checking
+self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m]
+scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m]
+# Parameters for "stop_distance"-type collision checking
+collision_distance_safety_factor: 1000.0 # Must be >= 1. A large safety factor is recommended to account for latency
+min_allowable_collision_distance: 0.01 # Stop if a collision is closer than this [m]

src/ar3_config/config/controller_manager.yaml

@@ -1,12 +1,12 @@
 controller_manager:
   ros__parameters:
-    update_rate: 100
+    update_rate: 20
 
     joint_trajectory_controller:
       type: joint_trajectory_controller/JointTrajectoryController
 
     joint_state_broadcaster:
-      type: joint_state_controller/JointStateBroadcaster
+      type: joint_state_broadcaster/JointStateBroadcaster
 
 joint_trajectory_controller:
   ros__parameters:

src/ar3_config/config/joint_limits.yaml

@@ -1,40 +0,0 @@
-joint_limits:
-  joint1:
-    has_velocity_limits: true
-    max_velocity: 40.1750
-    has_acceleration_limits: true
-    max_acceleration: 50.75
-  joint2:
-    has_velocity_limits: true
-    max_velocity: 40.1750
-    has_acceleration_limits: true
-    max_acceleration: 30.875
-  joint3:
-    has_velocity_limits: true
-    max_velocity: 40.1750
-    has_acceleration_limits: true
-    max_acceleration: 40.5
-  joint4:
-    has_velocity_limits: true
-    max_velocity: 40.1750
-    has_acceleration_limits: true
-    max_acceleration: 50.125
-  joint5:
-    has_velocity_limits: true
-    max_velocity: 40.6100
-    has_acceleration_limits: true
-    max_acceleration: 50.75
-  joint6:
-    has_velocity_limits: true
-    max_velocity: 40.6100
-    has_acceleration_limits: true
-    max_acceleration: 70.0
-
-cartesian_limits:
-  max_trans_vel: 10
-  max_trans_acc: 20.25
-  max_trans_dec: -50
-  max_rot_vel: 10.57
-
-default_acceleration_scaling_factor: 1.0
-default_velocity_scaling_factor: 1.0

src/ar3_config/config/ompl_planning.yaml

@@ -153,3 +153,4 @@ move_group:
       - LazyPRMstarkConfigDefault
       - SPARSkConfigDefault
       - SPARStwokConfigDefault
+

src/ar3_config/launch/dev.launch.py

@@ -1,10 +1,12 @@
 from os.path import join
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
-from launch_ros.actions import Node
+from launch_ros.actions import Node, ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
 from launch.actions import ExecuteProcess
 import yaml
 
+# Helper functions to make passing parameters less disgusting
 def get_file(path):
     with open(path, 'r') as f:
         return f.read()
@@ -15,36 +17,91 @@ def get_yaml(path):
 
 
 def generate_launch_description():
+    # Get package locations
     launch_root = get_package_share_directory('ar3_config')
     description_root = get_package_share_directory('ar3_description')
     config_root = join(launch_root, 'config')
 
+    # Load files for the urdf(robot model), srdf (semantic model), and the kinematics
     robot_description  = {'robot_description': get_file(join(description_root, 'ar3.urdf'))}
     robot_description_semantic = {'robot_description_semantic': get_file(join(description_root, 'ar3.srdf'))}
     robot_description_kinematics = get_yaml(join(config_root, 'kinematics.yaml'))
 
     return LaunchDescription([
-        # RViz
+        # Start RViz (ui)
         Node(
             package='rviz2',
             executable='rviz2',
             output='log',
             arguments=['-d', join(launch_root, 'rviz', 'ar3.rviz')],
-            parameters=[robot_description, robot_description_semantic, robot_description_kinematics]),
+            parameters=[robot_description, robot_description_semantic, robot_description_kinematics, get_yaml(join(config_root, 'ompl_planning.yaml'))]
+        ),
 
         # Attatch robot to map
         Node(
             package='tf2_ros',
             executable='static_transform_publisher',
             output='screen',
-            arguments=['0', '0', '0', '0', '0', '0', 'world', 'base_link']),
+            arguments=['0', '0', '0', '0', '0', '0', 'world', 'base_link']
+        ),
 
-        # Controller
+        # creates the cpp interface so you can use /move_pose
+        Node(
+            package='move_group_interface',
+            executable='move_group_interface_node',
+            output='screen',
+            parameters=[robot_description, robot_description_semantic, robot_description_kinematics]
+                #get_yaml(join(config_root, 'ompl_planning.yaml')),
+                #get_yaml(join(config_root, 'trajectory.yaml')),
+                #get_yaml(join(config_root, 'planning_scene.yaml')),
+                #get_yaml(join(config_root, "ar3_controllers.yaml")),
+                #{ "default_planning_pipeline": "ompl" },
+                #{ "planning_pipelines": {"pipeline_names":["ompl"], "ompl": {"planning_plugin": "ompl_interface/OMPLPlanner"} } }]
+        ),
+
+        # Our joystick node
+        #Node(
+        #    package='joystickmove',
+        #    executable='move',
+        #    output='screen',
+        #),
+
+        # Create joystick controller and ComposableNodes for servo_server
+        ComposableNodeContainer(
+            name="joystick_container",
+            namespace="/",
+            package="rclcpp_components",
+            executable="component_container",
+            composable_node_descriptions=[
+                #ComposableNode(
+                #    package="moveit_servo",
+                #    plugin="moveit_servo::ServoServer",
+                #    name="servo_server",
+                #    parameters=[
+                #        {'moveit_servo': get_yaml(join(config_root, 'ar3_joy.yaml'))},
+                #        robot_description,
+                #        robot_description_semantic,
+                #        robot_description_kinematics,
+                #    ],
+                #    extra_arguments=[{"use_intra_process_comms": True}],
+                #),
+                ComposableNode(
+                    package="joy",
+                    plugin="joy::Joy",
+                    name="joy_node",
+                    extra_arguments=[{"use_intra_process_comms": True}],
+                )
+            ],
+            output="screen",
+        ),
+
+        # Setup the controller manager
         Node(
             package='controller_manager',
             executable='ros2_control_node',
             output='screen',
-            parameters=[robot_description, join(config_root, 'controller_manager.yaml')]),
+            parameters=[robot_description, join(config_root, 'controller_manager.yaml')]
+        ),
 
         # Start controllers
         ExecuteProcess(
@@ -52,11 +109,20 @@ def generate_launch_description():
             shell=True,
             output='screen'),
         ExecuteProcess(
-            cmd=['ros2 run controller_manager spawner.py joint_state_controller'],
+            cmd=['ros2 run controller_manager spawner.py joint_state_broadcaster'],
             shell=True,
             output='screen'),
 
-        # Run platform
+        Node(
+                package='voice_move',
+                executable='voice_listener',
+                output='screen'),
+        Node(
+                package='high_lvl_cmd_srv',
+                executable='high_lvl_cmd_srv',
+                output='screen'),
+
+        # Run platform for interface between teensy
         Node(
             package='cobot_platform',
             executable='move',
@@ -64,6 +130,7 @@ def generate_launch_description():
         ),
 
         # Start the actual move_group node/action server
+        # TODO: AAAAAAAAAAAA
         Node(
             package="moveit_ros_move_group",
             executable="move_group",
@@ -76,7 +143,6 @@ def generate_launch_description():
                 get_yaml(join(config_root, 'trajectory.yaml')),
                 get_yaml(join(config_root, 'planning_scene.yaml')),
                 get_yaml(join(config_root, "ar3_controllers.yaml")),
-                get_yaml(join(config_root, "joint_limits.yaml")),
             ],
         ),
 
@@ -85,13 +151,6 @@ def generate_launch_description():
             package='robot_state_publisher',
             executable='robot_state_publisher',
             output='screen',
-            parameters=[robot_description]),
-
-        # Fake robot state publisher
-        # Node(
-        #     package='ar3_description',
-        #     executable='state_publisher',
-        #     name='state_publisher',
-        #     emulate_tty=True,
-        #     output='screen'),
+            parameters=[robot_description]
+        ),
     ])

src/ar3_description/ar3_description/state_publisher.py

@@ -47,7 +47,7 @@ class StatePublisher(Node):
                 # joint_state.position = [random.random() * pi / 2] * 6
                 # joint_state.position = [angle * 16, pi/2., -25 / 180 * pi, 0., 0., 0.]
                 # joint_state.position = [angle * 0.01 * random.random()] * 6
-                joint_state.position = [0., 0., 0., 0., 0., 0.]
+                # joint_state.position = [0., 0., 0., 0., 0., 0.]
 
                 # update transform
                 # (moving in a circle with radius=2)

src/ar3_description/urdf/ar3.urdf

@@ -82,7 +82,7 @@
   </link>
   <joint
     name="joint_1"
-    type="continuous">
+    type="revolute">
     <origin
       xyz="0 0 0.003445"
       rpy="3.1416 -1.0248E-21 -1.6736E-05" />
@@ -92,7 +92,7 @@
       link="link_1" />
     <axis
       xyz="0 0 1" />
-    <limit effort="0" velocity="-20.0" lower="-3.14" upper="3.14" />
+    <limit lower="-2.96706" upper="2.96706" effort="0" velocity="0"/>
   </joint>
   <link
     name="link_2">
@@ -136,7 +136,7 @@
   </link>
   <joint
     name="joint_2"
-    type="continuous">
+    type="revolute">
     <origin
       xyz="0 0.064146 -0.16608"
       rpy="1.5708 0.5236 -1.5708" />
@@ -146,7 +146,7 @@
       link="link_2" />
     <axis
       xyz="0 0 -1" />
-    <limit effort="0" velocity="20.0" lower="-3.14" upper="3.14" />
+    <limit lower="-0.69115024" upper="1.570796" effort="0" velocity="0"/>
   </joint>
   <link
     name="link_3">
@@ -190,17 +190,17 @@
   </link>
   <joint
     name="joint_3"
-    type="continuous">
+    type="revolute">
     <origin
       xyz="0.1525 -0.26414 0"
-      rpy="3.3431E-16 -1.4164E-16 -1.4953816339" />
+      rpy="3.3431E-16 -1.4164E-16 -1.047197" />
     <parent
       link="link_2" />
     <child
       link="link_3" />
     <axis
       xyz="0 0 -1" />
-    <limit effort="0" velocity="200.0" lower="-3.14" upper="3.14" />
+    <limit lower="0" upper="2.5080381" effort="0" velocity="0"/>
   </joint>
   <link
     name="link_4">
@@ -244,7 +244,7 @@
   </link>
   <joint
     name="joint_4"
-    type="continuous">
+    type="revolute">
     <origin
       xyz="0 0 0.00675"
       rpy="1.5708 -1.2554 -1.5708" />
@@ -254,7 +254,7 @@
       link="link_4" />
     <axis
       xyz="0 0 -1" />
-    <limit effort="0" velocity="20.0" lower="-3.14" upper="3.14" />
+    <limit lower="-2.8710666" upper="2.8710666" effort="0" velocity="0"/>
   </joint>
   <link
     name="link_5">
@@ -298,7 +298,7 @@
   </link>
   <joint
     name="joint_5"
-    type="continuous">
+    type="revolute">
     <origin
       xyz="0 0 -0.22225"
       rpy="3.1416 0 -2.8262" />
@@ -308,7 +308,7 @@
       link="link_5" />
     <axis
       xyz="-1 0 0" />
-    <limit effort="0" velocity="20.0" lower="-3.14" upper="3.14" />
+    <limit lower="-1.81776042" upper="1.81776042" effort="0" velocity="0"/>
   </joint>
   <link
     name="link_6">
@@ -352,7 +352,7 @@
   </link>
   <joint
     name="joint_6"
-    type="continuous">
+    type="revolute">
     <origin
       xyz="-0.000294 0 0.02117"
       rpy="0 0 3.1416" />
@@ -362,7 +362,7 @@
       link="link_6" />
     <axis
       xyz="0 0 1" />
-    <limit effort="0" velocity="20.0" lower="-3.14" upper="3.14" />
+    <limit lower="-2.5848326" upper="2.5848326" effort="0" velocity="0"/>
   </joint>
 
 

src/ar3_hardware/include/ar3_hardware/ar3_system.hpp

@@ -60,6 +60,20 @@ private:
   rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr j4_pub_;
   rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr j5_pub_;
   rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr j6_pub_;
+
+  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr s_j1;
+  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr s_j2;
+  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr s_j3;
+  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr s_j4;
+  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr s_j5;
+  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr s_j6;
+
+  void update_j1_cb(std_msgs::msg::Float32::SharedPtr msg);
+  void update_j2_cb(std_msgs::msg::Float32::SharedPtr msg);
+  void update_j3_cb(std_msgs::msg::Float32::SharedPtr msg);
+  void update_j4_cb(std_msgs::msg::Float32::SharedPtr msg);
+  void update_j5_cb(std_msgs::msg::Float32::SharedPtr msg);
+  void update_j6_cb(std_msgs::msg::Float32::SharedPtr msg);
 };
 
 }  // namespace ar3_hardware

src/ar3_hardware/src/ar3_system.cpp

@@ -14,6 +14,7 @@ namespace ar3_hardware
 hardware_interface::return_type Ar3SystemHardware::configure(
   const hardware_interface::HardwareInfo & info)
 {
+  // Initialize default configuration
   if (configure_default(info) != hardware_interface::return_type::OK)
   {
     return hardware_interface::return_type::ERROR;
@@ -21,12 +22,15 @@ hardware_interface::return_type Ar3SystemHardware::configure(
 
   //hw_start_sec_ = stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]);
   //hw_stop_sec_ = stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]);
+
+  // Resize position and command vectors to match actual configuration
+  // This makes sure a default value is initialized for each joint and saves
   hw_positions_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
   hw_commands_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
 
   for (const hardware_interface::ComponentInfo & joint : info_.joints)
   {
-    // DiffBotSystem has exactly two states and one command interface on each joint
+    // We only use one command interface
     if (joint.command_interfaces.size() != 1)
     {
       RCLCPP_FATAL(
@@ -36,6 +40,7 @@ hardware_interface::return_type Ar3SystemHardware::configure(
       return hardware_interface::return_type::ERROR;
     }
 
+    // Make sure the command interface is in position mode
     if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION)
     {
       RCLCPP_FATAL(
@@ -45,6 +50,7 @@ hardware_interface::return_type Ar3SystemHardware::configure(
       return hardware_interface::return_type::ERROR;
     }
 
+    // We only publish to one state interface
     if (joint.state_interfaces.size() != 1)
     {
       RCLCPP_FATAL(
@@ -54,6 +60,7 @@ hardware_interface::return_type Ar3SystemHardware::configure(
       return hardware_interface::return_type::ERROR;
     }
 
+    // Make sure the state interface is in position mode
     if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION)
     {
       RCLCPP_FATAL(
@@ -71,6 +78,7 @@ hardware_interface::return_type Ar3SystemHardware::configure(
 
 std::vector<hardware_interface::StateInterface> Ar3SystemHardware::export_state_interfaces()
 {
+  // Tell the controller about all joints that we publish state info for
   std::vector<hardware_interface::StateInterface> state_interfaces;
   for (auto i = 0u; i < info_.joints.size(); i++)
   {
@@ -83,6 +91,7 @@ std::vector<hardware_interface::StateInterface> Ar3SystemHardware::export_state_
 
 std::vector<hardware_interface::CommandInterface> Ar3SystemHardware::export_command_interfaces()
 {
+  // Tell the controller about all joints that we take commands from
   std::vector<hardware_interface::CommandInterface> command_interfaces;
   for (auto i = 0u; i < info_.joints.size(); i++)
   {
@@ -107,6 +116,12 @@ hardware_interface::return_type Ar3SystemHardware::start()
     }
   }
 
+  // Hack to make sure the node stays running by putting it in its own thread.
+  // this is because we wrote the hardware interface in python earlier and
+  // the driver just listens on some subscribers. This allows a node in this
+  // controller to publish values to that driver
+  // The correct thing to do would be to move all of the driver code into this
+  // controller and remove the node, publishers, and subscribers entierly
   executor_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
   nh_ = std::make_shared<rclcpp::Node>("ar3_hardware");
   executor_->add_node(nh_);
@@ -118,6 +133,8 @@ hardware_interface::return_type Ar3SystemHardware::start()
   };
   thread_executor_spin_ = std::thread(spin);
 
+  // Create joint state publishers
+  // TODO: Don't use Float32, use Pose
   j1_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j1", 10);
   j2_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j2", 10);
   j3_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j3", 10);
@@ -125,6 +142,15 @@ hardware_interface::return_type Ar3SystemHardware::start()
   j5_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j5", 10);
   j6_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j6", 10);
 
+  // Create joint command subscribers
+  // TODO: Don't use Float32, use Pose
+  s_j1 = nh_->create_subscription<std_msgs::msg::Float32>("/update/j1", 10, std::bind(&Ar3SystemHardware::update_j1_cb, this, std::placeholders::_1));
+  s_j2 = nh_->create_subscription<std_msgs::msg::Float32>("/update/j2", 10, std::bind(&Ar3SystemHardware::update_j2_cb, this, std::placeholders::_1));
+  s_j3 = nh_->create_subscription<std_msgs::msg::Float32>("/update/j3", 10, std::bind(&Ar3SystemHardware::update_j3_cb, this, std::placeholders::_1));
+  s_j4 = nh_->create_subscription<std_msgs::msg::Float32>("/update/j4", 10, std::bind(&Ar3SystemHardware::update_j4_cb, this, std::placeholders::_1));
+  s_j5 = nh_->create_subscription<std_msgs::msg::Float32>("/update/j5", 10, std::bind(&Ar3SystemHardware::update_j5_cb, this, std::placeholders::_1));
+  s_j6 = nh_->create_subscription<std_msgs::msg::Float32>("/update/j6", 10, std::bind(&Ar3SystemHardware::update_j6_cb, this, std::placeholders::_1));
+
   status_ = hardware_interface::status::STARTED;
 
   RCLCPP_INFO(rclcpp::get_logger("Ar3SystemHardware"), "System Successfully started!");
@@ -143,10 +169,39 @@ hardware_interface::return_type Ar3SystemHardware::stop()
   return hardware_interface::return_type::OK;
 }
 
+// These callbacks are called from the command subscribers, they update
+// the hw_positions_ array with the current joint state that will be
+// sent to the controller.
+// Again, this is wrong and these values should be filled in by the read() function
+void Ar3SystemHardware::update_j1_cb(std_msgs::msg::Float32::SharedPtr msg) {
+    hw_positions_[0] = msg->data;
+}
+
+void Ar3SystemHardware::update_j2_cb(std_msgs::msg::Float32::SharedPtr msg) {
+    hw_positions_[1] = msg->data;
+}
+
+void Ar3SystemHardware::update_j3_cb(std_msgs::msg::Float32::SharedPtr msg) {
+    hw_positions_[2] = msg->data;
+}
+
+void Ar3SystemHardware::update_j4_cb(std_msgs::msg::Float32::SharedPtr msg) {
+    hw_positions_[3] = msg->data;
+}
+
+void Ar3SystemHardware::update_j5_cb(std_msgs::msg::Float32::SharedPtr msg) {
+    hw_positions_[4] = msg->data;
+}
+
+void Ar3SystemHardware::update_j6_cb(std_msgs::msg::Float32::SharedPtr msg) {
+    hw_positions_[5] = msg->data;
+}
+
 hardware_interface::return_type Ar3SystemHardware::read()
 {
   // RCLCPP_INFO(rclcpp::get_logger("Ar3SystemHardware"), "Reading...");
 
+  /*
   for (uint i = 0; i < hw_commands_.size(); i++)
   {
     hw_positions_[i] = 6.9;
@@ -156,6 +211,7 @@ hardware_interface::return_type Ar3SystemHardware::read()
   //    "Got position state %.5f for '%s'!", hw_positions_[i],
   //    info_.joints[i].name.c_str());
   }
+  */
 
   //RCLCPP_INFO(
   //  rclcpp::get_logger("Ar3SystemHardware"), "Joints successfully read! (%.5f,%.5f,%.5f)",
@@ -168,6 +224,7 @@ hardware_interface::return_type ar3_hardware::Ar3SystemHardware::write()
 {
   //RCLCPP_INFO(rclcpp::get_logger("Ar3SystemHardware"), "Writing...");
 
+  // Publish joint commands to the arm driver
   auto message = std_msgs::msg::Float32();
   message.data = hw_commands_[0];
   j1_pub_->publish(message);

src/cobot_platform/cobot_platform/move.py

@@ -3,34 +3,38 @@ from rclpy.node import Node
 from rclpy.qos import QoSProfile
 
 from sensor_msgs.msg import JointState
+from std_msgs.msg import Float32
 
 from math import pi
 
-from std_msgs.msg import Float32
-
 from serial import Serial
 
-from enum import Enum
-
-class Servos(Enum):
-    J1 = 'A'
-    J2 = 'B'
-    J3 = 'C'
+J1_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * 10 * 4
+J2_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * 50
+J3_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * 50
+J4_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * (13 + 212 / 289) * 2.8
+J5_COUNTS_PER_RAD = 1 / (2 * pi) * 800 * 10
+J6_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * (19 + 38 / 187)
 
 class CobotMove(Node):
+
     def __init__(self):
         super().__init__('cobot_move')
+        # Set up a logger for the node
         self._logger = self.get_logger()
         self._logger.info('COBOT init')
 
+        # Connect to the serial port on the teensy
         self._serial = Serial('/dev/ttyACM0', 115200)
 
+        # Set initial joint values to all 0s
         self._cur = [0.0] * 6
-        # self._buf = [90, 90, 90] # 3 servos
-        # self._prev_buf = [90, 90, 90]
 
+        # create a timer to update all the joint angles at once
+        self._timer = self.create_timer(1 / 40, self._dosomething)
+
+        # Set joint angles independantly
         base_topic = 'move/'
-
         self.create_subscription(Float32, base_topic+'j1', self._move_j1_cb, rclpy.qos.qos_profile_sensor_data)
         self.create_subscription(Float32, base_topic+'j2', self._move_j2_cb, rclpy.qos.qos_profile_sensor_data)
         self.create_subscription(Float32, base_topic+'j3', self._move_j3_cb, rclpy.qos.qos_profile_sensor_data)
@@ -39,70 +43,83 @@ class CobotMove(Node):
         self.create_subscription(Float32, base_topic+'j6', self._move_j6_cb, rclpy.qos.qos_profile_sensor_data)
 
         qos = QoSProfile(depth=10)
-        self._joint_pub = self.create_publisher(JointState, 'joint_states', qos)
-        self._joint_state = JointState()
 
+        # Publishing the values so other nodes can read them (so they don't need to keep track of current positions)
+        publish_base_topic = 'update/'
+        self.s_j1 = self.create_publisher(Float32, publish_base_topic+'j1', qos)
+        self.s_j2 = self.create_publisher(Float32, publish_base_topic+'j2', qos)
+        self.s_j3 = self.create_publisher(Float32, publish_base_topic+'j3', qos)
+        self.s_j4 = self.create_publisher(Float32, publish_base_topic+'j4', qos)
+        self.s_j5 = self.create_publisher(Float32, publish_base_topic+'j5', qos)
+        self.s_j6 = self.create_publisher(Float32, publish_base_topic+'j6', qos)
+
+        # Set the initial position of the robot to 0
         self._serial.write(b'STA0.0.1\n')
         print(self._serial.readline())
 
+    # Callbacks for move/jX subscriptions
     def _move_j1_cb(self, msg):
-        self._cur[0] = int(msg.data / pi / 2. * 8000 * 4)
-        self._move_servo()
+        self._cur[0] = int(msg.data * J1_COUNTS_PER_RAD)
 
     def _move_j2_cb(self, msg):
-        self._cur[1] = int(msg.data / pi / 2. * 10000 * 4)
-        self._move_servo()
+        self._cur[1] = int(msg.data * J2_COUNTS_PER_RAD)
 
     def _move_j3_cb(self, msg):
-        self._cur[2] = int(msg.data / pi / 2. * 10000)
-        self._move_servo()
+        self._cur[2] = int(msg.data * J3_COUNTS_PER_RAD)
 
     def _move_j4_cb(self, msg):
-        self._cur[3] = int(msg.data / pi / 2. * 2000)
-        self._move_servo()
+        self._cur[3] = int(msg.data * J4_COUNTS_PER_RAD)
 
     def _move_j5_cb(self, msg):
-        self._cur[4] = int(msg.data / pi / 2. * 2000)
-        self._move_servo()
+        self._cur[4] = int(msg.data * J5_COUNTS_PER_RAD)
 
     def _move_j6_cb(self, msg):
-        self._cur[5] = int(msg.data / pi / 2. * 2000)
-        self._move_servo()
+        self._cur[5] = int(msg.data * J6_COUNTS_PER_RAD)
 
-    def _move_servo(self):
+    # Send all current positions to the teensy
+    def _dosomething(self):
         self._serial.write(f'MTA{self._cur[0]}B{self._cur[1]}C{self._cur[2]}D{self._cur[3]}E{self._cur[4]}F{self._cur[5]}\n'.encode('UTF-8'))
 
-
+        # Get a response from line above of positions
         line = self._serial.readline().decode('UTF-8')
-        # print(line)
 
+        # Parse the line into each angle
         blocks = re.findall(r'[ABCDEF]-*\d+', line)
-        a = float(blocks[0][1:]) / 8000. / 4. * pi * 2.
-        b = float(blocks[1][1:]) / 10000. / 4. * pi * 2.
-        c = float(blocks[2][1:]) / 10000. * pi * 2.
-        d = float(blocks[3][1:]) / 2000. * pi * 2.
-        e = float(blocks[4][1:]) / 2000. * pi * 2.
-        f = float(blocks[5][1:]) / 2000. * pi * 2.
+        a = float(blocks[0][1:]) / J1_COUNTS_PER_RAD
+        b = float(blocks[1][1:]) / J2_COUNTS_PER_RAD
+        c = float(blocks[2][1:]) / J3_COUNTS_PER_RAD
+        d = float(blocks[3][1:]) / J4_COUNTS_PER_RAD
+        e = float(blocks[4][1:]) / J5_COUNTS_PER_RAD
+        f = float(blocks[5][1:]) / J6_COUNTS_PER_RAD
 
-        self._joint_state.header.stamp = self.get_clock().now().to_msg()
-        self._joint_state.name = ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6']
-        self._joint_state.position = [a, b, c, d, e, f]
+        a = float(self._cur[0] / J1_COUNTS_PER_RAD)
+        b = float(self._cur[1] / J2_COUNTS_PER_RAD)
+        c = float(self._cur[2] / J3_COUNTS_PER_RAD)
+        d = float(self._cur[3] / J4_COUNTS_PER_RAD)
+        e = float(self._cur[4] / J5_COUNTS_PER_RAD)
+        f = float(self._cur[5] / J6_COUNTS_PER_RAD)
 
-        self._joint_pub.publish(self._joint_state)
+        # Publish those to the update/jX subscribers
+        self.s_j1.publish(Float32(data=a))
+        self.s_j2.publish(Float32(data=b))
+        self.s_j3.publish(Float32(data=c))
+        self.s_j4.publish(Float32(data=d))
+        self.s_j5.publish(Float32(data=e))
+        self.s_j6.publish(Float32(data=f))
 
-#        print(self._joint_state.position)
-
-    # def _push ?
 
 def main(args=None):
     rclpy.init(args=args)
 
+    # start the node
     cobot_move = CobotMove()
     rclpy.spin(cobot_move)
 
+    # kill the node
     cobot_move.destroy_node()
     rclpy.shutdown()
 
 
 if __name__ == '__main__':
     main()
+

src/defs/CMakeLists.txt

@@ -0,0 +1,40 @@
+cmake_minimum_required(VERSION 3.5)
+project(defs)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+find_package(rosidl_default_generators REQUIRED)
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "srv/HighLvlCmds.srv"
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/defs/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>defs</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="dw268145@gmail.com">daniel</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <build_depend>rosidl_default_generators</build_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/defs/srv/HighLvlCmds.srv

@@ -0,0 +1,7 @@
+int8 command
+int8 item
+int8 direction
+int8 position
+int16 degrees
+---
+bool success

src/high_lvl_cmd_srv/high_lvl_cmd_srv/cmd_constants.py

@@ -0,0 +1,27 @@
+from enum import Enum
+
+
+class Cmd(Enum):
+    NONE        = -1
+    IGNORE      = 0     # Do not try to execute a command
+    OPEN        = 1     # Open the claw
+    CLOSE       = 2     # Close the claw
+    MOVE        = 3     # Move to a stated position in space
+    TURN        = 4     # Turn a stated degree amount and direction
+    PICKUP      = 5     # Pick up a stated item
+    SETDOWN     = 6     # Set down the currently held item
+    GIVE        = 7     # Give the operator a stated item
+
+class Item(Enum):
+    NONE        = -1
+    CAN         = 0
+    BLOCK       = 1
+    BALL        = 2
+    UNKNOWN     = 3
+
+class Direction(Enum):
+    NONE        = -1
+    LEFT        = 0
+    RIGHT       = 1
+    UP          = 2
+    DOWN        = 3

src/high_lvl_cmd_srv/high_lvl_cmd_srv/high_lvl_cmd_srv.py

@@ -0,0 +1,353 @@
+import rclpy
+from rclpy.node import Node
+
+from std_msgs.msg import Header
+from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion
+
+from defs.srv import HighLvlCmds
+from high_lvl_cmd_srv.cmd_constants import *
+
+import transforms3d
+
+# To add a new voice command, item, etc.:
+# 1. Add to Enum in cmd_constants.py
+# 2. Add words to word list in voice_listener.py
+# 3. Add function to HighLvlCmdSrv
+# 4. Add that function to to HighLvlCmdSrv.CmdFunc
+
+class VoiceCommandException(Exception):
+    pass
+class OpenError(VoiceCommandException):
+    pass
+class MoveError(VoiceCommandException):
+    pass
+class TurnError(VoiceCommandException):
+    pass
+class PickUpError(VoiceCommandException):
+    pass
+class SetDownError(VoiceCommandException):
+    pass
+class GiveError(VoiceCommandException):
+    pass
+class NotRecognizedError(VoiceCommandException):
+    pass
+
+
+# Temp for demo -- position presets
+Position = [    [ 0.0, 0.0, 0.0 ],
+                [ 0.5, 0.0, 0.0 ],
+                [ 0.0, -0.5, 0.4 ],
+                [ 0.0, 0.0, 0.5 ],
+                [ 0.2, 0.7, 0.1 ]   ]
+
+
+class HighLvlCmdSrv(Node):
+    
+    def __init__(self):
+
+        super().__init__('high_lvl_cmd_srv')
+
+        self.logger = self.get_logger()
+
+        self.srv = self.create_service(HighLvlCmds, 'high_lvl_cmds', self._high_lvl_cmd_callback)
+
+        self.fake_action_server_pub = self.create_publisher(PoseStamped, 'fake_action_server_topic', 10) # for testing
+        self.pose_pub = self.create_publisher(PoseStamped, 'move_pose', 10)
+
+        # # Subscribe to hand for self.holdingItem
+        # self.subscription = self.create_subscription(
+        #     ???,
+        #     '???',
+        #     self._hand_callback,
+        #     10)
+
+        self.holdingItem = Item.NONE   # Item currently being held; won't be necessary when hand/arbiter knows this
+
+        self.CmdFunc = {    Cmd.NONE     : self.none,
+                            Cmd.IGNORE   : self.ignore,
+                            Cmd.OPEN     : self.open,
+                            Cmd.CLOSE    : self.close,
+                            Cmd.MOVE     : self.move,
+                            Cmd.TURN     : self.turn,
+                            Cmd.PICKUP   : self.pickup,
+                            Cmd.SETDOWN  : self.setdown,
+                            Cmd.GIVE     : self.give    }
+
+
+    def _hand_callback(self, msg):
+        # Receive info on which object is currently being held
+        self.holdingItem = msg.data
+
+
+    def _high_lvl_cmd_callback(self, request, response):
+
+        self.requestedCommand       = Cmd(request.command)
+        self.requestedItem          = Item(request.item)
+        self.requestedDirection     = Direction(request.direction)
+        self.requestedPosition      = request.position
+        self.requestedDegrees       = request.degrees
+
+        if not (    self.requestedCommand == Cmd.NONE       or  \
+                    self.requestedCommand == Cmd.PICKUP     or  \
+                    self.requestedCommand == Cmd.SETDOWN    or  \
+                    self.requestedCommand == Cmd.GIVE               ):
+            self.tts('This command does not yet interface with the arm. Try pickup, setdown, or give')
+
+        response.success = False
+        try:
+            self.CmdFunc[self.requestedCommand]()
+            response.success = True
+        except Exception as e:
+            self.tts(f"Error: {e}") # Send to some tts node?
+
+        self.requestedCommand       = Cmd.NONE
+        self.requestedItem          = Item.NONE
+        self.requestedDirection     = Direction.NONE
+        self.requestedPosition      = -1
+        self.requestedDegrees       = -1
+
+        return response
+
+
+    def move_pose(self, point, level=False):
+
+        poseStamped = PoseStamped()
+
+        # Default header for now
+        poseStamped.header = Header()
+
+        poseStamped.pose.position = Point(x=point[0],
+                                          y=point[1],
+                                          z=point[2])
+
+        vector = [point[0], point[1], point[2]]
+        if level:
+            vector[2] = 0
+
+        quat = transforms3d.quaternions.axangle2quat(vector, 0)
+        
+        #poseStamped.pose.orientation = Quaternion(w=quat[0],
+        #                                          x=quat[1],
+        #                                          y=quat[2],
+        #                                          z=quat[3])
+        poseStamped.pose.orientation = Quaternion(w=0.7071069,
+                                                  x=0.70710667,
+                                                  y=0.0,
+                                                  z=0.0)
+
+        self.pose_pub.publish(poseStamped)
+        self.fake_action_server_pub.publish(poseStamped)
+
+
+    def open(self):
+
+        # requestedItem = self.parseWords(WordsType.ITEM)
+
+        if self.requestedItem != Item.NONE:
+            if self.requestedItem != self.holdingItem:
+                raise OpenError(f"Can't drop {self.requestedItem} because currently holding {self.holdingItem}")
+            
+        if self.holdingItem == Item.NONE:
+            self.tts('Attempting to open the claw')
+        else:
+            self.tts(f'Attempting to drop the {self.holdingItem}')
+
+        # Send command to action server to open the claw
+        
+        # self._send_goal_future = self._action_client.send_goal_async(
+        #     goal_msg,
+        #     feedback_callback=self.feedback_callback)
+
+        self.holdingItem = Item.NONE # Delete this line once hand/arbiter is publishing this info
+
+
+    def close(self):
+
+        self.tts('Attempting to close the claw')
+
+        # Send command to action server to close the claw
+
+        self.holdingItem = Item.UNKNOWN # Delete this line once hand/arbiter is publishing this info
+
+
+    def move(self):
+        
+        if self.requestedDirection == Direction.NONE:
+            raise TurnError('A direction to move in must be specified')
+
+        # if not (1 <= self.requestedPosition or self.requestedPosition <= len(Position)):
+        #     raise TurnError(f'Please state a position from 1 to {len(Position)}')
+
+        self.tts (f'Attempting to move')
+        # self.tts(f'Attempting to move to a preset position {self.requestedPosition} {self.requestedDirection}')
+
+        # self.move_pose(Position[self.requestedPosition-1])
+
+
+    def turn(self):
+
+        # self.requestedDegrees = self.parseWords(WordsType.DEGREES)
+        # self.requestedDirection = self.parseWords(WordsType.DIRECTION)
+        
+        if self.requestedDirection == Direction.NONE:
+            if self.requestedDegrees % 180 == 0:
+                # Left/right does not matter if doing a 180
+                self.requestedDirection = Direction.LEFT
+            else:
+                raise TurnError('A direction to turn must be specified')
+
+        self.tts(f'Attempting to turn {self.requestedDegrees} degrees {self.requestedDirection}')
+
+        # Calculate new position from current position + degrees to rotate
+
+        # Send command to action server to move to new position
+
+
+    def pickup(self):
+
+        # requestedItem = self.parseWords(WordsType.ITEM)
+
+        # Make sure hand is currently empty
+        if self.holdingItem != Item.NONE:
+            raise PickUpError(f"Can't pick up {self.requestedItem}, already holding {self.holdingItem}")
+
+        if self.requestedItem == Item.NONE:
+            raise PickUpError(f'Did not hear a valid item specified to pick up')
+
+        self.tts(f'Attempting to pick up {self.requestedItem}')
+
+        # Temp
+        self.move_pose(Position[2])
+
+        # Send command to action server to open the claw
+
+        # Use video processing service client to ask for position of target
+        # if !targetFound:
+            # raise PickUpError(f'Unable to find the requested item')
+
+        # Move to current item position, checking if it has moved
+
+        # prevTargetPos = None
+        # targetPos = None
+        # firstTime = True
+        # While (robot hand hasn't yet reached the target) or (firstTime)
+
+            # targetPos = video_processing_service_request(target)
+            
+            # if targetPos == None:  # If the target wasn't found by video processing serivce
+                # if firstTime:
+                    # raise PickUpError(f'Unable to find the requested item')
+                # targetPos = prevTargetPos
+
+            # if firstTime or distance(targetPos, prevTargetPos) > some_threshold_distance
+                # Send goal to action server to move to that position
+
+        # Send command to action server to close the claw
+
+        self.holdingItem = self.requestedItem # Delete this line once hand/arbiter is publishing this info
+
+
+    def setdown(self):
+    
+        # Make sure hand is currently holding something
+        if self.holdingItem == Item.NONE:
+            raise SetDownError(f'Not currently holding anything to set down')
+
+        # requestedItem = self.parseWords(WordsType.ITEM)
+
+        # If an item was mentioned
+        if self.requestedItem != Item.NONE:
+            # Make sure it is the currently held item
+            if self.requestedItem != self.holdingItem:
+                raise SetDownError(f"Can't set down {self.requestedItem}, not currently holding it")
+
+        self.tts(f'Attempting to set down {self.requestedItem}')
+
+
+        # Temp
+        self.move_pose(Position[3])
+
+
+        # Send command to action server to go down to the table/surface (just current position but with Z = 0?)
+
+        # Send command to action server to open the claw
+
+        self.holdingItem = Item.NONE # Delete this line once hand/arbiter is publishing this info
+
+
+    def give(self):
+        
+        # If the claw is not currently holding anything
+        if self.holdingItem == Item.NONE:
+
+            # Pick up the item first
+            self.pickup()
+
+        else:
+
+            # requestedItem = self.parseWords(WordsType.ITEM)
+
+            # If a specific item was requested
+            if self.requestedItem != Item.NONE:
+
+                # Make sure the requested item is currently being held
+                if self.requestedItem != self.holdingItem:
+                    raise GiveError(f"Can't give you {self.requestedItem}, already holding {self.holdingItem}")
+
+
+        self.tts(f'Attempting to give {self.holdingItem} to the operator')
+
+
+        # Temp
+        self.move_pose(Position[5])
+
+        # prevHandPos = None
+        # handPos = None
+        # firstTime = True
+        # While (robot hand hasn't yet reached operator hand) or (firstTime)
+
+            # handPos = video_processing_service_request(opHand)
+            
+            # if handPos == None:  # If operator hand wasn't found by video processing serivce
+                # if firstTime:
+                    # raise GiveError('Unable to find the requested item')
+                # handPos = prevHandPos
+
+            # if firstTime or distance(handPos, prevHandPos) > some_threshold_distance
+                # Send goal to action server to move to that position
+
+
+        # isGrabbed = hand_service_client(something) to block until the hand determines that the operator is holding the target
+        # if isGrabbed:
+            # Send command to action server to open the claw
+        # else:
+            # raise GrabError(f'Couldn't tell whether the {self.holdingItem} was being grabbed')
+
+        self.holdingItem = Item.NONE # Delete this line once hand/arbiter is publishing this info
+
+
+    def ignore(self):
+        self.tts('Ignoring')
+
+
+    def none(self):
+        raise NotRecognizedError("Sorry, I didn't get that.")
+
+
+    def tts(self, phrase):
+        # Send to some text-to-speech node, maybe the arbiter?
+        self.logger.info(phrase)
+
+
+
+def main(args=None):
+
+    rclpy.init(args=args)
+    high_lvl_cmd_service = HighLvlCmdSrv()
+    rclpy.spin(high_lvl_cmd_service)
+
+    high_lvl_cmd_service.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/high_lvl_cmd_srv/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>high_lvl_cmd_srv</name>
+  <version>0.0.0</version>
+  <description>Service server for high-level commands</description>
+  <maintainer email="dwall2018@my.fit.edu">Daniel Wall</maintainer>
+  <license>TODO: License declaration</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+  
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/high_lvl_cmd_srv/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script-dir=$base/lib/high_lvl_cmd_srv
+[install]
+install-scripts=$base/lib/high_lvl_cmd_srv

src/high_lvl_cmd_srv/setup.py

@@ -0,0 +1,26 @@
+from setuptools import setup
+
+package_name = 'high_lvl_cmd_srv'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='Daniel Wall',
+    maintainer_email='dwall2018@my.fit.edu',
+    description='Service server for high-level commands',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'high_lvl_cmd_srv = high_lvl_cmd_srv.high_lvl_cmd_srv:main'
+        ],
+    },
+)

src/high_lvl_cmd_srv/test/test_copyright.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

src/high_lvl_cmd_srv/test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

src/high_lvl_cmd_srv/test/test_pep257.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'

src/joystickmove/joystickmove/move.py

@@ -1,30 +1,141 @@
 import rclpy
 from rclpy.node import Node
 
+from std_srvs.srv import Trigger
+
 from std_msgs.msg import Float32
 from sensor_msgs.msg import Joy
+from geometry_msgs.msg import TwistStamped
+from control_msgs.msg import JointJog
+from moveit_msgs.msg import PlanningScene, PlanningSceneWorld
+
+from enum import Enum
+
+
+# Mappings for a DualShock4 (PS4) controller
+class Buttons(Enum):
+    CROSS = 0
+    CIRCLE = 1
+    TRIANGLE = 2
+    SQUARE = 3
+    L1 = 4
+    R1 = 5
+    L2 = 6
+    R2 = 7
+    SELECT = 8
+    START = 9
+    CENTER = 10
+    L3 = 11
+    R3 = 12
+
+class Axes(Enum):
+    LEFT_X = 0
+    LEFT_Y = 1
+    L1 = 2
+    RIGHT_X = 3
+    RIGHT_Y = 4
+    R1 = 5
+    DPAD_UPDOWN = 6
+    DPAD_LEFTRIGHT = 7
 
 class JoyMove(Node):
     def __init__(self):
         super().__init__('joystickmove')
+
+        # Get the logger
         self._logger = self.get_logger()
         self._logger.info('joystick init')
 
+        # Set up topics for publishing
+        JOY_TOPIC = '/joy'
+        TWIST_TOPIC = '/servo_server/delta_twist_cmds'
+        JOINT_TOPIC = '/servo_server/delta_joint_cmds'
+
+        # Define some constants
+        EE = 'joint_6'
+        BASE = 'base_link'
+
+        # Create the needed subscribers and publishers for moveit_servo
         self.create_subscription(Joy, '/joy', self._joy_cb, rclpy.qos.qos_profile_sensor_data)
 
-        self.base_pub = self.create_publisher(Float32, '/move/j1', 10)
+        self.twist_pub = self.create_publisher(TwistStamped, TWIST_TOPIC, 10);
+        self.joint_pub = self.create_publisher(JointJog, JOINT_TOPIC, 10);
+        self.collision_pub = self.create_publisher(PlanningScene, "/planning_scene", 10);
 
+        # Start a moveit_servo client
+        self.servo_client = self.create_client(Trigger, '/servo_server/start_servo')
+        # Check if the a service is available
+        while not self.servo_client.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info('servo_server not available, waiting again...')
+        self._logger.info('servo_server connected.')
+
+        # Create a request to tell the server the joystick exists
+        req = Trigger.Request()
+        self.servo_client.call_async(req)
+
+    # Callback for any button or stick that is pressed on the joystick
     def _joy_cb(self, msg):
-        asd = msg.axes[1]
-        self.base_pub.publish(Float32(data=asd))
+        axes = msg.axes
+        buttons = msg.buttons
+
+        # If you press main buttons or dpad arrows
+        if buttons[Buttons.CROSS.value] or buttons[Buttons.CIRCLE.value] or buttons[Buttons.TRIANGLE.value] \
+            or buttons[Buttons.SQUARE.value] or axes[Axes.DPAD_UPDOWN.value] or axes[Axes.DPAD_LEFTRIGHT.value]:
+
+            # combine default values and whatever you have pushed to create a JointJog object
+            joint = JointJog()
+
+            joint.joint_names = ['joint_1', 'joint_2',
+                                 'joint_6', 'joint_5']
+            joint.velocities = [float(axes[Axes.DPAD_UPDOWN.value]), float(axes[Axes.DPAD_LEFTRIGHT.value]),
+                                float(buttons[Buttons.CIRCLE.value] - buttons[Buttons.CROSS.value]), float(buttons[Buttons.SQUARE.value] - buttons[Buttons.TRIANGLE.value])]
+            joint.duration = 0.1
+
+            # Add a timestamp so we know when the action actually took place
+            joint.header.stamp = self.get_clock().now().to_msg()
+            joint.header.frame_id = "link_6"
+
+            # Publish the JointJog to servo_server
+            self.joint_pub.publish(joint)
+            return
+
+        # If you pressed a trigger or stick
+        ts = TwistStamped()
+
+        # Define linear depth from right stick
+        ts.twist.linear.z = axes[Axes.RIGHT_Y.value]
+        ts.twist.linear.y = axes[Axes.RIGHT_X.value]
+
+        # Define linear X from trigger analog values
+        lin_x_right = -0.5 * axes[Axes.R1.value] # - AXIS_DEFAULTS.at(R1));
+        lin_x_left = 0.5 * axes[Axes.L1.value] # - AXIS_DEFAULTS.at(L1));
+        ts.twist.linear.x = lin_x_right + lin_x_left
+
+        # Define angular up and down from left stick
+        ts.twist.angular.y = axes[Axes.LEFT_Y.value]
+        ts.twist.angular.x = axes[Axes.LEFT_X.value]
+
+        # Left and Right bumpers to control roll
+        roll_positive = buttons[Buttons.R2.value]
+        roll_negative = -1 * buttons[Buttons.L2.value]
+        ts.twist.angular.z = float(roll_positive + roll_negative)
+
+        # Add a timestamp so we know when the action actually took place
+        ts.header.stamp = self.get_clock().now().to_msg()
+        ts.header.frame_id = "base_link"
+
+        # Publish the Twist to servo_server
+        self.twist_pub.publish(ts)
 
 
 def main(args=None):
     rclpy.init(args=args)
 
+    # Start the node
     joy_move = JoyMove()
     rclpy.spin(joy_move)
 
+    # Kill the node
     joy_move.destroy_node()
     rclpy.shutdown()
 

src/joystickmove/setup.py

@@ -20,7 +20,7 @@ setup(
     tests_require=['pytest'],
     entry_points={
         'console_scripts': [
-            'joystickmove = joystickmove.move:main'
+            'move = joystickmove.move:main'
         ],
     },
 )

src/move_group_interface/CMakeLists.txt

@@ -0,0 +1,55 @@
+cmake_minimum_required(VERSION 3.5)
+project(move_group_interface)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(moveit_ros_planning_interface REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+# Create service stuff
+#rosidl_generate_interfaces(${PROJECT_NAME}
+#    "srv/MovePose.srv"
+#  DEPENDENCIES geometry_msgs
+#)
+
+# move_group_node
+add_executable(move_group_interface_node src/move_group_interface_node.cpp)
+ament_target_dependencies(move_group_interface_node rclcpp geometry_msgs moveit_ros_planning_interface)
+#rosidl_target_interfaces(move_group_interface_node ${PROJECT_NAME} "rosidl_typesupport_cpp")
+
+#add_executable(client_node src/client_node.cpp)
+#ament_target_dependencies(client_node rclcpp)
+#rosidl_target_interfaces(client_node ${PROJECT_NAME} "rosidl_typesupport_cpp")
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+# Mark executables for installation
+install(TARGETS move_group_interface_node
+    DESTINATION lib/${PROJECT_NAME})
+
+ament_package()

src/move_group_interface/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>move_group_interface</name>
+  <version>1.0.0</version>
+  <description>MoveGroupInterface but for ros2</description>
+  <maintainer email="farrell2017@my.fit.edu">john</maintainer>
+  <license>TODO: License declaration</license>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>rosidl_default_generators</build_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <depend>rclcpp</depend>
+  <depend>geometry_msgs</depend>
+  <depend>moveit_ros_planning_interface</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/move_group_interface/src/move_group_interface_node.cpp

@@ -0,0 +1,117 @@
+// https://github.com/ros-planning/moveit2_tutorials/blob/main/doc/examples/moveit_cpp/src/moveit_cpp_tutorial.cpp
+
+// https://moveit.picknik.ai/foxy/doc/move_group_interface/move_group_interface_tutorial.html
+// https://moveit.picknik.ai/foxy/doc/robot_model_and_robot_state/robot_model_and_robot_state_tutorial.html
+// https://docs.ros.org/en/indigo/api/moveit_core/html/classmoveit_1_1core_1_1RobotState.html
+// https://docs.ros.org/en/kinetic/api/moveit_core/html/classmoveit_1_1core_1_1JointModelGroup.html
+
+
+
+#include <rclcpp/rclcpp.hpp>
+#include <string>
+#include <thread>
+
+// MoveitCpp
+#include <moveit/common_planning_interface_objects/common_objects.h>
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit/rdf_loader/rdf_loader.h>
+#include <moveit/robot_trajectory/robot_trajectory.h>
+#include <moveit/trajectory_processing/iterative_time_parameterization.h>
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+
+const std::string PLANNING_GROUP = "ar3_arm";
+
+class ScanNPlan : public rclcpp::Node {
+public:
+    ScanNPlan() : Node("scan_n_plan") {
+        callback_group_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant);
+
+        rclcpp::SubscriptionOptions options;
+        options.callback_group = callback_group_;
+
+        // Create /move_pose subscriber
+        pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
+            "move_pose",
+            rclcpp::QoS(1),
+            std::bind(&ScanNPlan::moveCallback, this, std::placeholders::_1),
+            options
+        );
+
+        relative_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
+            "move_relative_pose",
+            rclcpp::QoS(1),
+            std::bind(&ScanNPlan::relativeMoveCallback, this, std::placeholders::_1),
+            options
+        );
+    }
+
+    void init() {
+        move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(
+                shared_from_this(), std::string("ar3_arm"));
+    }
+
+private:
+    void moveCallback(geometry_msgs::msg::PoseStamped::SharedPtr p) {
+    	RCLCPP_INFO(this->get_logger(), "GOT MSG");
+
+        move_group_->setStartStateToCurrentState();
+        move_group_->setPoseTarget(p->pose);
+
+        moveit::planning_interface::MoveGroupInterface::Plan plan;
+        bool success = (move_group_->plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
+
+        RCLCPP_INFO(this->get_logger(), "Did work? %s", success ? "yes" : "no");
+
+        move_group_->move();
+    }
+
+    void relativeMoveCallback(geometry_msgs::msg::PoseStamped::SharedPtr p) {
+    	RCLCPP_INFO(this->get_logger(), "GOT MSG");
+
+        moveit::core::RobotStatePtr robot_state = move_group_->getCurrentState();
+        const Eigen::Isometry3d &end_effector_state = robot_state->getGlobalLinkTransform(move_group_->getEndEffectorLink());
+        RCLCPP_INFO_STREAM(this->get_logger(), "POS: \n" << end_effector_state.translation() << "\n");
+        RCLCPP_INFO_STREAM(this->get_logger(), "ROT: \n" << end_effector_state.rotation() << "\n");
+
+        geometry_msgs::msg::Pose target_pose;
+        target_pose.position.x = 0.24;
+        target_pose.position.y = -0.32;
+        target_pose.position.z = 0.56;
+        move_group_->setPoseTarget(target_pose);
+
+        moveit::planning_interface::MoveGroupInterface::Plan plan;
+        bool success = (move_group_->plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
+
+        RCLCPP_INFO(this->get_logger(), "Did work? %s", success ? "yes" : "no");
+
+        move_group_->move();
+    }
+private:
+    rclcpp::CallbackGroup::SharedPtr callback_group_;
+
+    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
+    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr relative_pose_sub_;
+
+    moveit::planning_interface::MoveGroupInterfacePtr move_group_;
+    moveit::core::RobotModelConstPtr robot_model_;
+};
+
+int main(int argc, char* argv[]) {
+    rclcpp::init(argc, argv);
+
+    auto node = std::make_shared<ScanNPlan>();
+    RCLCPP_INFO(node->get_logger(), "MoveGroup node started.");
+
+    rclcpp::executors::MultiThreadedExecutor executor;
+    node->init();
+    executor.add_node(node);
+    executor.spin();
+    // rclcpp::spin(node);
+
+    RCLCPP_INFO(node->get_logger(), "Terminating MoveGroup");
+    rclcpp::shutdown();
+    return 0;
+}
+

src/move_group_interface/srv/MovePose.srv

@@ -0,0 +1,8 @@
+#request
+string base_frame
+geometry_msgs/PoseStamped pose
+---
+#response
+std_msgs/Header header
+geometry_msgs/Pose pose
+bool success

src/voice_move/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>voice_move</name>
+  <version>0.0.0</version>
+  <description>AR3 voice command package</description>
+  <maintainer email="dw268145@gmail.com">Daniel Wall</maintainer>
+  <license>TODO: License declaration</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/voice_move/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script-dir=$base/lib/voice_move
+[install]
+install-scripts=$base/lib/voice_move

src/voice_move/setup.py

@@ -0,0 +1,25 @@
+from setuptools import setup
+
+package_name = 'voice_move'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='Daniel Wall',
+    maintainer_email='dw268145@gmail.com',
+    description='AR3 voice command package',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'voice_listener = voice_move.voice_listener:main'        ],
+    },
+)

src/voice_move/test/test_copyright.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

src/voice_move/test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

src/voice_move/test/test_pep257.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'

src/voice_move/voice_move/resources/ar3.lm

@@ -0,0 +1,801 @@
+
+\data\
+ngram 1=217
+ngram 2=496
+ngram 3=75
+
+\1-grams:
+-0.927389	</s>
+-99	<s>	-0.2633025
+-2.554981	English	-0.1232651
+-2.276134	I	-0.1232651
+-2.554981	I'm	-0.1232651
+-1.945257	a	-0.1298478
+-2.554981	able	-0.1232652
+-2.554981	ah	-0.1232651
+-2.554981	all	-0.1232652
+-2.309698	also	-0.1232651
+-2.554981	am	-0.1232651
+-2.554981	an	-0.1232652
+-2.276134	and	-0.1232652
+-2.309698	apple	-0.1232651
+-2.554981	apricot	-0.1232651
+-2.484276	ar3	-0.1455714
+-2.136016	are	-0.1831803
+-2.554981	arm	-0.1232652
+-2.554981	arms	-0.1232651
+-2.309698	as	-0.1232651
+-2.554981	at	-0.1232652
+-2.554981	ay	-0.1232652
+-2.484276	ball	-0.1433201
+-2.484276	banana	-0.1433201
+-2.309698	be	-0.1232652
+-2.554981	below	-0.1232652
+-2.554981	best	-0.1232652
+-2.484276	block	-0.1433201
+-2.554981	bop	-0.1232651
+-2.554981	but	-0.1232652
+-2.030246	can	-0.2001237
+-2.309698	claw	-0.2527046
+-2.309698	close	-0.2051412
+-2.554981	concerns	-0.1232651
+-2.554981	conscience	-0.1232651
+-2.554981	cool	-0.1232651
+-2.484276	could	-0.2051412
+-2.554981	couple	-0.1232652
+-2.554981	cut	-0.1232651
+-2.554981	dangerous	-0.1232651
+-1.630039	degrees	-0.3655295
+-2.484276	do	-0.1232651
+-2.554981	doesn't	-0.1232651
+-2.554981	doing	-0.1232652
+-2.554981	don't	-0.1232651
+-2.030246	down	-0.1731316
+-2.309698	drop	-0.2527047
+-2.309698	eighty	-0.2051412
+-2.554981	ethical	-0.1232651
+-2.554981	expand	-0.1232651
+-2.554981	extra	-0.1232651
+-2.309698	false	-0.1232651
+-2.554981	familiar	-0.1232652
+-2.554981	fantastic	-0.1232651
+-2.554981	favorite	-0.1232651
+-2.484276	fifteen	-0.1433201
+-2.554981	fifty	-0.1232652
+-2.554981	filler	-0.1232651
+-2.554981	firmly	-0.1232652
+-2.136016	five	-0.3651688
+-2.554981	for	-0.1232651
+-2.554981	forever	-0.1232651
+-2.554981	forty	-0.1232652
+-2.554981	fourty	-0.1232651
+-2.309698	free	-0.2527046
+-2.554981	fruit	-0.1232652
+-2.554981	fruits	-0.1232652
+-2.554981	fun	-0.1232651
+-2.309698	get	-0.2051412
+-2.309698	gimme	-0.2527047
+-2.484276	give	-0.1351866
+-2.554981	go	-0.1232652
+-2.554981	gonna	-0.1232651
+-2.554981	good	-0.1232651
+-2.484276	grab	-0.1382189
+-2.309698	grasp	-0.1831803
+-2.136016	hand	-0.1731316
+-2.484276	have	-0.1232651
+-2.484276	here	-0.2051412
+-2.554981	hey	-0.1232651
+-2.554981	hundred	-0.1232652
+-2.484276	if	-0.2051412
+-2.554981	ignore	-0.1232651
+-2.554981	in	-0.1232651
+-2.554981	infamy	-0.1232651
+-2.554981	into	-0.1232652
+-2.554981	irrelevant	-0.1232651
+-2.484276	is	-0.1232651
+-1.501625	it	-0.3470554
+-2.554981	it's	-0.1232651
+-2.554981	its	-0.1232651
+-2.554981	job	-0.1232651
+-2.554981	knife	-0.1232651
+-2.554981	know	-0.1232652
+-2.554981	language	-0.1232651
+-2.554981	lanugage	-0.1232652
+-2.554981	leave	-0.1232652
+-2.484276	left	-0.1521903
+-2.309698	let	-0.2527047
+-2.554981	list	-0.1232652
+-2.554981	live	-0.1232651
+-2.554981	long	-0.1232652
+-2.554981	matter	-0.1232652
+-2.309698	maybe	-0.2527046
+-2.484276	me	-0.1831803
+-2.554981	might	-0.1232651
+-2.554981	mind	-0.1232651
+-2.554981	minimize	-0.1232652
+-2.309698	model	-0.2527046
+-2.554981	model's	-0.1232651
+-2.484276	move	-0.1232651
+-2.554981	my	-0.1232651
+-2.554981	need	-0.1232652
+-2.554981	needs	-0.1232652
+-2.309698	never	-0.2527047
+-2.554981	nice	-0.1232651
+-2.554981	nine	-0.1232651
+-2.309698	ninety	-0.2527047
+-2.309698	no	-0.1232652
+-2.554981	nothing	-0.1232652
+-2.554981	now	-0.1232652
+-2.554981	object	-0.1232651
+-2.136016	of	-0.2051412
+-2.309698	on	-0.1232652
+-2.276134	one	-0.1831803
+-2.554981	open	-0.1232652
+-2.554981	operator	-0.1232651
+-2.554981	or	-0.1232652
+-2.554981	orange	-0.1232651
+-2.554981	other	-0.1232651
+-2.309698	our	-0.1232652
+-2.554981	outside	-0.1232651
+-2.554981	pear	-0.1232651
+-2.554981	peel	-0.1232652
+-2.554981	phrase	-0.1232651
+-2.309698	phrases	-0.1232652
+-2.276134	pick	-0.2682588
+-2.276134	please	-0.1705177
+-2.554981	positive	-0.1232651
+-2.554981	positives	-0.1232651
+-2.554981	possible	-0.1232651
+-2.554981	pull	-0.1232651
+-2.554981	put	-0.1232651
+-2.554981	raise	-0.1232651
+-2.554981	rate	-0.1232652
+-2.554981	recognize	-0.1232651
+-2.554981	reduces	-0.1232651
+-2.309698	ree	-0.2527046
+-2.309698	release	-0.1232652
+-2.136016	right	-0.1564843
+-2.309698	robot	-0.1232651
+-2.554981	rotate	-0.1232652
+-2.554981	says	-0.1232652
+-2.554981	second	-0.1232651
+-2.484276	set	-0.2527046
+-2.554981	seventy	-0.1232651
+-2.554981	shoehorn	-0.1232651
+-2.554981	should	-0.1232651
+-2.554981	shouldn't	-0.1232652
+-2.554981	similar	-0.1232652
+-2.554981	six	-0.1232651
+-2.309698	sixty	-0.1232652
+-2.554981	so	-0.1232652
+-2.554981	some	-0.1232652
+-2.554981	someone	-0.1232651
+-2.484276	something	-0.2051412
+-2.554981	sound	-0.1232651
+-2.554981	spoken	-0.1232651
+-2.554981	stabbed	-0.1232651
+-2.554981	still	-0.1232651
+-2.554981	stuff	-0.1232651
+-2.309698	take	-0.2051412
+-2.554981	task	-0.1232651
+-2.309698	ten	-0.2527047
+-1.945257	that	-0.1705177
+-1.304082	the	-0.2475785
+-2.554981	these	-0.1232651
+-2.554981	things	-0.1232652
+-2.484276	thirty	-0.1831803
+-2.309698	this	-0.1232651
+-2.554981	though	-0.1232652
+-2.554981	thought	-0.1232651
+-2.484276	three	-0.2051412
+-1.669099	to	-0.1629548
+-2.554981	train	-0.1232651
+-2.554981	training	-0.1232651
+-2.554981	turn	-0.1232651
+-2.309698	twelve	-0.2527047
+-2.136016	twenty	-0.2351289
+-2.554981	twist	-0.1232651
+-2.309698	two	-0.1232651
+-2.554981	type	-0.1232651
+-2.484276	uh	-0.1232651
+-2.484276	uhh	-0.1232651
+-2.309698	um	-0.1232652
+-2.309698	umm	-0.1232651
+-2.276134	up	-0.1831803
+-2.554981	use	-0.1232652
+-2.554981	useless	-0.1232651
+-2.554981	very	-0.1232651
+-2.309698	wait	-0.2051412
+-2.554981	wake	-0.1232651
+-2.554981	want	-0.1232652
+-2.484276	we	-0.1232652
+-2.554981	we're	-0.1232651
+-2.309698	what	-0.1232652
+-2.554981	whatever	-0.1232652
+-2.554981	why	-0.1232652
+-2.554981	will	-0.1232652
+-2.484276	with	-0.1232651
+-2.554981	without	-0.1232651
+-2.554981	word	-0.1232651
+-2.030246	words	-0.3143101
+-2.484276	would	-0.1831803
+-2.554981	write	-0.1232652
+-2.484276	you	-0.1232651
+-2.309698	your	-0.2051412
+
+\2-grams:
+-2.072016	<s> I
+-2.510955	<s> I'm
+-2.510955	<s> ah
+-2.351471	<s> and
+-2.372946	<s> apple
+-2.510955	<s> apricot
+-1.193228	<s> ar3	-0.106434
+-2.255607	<s> are
+-2.163895	<s> ay	-0.1525484
+-2.474674	<s> banana
+-2.510955	<s> below
+-2.510955	<s> bop
+-1.971793	<s> can	-0.2651552
+-2.083168	<s> close
+-2.510955	<s> cool
+-2.474674	<s> could
+-2.132391	<s> do	-0.2651552
+-2.510955	<s> doesn't
+-2.096368	<s> drop	-0.1468107
+-2.510955	<s> fantastic
+-2.510955	<s> firmly
+-2.083168	<s> get
+-2.083168	<s> gimme	-0.2651552
+-2.132391	<s> give
+-2.510955	<s> good
+-2.147203	<s> grab	-0.1551153
+-2.096368	<s> grasp
+-2.01902	<s> hand
+-2.474674	<s> have
+-2.132391	<s> here	-0.2651552
+-1.414515	<s> hey	-0.1100757
+-2.474674	<s> if
+-2.510955	<s> ignore
+-1.647452	<s> it
+-2.148514	<s> leave
+-2.096368	<s> let	-0.1593523
+-2.372946	<s> maybe
+-1.114947	<s> move	-0.1288264
+-2.510955	<s> my
+-2.372946	<s> never
+-2.510955	<s> nice
+-2.372946	<s> no
+-2.163895	<s> now
+-2.255607	<s> of
+-2.372946	<s> on
+-2.163895	<s> open	-0.1526322
+-2.510955	<s> orange
+-2.510955	<s> other
+-2.510955	<s> pear
+-2.510955	<s> peel
+-2.351471	<s> pick
+-2.084877	<s> please	-0.1558526
+-2.510955	<s> pull
+-2.148514	<s> put
+-2.372946	<s> release
+-2.148514	<s> rotate
+-2.147203	<s> set	-0.1475396
+-2.510955	<s> so
+-2.510955	<s> spoken
+-2.083168	<s> take
+-1.7483	<s> that	-0.1113207
+-2.372946	<s> this
+-1.762749	<s> to
+-1.291067	<s> turn	-0.08356892
+-2.510955	<s> twist
+-2.147203	<s> uh
+-2.147203	<s> uhh
+-2.083168	<s> um
+-2.372946	<s> umm
+-2.083168	<s> wait
+-2.474674	<s> we
+-2.372946	<s> what
+-2.474674	<s> with
+-2.510955	<s> without
+-2.177124	<s> words
+-0.4735377	English </s>
+-1.288036	I am
+-1.248388	I can
+-1.288036	I don't
+-1.288036	I want
+-1.288036	I write
+-0.6034485	I'm gonna
+-1.52451	a banana
+-1.463785	a can
+-1.529851	a couple
+-1.018816	a hundred	-0.1525484
+-1.529851	a knife
+-1.503311	a one
+-1.52451	a three
+-0.5796575	able to
+-0.4735377	ah </s>
+-0.5975516	all of
+-0.8995579	also have
+-0.900838	also very
+-0.6034485	am training
+-0.6006823	an apple
+-0.8216937	and </s>
+-1.192163	and fifteen
+-1.183983	and our
+-1.183983	and phrases
+-0.6725531	apple </s>
+-0.900838	apple or
+-0.4735377	apricot </s>
+-0.8256481	ar3 </s>
+-1.64756	ar3 close
+-1.67054	ar3 could
+-1.64756	ar3 drop
+-1.64756	ar3 gimme	-0.2651552
+-1.615522	ar3 hand
+-1.153575	ar3 move
+-1.64756	ar3 never
+-1.64216	ar3 please
+-1.67054	ar3 uh
+-0.9649264	are </s>
+-1.467213	are free
+-0.9503368	are some
+-1.480979	are three
+-1.480979	are we
+-0.949087	are you	-0.2651552
+-0.5796575	arm to
+-0.6034485	arms though
+-0.900838	as it's
+-0.900838	as long
+-0.5975516	at hand
+-0.8892196	ay are
+-0.8953688	ay ree
+-0.4364524	ball </s>
+-0.4364524	banana </s>
+-1.073319	be able
+-1.073319	be dangerous
+-1.065208	be on
+-0.5975516	below are
+-0.5796575	best to
+-0.4364524	block </s>
+-0.5673129	bop it
+-0.6006823	but also
+-0.471241	can </s>
+-1.385271	can down
+-1.431117	can type
+-1.42749	can you
+-0.2948165	claw </s>
+-0.434388	close </s>
+-0.9456691	close the
+-0.4735377	concerns </s>
+-0.4735377	conscience </s>
+-0.4735377	cool </s>
+-1.068419	could also
+-0.5284965	could you
+-0.5975516	couple of
+-0.6034485	cut an
+-0.6034485	dangerous but
+-0.3024544	degrees </s>
+-1.816673	degrees down
+-1.898075	degrees left
+-1.842149	degrees right	-0.2651552
+-1.322732	degrees to	-0.284291
+-1.898075	degrees uhh
+-1.391353	degrees up	-0.1593523
+-1.041396	do a
+-1.073319	do its
+-1.071416	do with
+-0.6034485	doesn't matter
+-0.5923633	doing that
+-0.6034485	don't know
+-0.6578178	down </s>
+-1.351895	down ten	-0.2651552
+-0.7438208	down the	-0.07963848
+-0.6230923	drop it	-0.1593523
+-0.6049852	drop the	-0.2651552
+-0.8066849	eighty </s>
+-0.5104414	eighty degrees	-0.1560122
+-0.6034485	ethical concerns
+-0.54594	expand the
+-0.6034485	extra useless
+-0.900838	false positive
+-0.900838	false positives
+-0.6028026	familiar with
+-0.4735377	fantastic </s>
+-0.6034485	favorite fruit
+-0.5258819	fifteen degrees	-0.1238342
+-0.5975516	fifty five
+-0.5949497	filler words
+-0.6006823	firmly grasp
+-0.2375018	five degrees	-0.198637
+-0.54594	for the
+-0.6034485	forever live
+-0.5975516	forty five
+-0.5771604	fourty degrees
+-0.2948165	free </s>
+-0.6028026	fruit is
+-0.6028026	fruits here
+-0.4735377	fun </s>
+-1.048463	get that
+-0.4880389	get the
+-0.3289228	gimme the	-0.1087829
+-1.139672	give it
+-0.7668757	give me	-0.1459006
+-1.066564	give the
+-0.5975516	go of	-0.1121509
+-0.6034485	gonna list
+-0.6034485	good job
+-1.154732	grab that
+-0.6074318	grab the	-0.1593523
+-0.6175543	grasp it	-0.2651552
+-1.025304	grasp the
+-1.187149	grasp your
+-0.6451473	hand </s>
+-1.205297	hand it
+-0.84545	hand me	-0.1593522
+-0.9743345	have it
+-1.073319	have nothing
+-0.9217272	have the
+-0.434388	here </s>
+-1.060846	here are	-0.2651552
+-1.192163	hey ar3
+-1.17211	hey are
+-1.183983	hey ree
+-1.192163	hey three
+-0.894358	hundred and
+-0.8892196	hundred twenty	-0.2651552
+-0.4880389	if the
+-1.075176	if we're
+-0.4735377	ignore </s>
+-0.6034485	in infamy
+-0.4735377	infamy </s>
+-0.6028026	into something
+-0.6034485	irrelevant stuff
+-1.071416	is banana
+-1.073319	is familiar
+-1.073319	is why
+-0.3316526	it </s>
+-1.845925	it cut
+-1.764467	it down	-0.2651552
+-1.839229	it is
+-1.845925	it might
+-1.845925	it needs
+-1.251256	it to	-0.2651552
+-1.845925	it will
+-1.839229	it would
+-0.6034485	it's English
+-0.6034485	its best
+-0.4735377	job </s>
+-0.4735377	knife </s>
+-0.6028026	know if
+-0.6034485	language model's
+-0.6006823	lanugage model
+-0.8318802	leave it
+-0.7933542	leave the
+-0.7132193	left </s>
+-1.483456	left fifty
+-1.443241	left five
+-1.483456	left seventy
+-1.483456	left six
+-1.478893	left thirty
+-1.464195	left twelve
+-0.3538837	let go	-0.284291
+-0.5923633	list a
+-0.6034485	live in
+-0.6006823	long as
+-0.6006823	matter what
+-0.3536137	maybe we
+-0.8560517	me </s>
+-1.083867	me it
+-0.5966411	me the	-0.07963848
+-0.6034485	might raise
+-0.4735377	mind </s>
+-0.6006823	minimize false
+-0.2948165	model </s>
+-0.6034485	model's possible
+-1.535624	move a
+-1.558863	move down	-0.2651552
+-1.64417	move forty
+-1.637128	move left
+-1.614714	move ninety
+-1.609443	move one
+-1.583417	move right
+-1.614714	move ten
+-1.637128	move thirty
+-1.435073	move to
+-1.614714	move twelve
+-1.609443	move up
+-0.6034485	my favorite
+-0.6006823	need two
+-0.5796575	needs to
+-0.3538837	never mind	-0.2651552
+-0.4735377	nice </s>
+-0.5771604	nine degrees
+-0.3427056	ninety degrees
+-0.6725531	no </s>
+-0.8953688	no wait
+-0.5796575	nothing to
+-1.065208	now get
+-1.056147	now hand
+-1.065208	now release
+-0.4735377	object </s>
+-1.367327	of fruits
+-1.215895	of it	-0.1593523
+-0.7495599	of the
+-0.8236019	of this
+-0.8953688	on our
+-0.900838	on second
+-0.6501603	one eighty
+-1.194319	one fifteen
+-1.176713	one twenty
+-0.7933542	open the
+-0.8953688	open your
+-0.6034485	operator says
+-0.6028026	or something
+-0.4735377	orange </s>
+-0.5949497	other words
+-0.900838	our conscience
+-0.8953688	our robot
+-0.4735377	outside </s>
+-0.4735377	pear </s>
+-0.5923633	peel a
+-0.6034485	phrase into
+-0.6725531	phrases </s>
+-0.8791218	phrases that
+-0.3337949	pick up	-0.1094048
+-1.287056	please give
+-1.287056	please grab
+-1.275753	please pick
+-0.7479384	please set	-0.2651552
+-0.6034485	positive rate
+-0.6034485	positives for
+-0.5949497	possible words
+-0.5673129	pull it
+-0.8841414	put down
+-0.7933542	put the
+-0.6034485	raise ethical
+-0.5975516	rate of
+-0.6034485	recognize things
+-0.54594	reduces the
+-0.2948165	ree </s>
+-0.8318802	release it
+-0.7933542	release the
+-0.6809343	right </s>
+-1.428945	right fourty
+-1.409006	right one
+-1.412076	right sixty
+-1.393621	right twenty
+-1.412076	right um
+-1.073319	robot stabbed
+-1.073319	robot use
+-1.071416	robot would
+-0.8892196	rotate right
+-0.854859	rotate to
+-0.6028026	says something
+-0.6034485	second thought
+-0.6463002	set down	-0.284291
+-0.6230923	set it	-0.2651552
+-0.5771604	seventy degrees
+-0.54594	shoehorn the
+-0.6034485	should train
+-0.6028026	shouldn't have
+-0.5796575	similar to
+-0.5771604	six degrees
+-0.6725531	sixty </s>
+-0.8953688	sixty two
+-0.6006823	so maybe
+-1.073319	some filler
+-1.065208	some phrases
+-1.048708	some words
+-0.4735377	someone </s>
+-0.434388	something </s>
+-1.075176	something outside
+-0.6034485	sound similar
+-0.6034485	spoken phrase
+-0.6034485	stabbed someone
+-0.6034485	still doing
+-0.4735377	stuff </s>
+-0.9913476	take it
+-0.4880389	take the
+-0.6034485	task at
+-0.3427056	ten degrees	-0.1238342
+-0.7745703	that </s>
+-1.569851	that ball
+-1.569851	that block
+-1.024437	that can	-0.2651552
+-1.569851	that is
+-1.337385	that it
+-1.575251	that sound
+-1.569851	that would
+-1.135635	the </s>
+-2.101737	the arm
+-1.126343	the ball	-0.6364735
+-1.30877	the block	-0.284291
+-0.8878639	the can	-0.4995283
+-1.612732	the claw	-0.1593523
+-2.040598	the false
+-2.101737	the language
+-2.086708	the left	-0.06976753
+-1.595258	the model	-0.2651552
+-1.31124	the object	-0.284291
+-2.101737	the operator
+-1.980039	the right	-0.1593523
+-1.595258	the robot
+-2.101737	the task
+-2.086708	the uh
+-2.086708	the uhh
+-2.040598	the umm
+-2.101737	the wake
+-0.6034485	these extra
+-0.5923633	things that
+-0.6271824	thirty degrees
+-1.176713	thirty five
+-1.196519	thirty nine
+-1.073319	this irrelevant
+-1.073319	this lanugage
+-1.073319	this reduces
+-0.6001691	though and
+-0.5673129	thought it
+-0.434388	three </s>
+-1.068419	three sixty
+-0.9990164	to </s>
+-1.672415	to ar3
+-1.650221	to be
+-1.672415	to do
+-1.679385	to expand
+-1.672415	to me	-0.2651552
+-1.679385	to minimize
+-1.679385	to recognize
+-1.679385	to shoehorn
+-0.7547235	to the	-0.3354435
+-0.54594	train the
+-0.54594	training the
+-1.409039	turn eighty	-0.2651552
+-1.409039	turn ninety
+-1.405749	turn one
+-1.389285	turn right
+-1.422862	turn thirty
+-1.288778	turn to	-0.1593522
+-1.389285	turn twenty
+-0.3427056	twelve degrees
+-0.417608	twenty degrees	-0.05376146
+-1.270225	twenty five
+-0.5673129	twist it
+-0.900838	two arms
+-0.8501649	two degrees
+-0.6034485	type whatever
+-0.8588269	uh </s>
+-1.248388	uh can
+-1.272398	uh pick
+-1.272398	uh please
+-1.284921	uh set
+-0.8588269	uhh </s>
+-1.284921	uhh ball
+-1.274816	uhh let
+-1.284921	uhh move
+-1.272398	uhh up
+-0.766093	um </s>
+-1.041396	um a
+-1.063714	um please
+-0.6725531	umm </s>
+-0.8995579	umm block
+-0.7262802	up </s>
+-0.9291229	up a
+-1.480979	up fifteen
+-1.416486	up that
+-1.197555	up the	-0.2651552
+-1.447536	up twenty
+-0.5923633	use a
+-0.5949497	useless words
+-0.6034485	very fun
+-0.434388	wait </s>
+-1.068419	wait no
+-0.6034485	wake word
+-0.6028026	want here
+-0.8216937	we </s>
+-1.192163	we could
+-1.194677	we should
+-1.194677	we shouldn't
+-0.6034485	we're still
+-0.894358	what I
+-0.8995579	what if
+-0.6001691	whatever I
+-0.6001691	why I
+-0.6028026	will do
+-0.766093	with </s>
+-1.073319	with all
+-0.9217272	with the
+-0.6034485	without these
+-0.4735377	word </s>
+-0.2747475	words </s>
+-1.359004	words and
+-0.6501603	would be
+-1.196519	would forever
+-1.196519	would need
+-0.6006823	write as
+-1.274816	you free	-0.2651552
+-1.284921	you give
+-1.284921	you grab
+-1.272398	you pick
+-1.274816	you take
+-0.5270156	your claw	-0.2651552
+-1.060846	your hand
+
+\3-grams:
+-1.03808	<s> ar3 drop
+-1.03808	<s> ar3 gimme
+-0.9054043	<s> ar3 move
+-0.2858141	here are some
+-0.4122591	<s> ay are
+-0.06875182	the ball </s>
+-0.1735543	the block </s>
+-0.3212658	<s> can you
+-0.1935282	that can </s>
+-0.1021581	the can </s>
+-0.1814045	the claw </s>
+-0.1352608	your claw </s>
+-0.4749602	eighty degrees to
+-0.2056416	fifteen degrees </s>
+-0.3012768	five degrees </s>
+-0.7169678	five degrees right
+-0.2056416	ten degrees </s>
+-0.2543235	twenty degrees </s>
+-0.2955816	<s> do a
+-0.2393137	it down </s>
+-0.3177687	move down ten
+-0.2410243	set down the
+-0.3403704	<s> drop it
+-0.2044024	turn eighty degrees
+-0.1352608	you free </s>
+-0.1477756	<s> gimme the
+-0.1477756	ar3 gimme the
+-0.2135184	let go of
+-0.4567531	<s> grab that
+-0.2974403	<s> here are
+-0.7908074	<s> hey ar3
+-0.7849265	<s> hey are
+-0.4122591	a hundred twenty
+-0.2006717	drop it </s>
+-0.1487505	grasp it </s>
+-0.2006717	of it </s>
+-0.3313491	set it down
+-0.7528197	the left thirty
+-0.2119222	<s> let go
+-0.3313827	give me the
+-0.3164783	hand me the
+-0.2736062	to me </s>
+-0.194185	never mind </s>
+-0.1352608	the model </s>
+-1.074147	<s> move down
+-1.090619	<s> move left
+-1.076775	<s> move right
+-1.08223	<s> move up
+-0.1837565	the object </s>
+-0.5613433	go of it
+-0.4135609	<s> open your
+-0.4716026	<s> please give
+-0.2440029	degrees right </s>
+-0.3452604	the right </s>
+-0.3479612	<s> set down
+-0.2369023	please set down
+-0.1526577	down ten degrees
+-0.6073971	<s> that would
+-0.5601725	down the can
+-0.3031707	drop the ball
+-0.5532022	gimme the ball
+-0.4449862	grab the ball
+-0.5601725	me the can
+-0.3570458	to the left
+-0.9697438	to the right
+-0.2779852	up the can
+-0.2427858	degrees to the
+-0.3293	it to me
+-0.3675284	turn to the
+-0.9211988	<s> turn eighty
+-0.886919	<s> turn to
+-0.1774735	hundred twenty degrees
+-0.3592568	degrees up </s>
+-0.5654147	pick up the
+-0.313555	are you free
+
+\end\

src/voice_move/voice_move/srv/HighLvlCmds.srv

@@ -0,0 +1,6 @@
+int8 command
+int8 item
+int8 direction
+int16 degrees
+---
+bool success

src/voice_move/voice_move/voice_listener.py

@@ -0,0 +1,154 @@
+import rclpy
+from rclpy.node import Node
+from defs.srv import HighLvlCmds
+from high_lvl_cmd_srv.cmd_constants import *
+
+import random
+from enum import Enum
+from pocketsphinx import LiveSpeech
+from word2number import w2n
+
+
+
+WakeWord        = 'hey ar3'
+LanguageModel   = '/ros_root/Control-Software/src/voice_move/voice_move/resources/ar3.lm'
+Dict            = '/ros_root/Control-Software/src/voice_move/voice_move/resources/ar3.dict'
+
+ConfirmWords    = ['yes', 'confirm', 'go ahead', 'do it']
+DenyWords       = ['no', 'deny', 'stop', 'wait', 'never mind', 'ignore', 'hold on']
+
+VoiceCmdWords   = { Cmd.IGNORE.value        : ['ignore', 'never mind', 'wait'],
+                    Cmd.OPEN.value          : ['drop', 'let it', 'let go', 'open', 'release'],
+                    Cmd.CLOSE.value         : ['close', 'grab', 'grasp', 'take'],
+                    Cmd.MOVE.value          : ['move', 'shift', 'go to', 'position'],
+                    Cmd.TURN.value          : ['turn', 'rotate'],
+                    Cmd.PICKUP.value        : ['pick', 'get'],
+                    Cmd.SETDOWN.value       : ['set', 'put', 'leave'],
+                    Cmd.GIVE.value          : ['give', 'gimme', 'hand the', 'hand it', 'hand me', 'pass'] }
+
+ItemWords       = { Item.CAN.value          : ['a can', 'the can', 'that can', 'this can', 'soda can'],
+                    Item.BLOCK.value        : ['block', 'cube'],
+                    Item.BALL.value         : ['ball', 'sphere'] }
+
+DirectionWords  = { Direction.LEFT.value    : ['left', 'clockwise'],
+                    Direction.RIGHT.value   : ['right', 'counter clockwise'],
+                    Direction.UP.value      : ['up'],
+                    Direction.DOWN.value    : ['down'] }
+
+
+# Used for choosing which set of word lists to use for word parsing
+EnumType = [Cmd, Item, Direction]
+Words = [VoiceCmdWords, ItemWords, DirectionWords]
+class WordsType(Enum):
+    CMD         = 0
+    ITEM        = 1
+    DIRECTION   = 2
+    POSITION    = 3
+    DEGREES     = 4
+
+
+
+class VoiceListener(Node):
+
+    def __init__(self):
+
+        self.wakeWordSpoken = False
+
+        self.speech = LiveSpeech(
+            audio_device='plughw:CARD=Generic_1,DEV=0',
+            lm=LanguageModel,
+            dic=Dict
+        )
+
+        super().__init__('voice_move')
+
+        self.logger = self.get_logger()
+        self.logger.info('voice_listener init')
+
+        self.cli = self.create_client(HighLvlCmds, 'high_lvl_cmds')
+        while not self.cli.wait_for_service(timeout_sec=1.0):
+            self.logger.info('service not available, waiting again...')
+
+        self.req = HighLvlCmds.Request()
+
+        self._timer = self.create_timer(0.01, self._listen)
+
+
+    def _listen(self):
+
+        if self.wakeWordSpoken:
+            self.logger.info(random.choice(['Yes?', 'Go on.', 'Go ahead.']))
+        else:
+            self.logger.info('Listening...')
+
+        for phrase in self.speech:
+
+            if self.wakeWordSpoken:
+
+                phraseStr = str(phrase)
+
+                self.logger.info(f"Heard '{phraseStr}'")
+
+                command     = self.parseWords(phraseStr, WordsType.CMD)
+                item        = Item.NONE
+                direction   = Direction.NONE
+                position    = -1
+                degrees     = -1
+
+                if command == Cmd.PICKUP or command == Cmd.SETDOWN or command == Cmd.GIVE:
+                    item = self.parseWords(phraseStr, WordsType.ITEM)
+                
+                if command == Cmd.MOVE:
+                    direction = self.parseWords(phraseStr, WordsType.DIRECTION)
+                    degrees = self.parseWords(phraseStr, WordsType.DEGREES)
+
+                self.req.command        = command.value
+                self.req.item           = item.value
+                self.req.direction      = direction.value
+                self.req.position       = position
+                self.req.degrees        = degrees
+
+                self.future = self.cli.call_async(self.req)
+
+                self.wakeWordSpoken = False
+
+            else:
+
+                if WakeWord in str(phrase):
+                    # Wait for the command in the next phrase
+                    self.wakeWordSpoken = True
+
+            return
+
+    def parseWords(self, phrase, wordsType):
+
+        if wordsType == WordsType.POSITION or wordsType == WordsType.DEGREES:
+            try:
+                return w2n.word_to_num(phrase.replace('a hundred', 'one hundred'))
+            except Exception:
+                return -1
+                # raise MoveError('Failed to interpret number of degrees')
+
+        theWords = Words[wordsType.value]
+        theEnum = EnumType[wordsType.value]
+
+        for i in range(len(theWords)):
+            wordList = theWords[i]
+            for word in wordList:
+                if word in phrase:
+                    return theEnum(i)
+
+        return theEnum.NONE
+
+def main(args=None):
+
+    rclpy.init(args=args)
+    voice_listener = VoiceListener()
+    rclpy.spin(voice_listener)
+
+    voice_listener.destroy_node()
+    rclpy.shutdown
+
+
+if __name__ == '__main__':
+    main()