├── ar3_gazebo
    ├── README.md
    ├── CMakeLists.txt
    ├── launch
    │   ├── ar3_world.launch
    │   └── ar3_gazebo_bringup.launch
    ├── package.xml
    └── config
    │   └── ar3_gazebo_controllers.yaml
├── ar3_moveit_config
    ├── README.md
    ├── launch
    │   ├── ar3_moveit_sensor_manager.launch.xml
    │   ├── ar3_moveit_controller_manager.launch.xml
    │   ├── planning_pipeline.launch.xml
    │   ├── fake_moveit_controller_manager.launch.xml
    │   ├── warehouse.launch
    │   ├── setup_assistant.launch
    │   ├── joystick_control.launch
    │   ├── sensor_manager.launch.xml
    │   ├── moveit_rviz.launch
    │   ├── warehouse_settings.launch.xml
    │   ├── default_warehouse_db.launch
    │   ├── run_benchmark_ompl.launch
    │   ├── ompl_planning_pipeline.launch.xml
    │   ├── planning_context.launch
    │   ├── trajectory_execution.launch.xml
    │   ├── ar3_moveit_bringup.launch
    │   ├── demo.launch
    │   ├── move_group.launch
    │   └── moveit.rviz
    ├── config
    │   ├── kinematics.yaml
    │   ├── fake_controllers.yaml
    │   ├── controllers.yaml
    │   ├── fake_joint_offsets.yaml
    │   ├── joint_limits.yaml
    │   ├── ar3.srdf
    │   └── ompl_planning.yaml
    ├── CMakeLists.txt
    └── package.xml
├── ar3_description
    ├── meshes
    │   ├── link_1.STL
    │   ├── link_2.STL
    │   ├── link_3.STL
    │   ├── link_4.STL
    │   ├── link_5.STL
    │   ├── link_6.STL
    │   └── base_link.STL
    ├── README.md
    ├── CMakeLists.txt
    ├── package.xml
    └── urdf
    │   ├── ar3.urdf.xacro
    │   ├── ar3.urdf
    │   └── ar3_gazebo.urdf.xacro
├── ar3_control
    ├── launch
    │   └── move_group_demo.launch
    ├── README.md
    ├── CMakeLists.txt
    ├── package.xml
    └── src
    │   └── move_group_demo.cpp
├── ar3_hardware_interface
    ├── config
    │   ├── hardware_interface.yaml
    │   ├── joint_offsets.yaml
    │   ├── hardware_driver.yaml
    │   └── controllers.yaml
    ├── src
    │   ├── ar3_hardware_interface_node.cpp
    │   └── ar3_hardware_interface.cpp
    ├── README.md
    ├── package.xml
    ├── launch
    │   └── ar3_hardware_bringup.launch
    ├── CMakeLists.txt
    └── include
    │   └── ar3_hardware_interface
    │       └── ar3_hardware_interface.h
├── ar3_hardware_drivers
    ├── package.xml
    ├── README.md
    ├── include
    │   └── ar3_hardware_drivers
    │   │   └── TeensyDriver.h
    ├── CMakeLists.txt
    └── src
    │   └── TeensyDriver.cpp
├── ar3_microcontrollers
    ├── README.md
    └── teensy
    │   └── baseline_dev
    │       └── baseline_dev.ino
├── LICENSE
├── .gitignore
├── CMakeLists.txt
└── README.md


/ar3_gazebo/README.md:
--------------------------------------------------------------------------------
1 | # ar3_gazebo
2 | Gazebo simulator package for the AR3.


--------------------------------------------------------------------------------
/ar3_moveit_config/README.md:
--------------------------------------------------------------------------------
1 | # ar3_moveit_config
2 | MoveIt package for the AR3.


--------------------------------------------------------------------------------
/ar3_moveit_config/launch/ar3_moveit_sensor_manager.launch.xml:
--------------------------------------------------------------------------------
1 | <launch>
2 | 
3 | </launch>
4 | 


--------------------------------------------------------------------------------
/ar3_description/meshes/link_1.STL:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/ongdexter/ar3_core/HEAD/ar3_description/meshes/link_1.STL


--------------------------------------------------------------------------------
/ar3_description/meshes/link_2.STL:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/ongdexter/ar3_core/HEAD/ar3_description/meshes/link_2.STL


--------------------------------------------------------------------------------
/ar3_description/meshes/link_3.STL:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/ongdexter/ar3_core/HEAD/ar3_description/meshes/link_3.STL


--------------------------------------------------------------------------------
/ar3_description/meshes/link_4.STL:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/ongdexter/ar3_core/HEAD/ar3_description/meshes/link_4.STL


--------------------------------------------------------------------------------
/ar3_description/meshes/link_5.STL:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/ongdexter/ar3_core/HEAD/ar3_description/meshes/link_5.STL


--------------------------------------------------------------------------------
/ar3_description/meshes/link_6.STL:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/ongdexter/ar3_core/HEAD/ar3_description/meshes/link_6.STL


--------------------------------------------------------------------------------
/ar3_description/meshes/base_link.STL:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/ongdexter/ar3_core/HEAD/ar3_description/meshes/base_link.STL


--------------------------------------------------------------------------------
/ar3_gazebo/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 2.8.3)
2 | project(ar3_gazebo)
3 | find_package(catkin REQUIRED)
4 | catkin_package()
5 | 


--------------------------------------------------------------------------------
/ar3_control/launch/move_group_demo.launch:
--------------------------------------------------------------------------------
1 | <launch>
2 |   <node name="move_group_demo_node" pkg="ar3_control" type="move_group_demo" respawn="false" output="screen"></node>
3 | </launch>
4 | 


--------------------------------------------------------------------------------
/ar3_moveit_config/config/kinematics.yaml:
--------------------------------------------------------------------------------
1 | manipulator:
2 |   kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
3 |   kinematics_solver_search_resolution: 0.005
4 |   kinematics_solver_timeout: 0.005
5 | 


--------------------------------------------------------------------------------
/ar3_moveit_config/config/fake_controllers.yaml:
--------------------------------------------------------------------------------
 1 | controller_list:
 2 |   - name: fake_arm_controller
 3 |     joints:
 4 |       - joint_1
 5 |       - joint_2
 6 |       - joint_3
 7 |       - joint_4
 8 |       - joint_5
 9 |       - joint_6
10 | 


--------------------------------------------------------------------------------
/ar3_hardware_interface/config/hardware_interface.yaml:
--------------------------------------------------------------------------------
 1 | ar3:
 2 |     hardware_interface:
 3 |         loop_hz: 10 # hz
 4 |         joints:
 5 |             - joint_1
 6 |             - joint_2
 7 |             - joint_3
 8 |             - joint_4
 9 |             - joint_5
10 |             - joint_6


--------------------------------------------------------------------------------
/ar3_description/README.md:
--------------------------------------------------------------------------------
1 | # ar3_description
2 | This module contains the hardware description for the AR3. The URDFs were modified from the original ones. The main modifications were to fix some of the joint rotation directions and to specify revolute joints. Note that there is a seperate URDF for simulation which contains the Gazebo plugins.


--------------------------------------------------------------------------------
/ar3_moveit_config/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 2.8.3)
2 | project(ar3_moveit_config)
3 | find_package(catkin REQUIRED)
4 | catkin_package()
5 | 
6 | install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
7 |   PATTERN "setup_assistant.launch" EXCLUDE)
8 | install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
9 | 


--------------------------------------------------------------------------------
/ar3_description/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | 
 3 | project(ar3_description)
 4 | 
 5 | find_package(catkin REQUIRED)
 6 | 
 7 | catkin_package()
 8 | 
 9 | find_package(roslaunch)
10 | 
11 | foreach(dir config launch meshes urdf)
12 | 	install(DIRECTORY ${dir}/
13 | 		DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
14 | endforeach(dir)
15 | 


--------------------------------------------------------------------------------
/ar3_moveit_config/config/controllers.yaml:
--------------------------------------------------------------------------------
 1 | controller_list:
 2 |     - name: "/ar3/controllers/position"
 3 |       action_ns: follow_joint_trajectory
 4 |       type: FollowJointTrajectory
 5 |       default: true
 6 |       joints:
 7 |           - joint_1
 8 |           - joint_2
 9 |           - joint_3
10 |           - joint_4
11 |           - joint_5
12 |           - joint_6
13 | 


--------------------------------------------------------------------------------
/ar3_moveit_config/launch/ar3_moveit_controller_manager.launch.xml:
--------------------------------------------------------------------------------
1 | <launch>
2 |     <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
3 | 
4 |     <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)" />
5 | 
6 |     <rosparam file="$(find ar3_moveit_config)/config/controllers.yaml" />
7 | </launch>
8 | 


--------------------------------------------------------------------------------
/ar3_hardware_interface/config/joint_offsets.yaml:
--------------------------------------------------------------------------------
 1 | ar3:
 2 |     hardware_interface:
 3 |         joint_offsets: # degrees
 4 |             joint_1: -170.0 # default -170.0
 5 |             joint_2: -39.6 # default -39.6
 6 |             joint_3: 0.0 # default 0.0
 7 |             joint_4: -164.5 # default -164.5
 8 |             joint_5: -104.5 # default -104.15
 9 |             joint_6: -148.1 # default -148.1
10 | 


--------------------------------------------------------------------------------
/ar3_moveit_config/launch/planning_pipeline.launch.xml:
--------------------------------------------------------------------------------
 1 | <launch>
 2 | 
 3 |   <!-- This file makes it easy to include different planning pipelines; 
 4 |        It is assumed that all planning pipelines are named XXX_planning_pipeline.launch  -->  
 5 | 
 6 |   <arg name="pipeline" default="ompl" />
 7 | 
 8 |   <include file="$(find ar3_moveit_config)/launch/$(arg pipeline)_planning_pipeline.launch.xml" />
 9 | 
10 | </launch>
11 | 


--------------------------------------------------------------------------------
/ar3_gazebo/launch/ar3_world.launch:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <launch>
 3 |     <!-- Gazebo world -->
 4 |     <include file="$(find gazebo_ros)/launch/empty_world.launch" />
 5 |     
 6 |     <!-- Gazebo model - add a delay to let the controller start properly -->
 7 |     <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" 
 8 |         args="-param robot_description -urdf -model ar3" respawn="false" output="screen"/>        
 9 | </launch>
10 | 


--------------------------------------------------------------------------------
/ar3_moveit_config/launch/fake_moveit_controller_manager.launch.xml:
--------------------------------------------------------------------------------
 1 | <launch>
 2 | 
 3 |   <!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
 4 |   <param name="moveit_controller_manager" value="moveit_fake_controller_manager/MoveItFakeControllerManager"/>
 5 | 
 6 |   <!-- The rest of the params are specific to this plugin -->
 7 |   <rosparam file="$(find ar3_moveit_config)/config/fake_controllers.yaml"/>
 8 | 
 9 | </launch>
10 | 


--------------------------------------------------------------------------------
/ar3_hardware_interface/src/ar3_hardware_interface_node.cpp:
--------------------------------------------------------------------------------
 1 | #include <ros/callback_queue.h>
 2 | #include <ar3_hardware_interface/ar3_hardware_interface.h>
 3 | 
 4 | int main(int argc, char** argv)
 5 | {
 6 |   ros::init(argc, argv, "ar3_hardware_interface");
 7 | 	ros::CallbackQueue ros_queue;
 8 | 
 9 |   ros::NodeHandle nh;
10 | 	nh.setCallbackQueue(&ros_queue);
11 |   ar3_hardware_interface::AR3HardwareInterface ar3(nh);
12 | 
13 | 	ros::MultiThreadedSpinner spinner(0);
14 | 	spinner.spin(&ros_queue);
15 | 
16 |   return 0;
17 | }
18 | 


--------------------------------------------------------------------------------
/ar3_hardware_drivers/package.xml:
--------------------------------------------------------------------------------
 1 | <package format="3">
 2 |   <name>ar3_hardware_drivers</name>
 3 |   <version>0.0.0</version>
 4 |   <description>The ar3_hardware_drivers package</description>
 5 | 
 6 |   <maintainer email="dexterong94@gmail.com">Dexter Ong</maintainer>
 7 | 
 8 |   <license>MIT</license>
 9 | 
10 |   <author email="dexterong94@gmail.com">Dexter Ong</author>
11 | 
12 |   <buildtool_depend>catkin</buildtool_depend>
13 | 
14 |   <depend>roscpp</depend>
15 |   <depend>rospy</depend>
16 |   <depend>std_msgs</depend>
17 |   
18 | </package>
19 | 


--------------------------------------------------------------------------------
/ar3_moveit_config/launch/warehouse.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |   
 3 |   <!-- The path to the database must be specified -->
 4 |   <arg name="moveit_warehouse_database_path" />
 5 | 
 6 |   <!-- Load warehouse parameters -->  
 7 |   <include file="$(find ar3_moveit_config)/launch/warehouse_settings.launch.xml" />
 8 | 
 9 |   <!-- Run the DB server -->
10 |   <node name="$(anon mongo_wrapper_ros)" cwd="ROS_HOME" type="mongo_wrapper_ros.py" pkg="warehouse_ros_mongo">
11 |     <param name="overwrite" value="false"/>
12 |     <param name="database_path" value="$(arg moveit_warehouse_database_path)" />
13 |   </node>
14 | 
15 | </launch>
16 | 


--------------------------------------------------------------------------------
/ar3_moveit_config/launch/setup_assistant.launch:
--------------------------------------------------------------------------------
 1 | <!-- Re-launch the MoveIt Setup Assistant with this configuration package already loaded -->
 2 | <launch>
 3 | 
 4 |   <!-- Debug Info -->
 5 |   <arg name="debug" default="false" />
 6 |   <arg unless="$(arg debug)" name="launch_prefix" value="" />
 7 |   <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
 8 | 
 9 |   <!-- Run -->
10 |   <node pkg="moveit_setup_assistant" type="moveit_setup_assistant" name="moveit_setup_assistant" 
11 | 	args="--config_pkg=ar3_moveit_config"
12 | 	launch-prefix="$(arg launch_prefix)"
13 |         output="screen" />
14 | 
15 | </launch>
16 | 


--------------------------------------------------------------------------------
/ar3_hardware_interface/config/hardware_driver.yaml:
--------------------------------------------------------------------------------
 1 | ar3:
 2 |     hardware_driver:
 3 |         serial_port: "COM4" # /dev/ttyACM0
 4 |         baudrate: 115200
 5 |         encoder_steps_per_deg: # encoder steps per joint angle degree
 6 |             joint_1: 227.5555555555556 # default 227.5555555555556
 7 |             joint_2: 284.4444444444444 # default 284.4444444444444 # joint enc 22.75555555555556
 8 |             joint_3: 284.4444444444444 # default 284.4444444444444
 9 |             joint_4: 223.0044444444444 # default 223.0044444444444
10 |             joint_5: 56.04224675948152 # default 56.04224675948152 # joint enc 22.75555555555556
11 |             joint_6: 108.0888888888889 # default 108.0888888888889
12 | 


--------------------------------------------------------------------------------
/ar3_moveit_config/launch/joystick_control.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |   <!-- See moveit_ros/visualization/doc/joystick.rst for documentation -->
 3 | 
 4 |   <arg name="dev" default="/dev/input/js0" />
 5 | 
 6 |   <!-- Launch joy node -->
 7 |   <node pkg="joy" type="joy_node" name="joy">
 8 |     <param name="dev" value="$(arg dev)" /> <!-- Customize this to match the location your joystick is plugged in on-->
 9 |     <param name="deadzone" value="0.2" />
10 |     <param name="autorepeat_rate" value="40" />
11 |     <param name="coalesce_interval" value="0.025" />
12 |   </node>
13 | 
14 |   <!-- Launch python interface -->
15 |   <node pkg="moveit_ros_visualization" type="moveit_joy.py" output="screen" name="moveit_joy"/>
16 |         
17 | </launch>
18 | 


--------------------------------------------------------------------------------
/ar3_moveit_config/launch/sensor_manager.launch.xml:
--------------------------------------------------------------------------------
 1 | <launch>
 2 | 
 3 |   <!-- This file makes it easy to include the settings for sensor managers -->  
 4 | 
 5 |   <!-- Params for the octomap monitor -->
 6 |   <!--  <param name="octomap_frame" type="string" value="some frame in which the robot moves" /> -->
 7 |   <param name="octomap_resolution" type="double" value="0.025" />
 8 |   <param name="max_range" type="double" value="5.0" />
 9 | 
10 |   <!-- Load the robot specific sensor manager; this sets the moveit_sensor_manager ROS parameter -->
11 |   <arg name="moveit_sensor_manager" default="ar3" />
12 |   <include file="$(find ar3_moveit_config)/launch/$(arg moveit_sensor_manager)_moveit_sensor_manager.launch.xml" />
13 |   
14 | </launch>
15 | 


--------------------------------------------------------------------------------
/ar3_description/package.xml:
--------------------------------------------------------------------------------
 1 | <package>
 2 |   <name>ar3_description</name>
 3 |   <version>1.0.0</version>
 4 |   <description>
 5 |     <p>URDF Description package for ar3</p>
 6 |     <p>This package contains configuration data, 3D models and launch files
 7 | for ar3 robot</p>
 8 |   </description>
 9 |   <author>me</author>
10 |   <maintainer email="chris.annin@gmail.com" />
11 |   <license>BSD</license>
12 |   <buildtool_depend>catkin</buildtool_depend>
13 |   <build_depend>roslaunch</build_depend>
14 |   <run_depend>robot_state_publisher</run_depend>
15 |   <run_depend>rviz</run_depend>
16 |   <run_depend>joint_state_publisher</run_depend>
17 |   <run_depend>gazebo</run_depend>
18 |   <export>
19 |     <architecture_independent />
20 |   </export>
21 | </package>


--------------------------------------------------------------------------------
/ar3_moveit_config/launch/moveit_rviz.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 | 
 3 |   <arg name="debug" default="false" />
 4 |   <arg unless="$(arg debug)" name="launch_prefix" value="" />
 5 |   <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
 6 | 
 7 |   <arg name="config" default="false" />
 8 |   <arg unless="$(arg config)" name="command_args" value="" />
 9 |   <arg     if="$(arg config)" name="command_args" value="-d $(find ar3_moveit_config)/launch/moveit.rviz" />
10 |   
11 |   <node name="$(anon rviz)" launch-prefix="$(arg launch_prefix)" pkg="rviz" type="rviz" respawn="false"
12 | 	args="$(arg command_args)" output="screen">
13 |     <rosparam command="load" file="$(find ar3_moveit_config)/config/kinematics.yaml"/>
14 |   </node>
15 | 
16 | </launch>
17 | 


--------------------------------------------------------------------------------
/ar3_moveit_config/launch/warehouse_settings.launch.xml:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |   <!-- Set the parameters for the warehouse and run the mongodb server. -->
 3 | 
 4 |   <!-- The default DB port for moveit (not default MongoDB port to avoid potential conflicts) -->  
 5 |   <arg name="moveit_warehouse_port" default="33829" /> 
 6 | 
 7 |   <!-- The default DB host for moveit -->
 8 |   <arg name="moveit_warehouse_host" default="localhost" /> 
 9 |   
10 |   <!-- Set parameters for the warehouse -->
11 |   <param name="warehouse_port" value="$(arg moveit_warehouse_port)"/>
12 |   <param name="warehouse_host" value="$(arg moveit_warehouse_host)"/>
13 |   <param name="warehouse_exec" value="mongod" />
14 |   <param name="warehouse_plugin" value="warehouse_ros_mongo::MongoDatabaseConnection" />
15 | 
16 | </launch>
17 | 


--------------------------------------------------------------------------------
/ar3_moveit_config/launch/default_warehouse_db.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 | 
 3 |   <arg name="reset" default="false"/>
 4 |   <!-- If not specified, we'll use a default database location -->
 5 |   <arg name="moveit_warehouse_database_path" default="$(find ar3_moveit_config)/default_warehouse_mongo_db" />
 6 | 
 7 |   <!-- Launch the warehouse with the configured database location -->
 8 |   <include file="$(find ar3_moveit_config)/launch/warehouse.launch">
 9 |     <arg name="moveit_warehouse_database_path" value="$(arg moveit_warehouse_database_path)" />
10 |   </include>
11 | 
12 |   <!-- If we want to reset the database, run this node -->
13 |   <node if="$(arg reset)" name="$(anon moveit_default_db_reset)" type="moveit_init_demo_warehouse" pkg="moveit_ros_warehouse" respawn="false" output="screen" />
14 | 
15 | </launch>
16 | 


--------------------------------------------------------------------------------
/ar3_gazebo/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package format="2">
 3 |   <name>ar3_gazebo</name>
 4 |   <version>2.2.0</version>
 5 |   <description>Gazebo simulation for Niryo One</description>
 6 |   <author>Edouard Renard</author>
 7 |   <maintainer email="e.rey-coquais@niryo.com">Etienne Rey-Coquais</maintainer>
 8 |   <license>GPLv3</license>
 9 | 
10 |   <buildtool_depend>catkin</buildtool_depend>
11 |   <exec_depend>controller_manager</exec_depend>
12 |   <exec_depend>joint_state_controller</exec_depend>
13 |   <exec_depend>robot_state_publisher</exec_depend>
14 |   <exec_depend>effort_controllers</exec_depend>
15 |   <exec_depend>gazebo_ros</exec_depend>
16 |   <exec_depend>gazebo_ros_control</exec_depend>
17 |   <exec_depend>ar3_description</exec_depend>
18 | 
19 |   <export>
20 |   </export>
21 | </package>
22 | 


--------------------------------------------------------------------------------
/ar3_microcontrollers/README.md:
--------------------------------------------------------------------------------
 1 | # ar3_microcontrollers
 2 | This module contains the microcontroller firmware for the AR3. 
 3 | 
 4 | ## Teensy
 5 | There are two Arduino sketches provided for the Teensy 3.5.  
 6 | 
 7 | * `baseline_with_ARCS` is fully compatible with the default configurations of the AR3 and also contains the original code for ARCS. You will be able to use both software without needing to reflash the Teensy. You cannot use both simultaneously, however. There will only be minor changes to this for fixing bugs and such.
 8 | 
 9 | * `baseline_dev` has been cleaned up for development and does not support ARCS. There will be documented changes that you can make which are adapted for my hardware, which are currently switching J2 and J5 to joint position encoders and switching the J2 motor to 4000 steps/rev. I will continue developing on this version to add more features.
10 | 


--------------------------------------------------------------------------------
/ar3_hardware_drivers/README.md:
--------------------------------------------------------------------------------
1 | # ar3_hardware_drivers
2 | This module contains the hardware drivers for the AR3. 
3 | 
4 | ## Teensy Driver
5 | This driver serves as a bridge between the higher level software handled by the hardware_interface and the Teensy. Since the original AR3 design only has a single motor controller in the form of the Teensy, which then manages the individual stepper drivers, I've decided to keep all communication synchronous to avoid complications with thread safety. In an effort to keep this work accessible to the general user, the message protocol also follows the standard used for ARCS.
6 | 
7 | The config file `hardware_driver.yaml` can be found in the hardware_interface configs where it is loaded to the parameter server. You will need to define the port and baudrate settings for the Teensy in that config file. More details are provided in `ar3_hardware_interface`.


--------------------------------------------------------------------------------
/ar3_hardware_interface/config/controllers.yaml:
--------------------------------------------------------------------------------
 1 | ar3:
 2 |   controllers:
 3 |     state:
 4 |       type: joint_state_controller/JointStateController
 5 |       publish_rate: 50
 6 |     position:
 7 |       type: position_controllers/JointTrajectoryController
 8 |       joints:
 9 |         - joint_1
10 |         - joint_2
11 |         - joint_3
12 |         - joint_4
13 |         - joint_5
14 |         - joint_6
15 |       constraints:
16 |         goal_time: 0.5
17 |         joint_1:
18 |           trajectory: 0.5
19 |           goal: 0.02
20 |         joint_2:
21 |           trajectory: 0.5
22 |           goal: 0.02
23 |         joint_3:
24 |           trajectory: 0.5
25 |           goal: 0.02
26 |         joint_4:
27 |           trajectory: 0.5
28 |           goal: 0.02
29 |         joint_5:
30 |           trajectory: 0.5
31 |           goal: 0.02
32 |         joint_6:
33 |           trajectory: 0.5
34 |           goal: 0.02
35 | 


--------------------------------------------------------------------------------
/ar3_gazebo/config/ar3_gazebo_controllers.yaml:
--------------------------------------------------------------------------------
 1 | # Publish all joint states
 2 | joint_state_controller:
 3 |     type: "joint_state_controller/JointStateController"
 4 |     publish_rate: 50
 5 |     
 6 | # Joint Trajectory Controller
 7 | /ar3/controllers/position:
 8 |     type: "position_controllers/JointTrajectoryController"
 9 |     joints: 
10 |         - joint_1  
11 |         - joint_2
12 |         - joint_3  
13 |         - joint_4  
14 |         - joint_5  
15 |         - joint_6
16 |     constraints:
17 |         goal_time: 0.5
18 |         joint_1:
19 |             trajectory: 0.5
20 |             goal: 0.02
21 |         joint_2:
22 |             trajectory: 0.5
23 |             goal: 0.02
24 |         joint_3:
25 |             trajectory: 0.5
26 |             goal: 0.02
27 |         joint_4:
28 |             trajectory: 0.5
29 |             goal: 0.02
30 |         joint_5:
31 |             trajectory: 0.5
32 |             goal: 0.02
33 |         joint_6:
34 |             trajectory: 0.5
35 |             goal: 0.02
36 | 
37 | 


--------------------------------------------------------------------------------
/ar3_moveit_config/launch/run_benchmark_ompl.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 | 
 3 |   <!-- This argument must specify the list of .cfg files to process for benchmarking -->
 4 |   <arg name="cfg" />
 5 | 
 6 |   <!-- Load URDF -->
 7 |   <include file="$(find ar3_moveit_config)/launch/planning_context.launch">
 8 |     <arg name="load_robot_description" value="true"/>
 9 |   </include>
10 | 
11 |   <!-- Start the database -->
12 |   <include file="$(find ar3_moveit_config)/launch/warehouse.launch">
13 |     <arg name="moveit_warehouse_database_path" value="moveit_ompl_benchmark_warehouse"/>
14 |   </include>  
15 | 
16 |   <!-- Start Benchmark Executable -->
17 |   <node name="$(anon moveit_benchmark)" pkg="moveit_ros_benchmarks" type="moveit_run_benchmark" args="$(arg cfg) --benchmark-planners" respawn="false" output="screen">
18 |     <rosparam command="load" file="$(find ar3_moveit_config)/config/kinematics.yaml"/>
19 |     <rosparam command="load" file="$(find ar3_moveit_config)/config/ompl_planning.yaml"/>
20 |   </node>
21 | 
22 | </launch>
23 | 


--------------------------------------------------------------------------------
/ar3_gazebo/launch/ar3_gazebo_bringup.launch:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <launch>
 3 | 
 4 |     <!-- URDF -->
 5 |     <param name="robot_description" command="$(find xacro)/xacro --inorder $(find ar3_description)/urdf/ar3_gazebo.urdf.xacro" />
 6 | 
 7 |     <!-- Load joint controller configurations from YAML file to parameter server -->
 8 |     <rosparam file="$(find ar3_gazebo)/config/ar3_gazebo_controllers.yaml" command="load"/>
 9 | 
10 |     <!-- Load controllers-->
11 |     <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"
12 |         args="joint_state_controller /ar3/controllers/position
13 |         --shutdown-timeout 1"/>
14 | 
15 |     <!-- Convert joint states to TF transforms for rviz, etc -->
16 |     <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
17 |         respawn="false" output="screen">
18 |     </node>
19 | 
20 |     <!-- Start gazebo model and world -->
21 |     <include file="$(find ar3_gazebo)/launch/ar3_world.launch" />
22 | </launch>


--------------------------------------------------------------------------------
/ar3_moveit_config/launch/ompl_planning_pipeline.launch.xml:
--------------------------------------------------------------------------------
 1 | <launch>
 2 | 
 3 |   <!-- OMPL Plugin for MoveIt! -->
 4 |   <arg name="planning_plugin" value="ompl_interface/OMPLPlanner" />
 5 | 
 6 |   <!-- The request adapters (plugins) used when planning with OMPL. 
 7 |        ORDER MATTERS -->
 8 |   <arg name="planning_adapters" value="default_planner_request_adapters/AddTimeParameterization
 9 | 				       default_planner_request_adapters/FixWorkspaceBounds
10 | 				       default_planner_request_adapters/FixStartStateBounds
11 | 				       default_planner_request_adapters/FixStartStateCollision
12 | 				       default_planner_request_adapters/FixStartStatePathConstraints" />
13 | 
14 |   <arg name="start_state_max_bounds_error" value="0.1" />
15 | 
16 |   <param name="planning_plugin" value="$(arg planning_plugin)" />
17 |   <param name="request_adapters" value="$(arg planning_adapters)" />
18 |   <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
19 | 
20 |   <rosparam command="load" file="$(find ar3_moveit_config)/config/ompl_planning.yaml"/>
21 | 
22 | </launch>
23 | 


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | MIT License
 2 | 
 3 | Copyright (c) 2021 Dexter Ong
 4 | 
 5 | Permission is hereby granted, free of charge, to any person obtaining a copy
 6 | of this software and associated documentation files (the "Software"), to deal
 7 | in the Software without restriction, including without limitation the rights
 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 | 
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 | 
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 | 


--------------------------------------------------------------------------------
/ar3_hardware_interface/README.md:
--------------------------------------------------------------------------------
1 | # ar3_hardware_interface
2 | This module contains the hardware interface for the AR3. 
3 | 
4 | ## Position Controller
5 | The hardware_interface implements a Position Joint Trajectory Controller from the [ros_control](http://wiki.ros.org/ros_control) framework. It serves as an interface between the higher level planning (MoveIt) and the actuator driver (Teensy driver).
6 | 
7 | ## Joint Encoder Calibration
8 | You can skip the automatic encoder calibration sequence with the parameter `use_existing_calibrations:=true`. Since the original encoders (AMT-102V) are relative encoders, they need to be calibrated against the limit switches when powered on. If you are restarting the software but not the Teensy, the encoders will have remained powered on and do not need to be recalibrated. Alternatively, you can change the REST_ENC_POSITIONS values in the Teensy sketch so that the encoders are initialised to those values on startup, provided that your arm is also always initialised in that position as well. Before starting any movements, it is always good to check in RViz that the model is in a sensible position to verify that the encoders are properly calibrated.
9 | 


--------------------------------------------------------------------------------
/ar3_moveit_config/config/fake_joint_offsets.yaml:
--------------------------------------------------------------------------------
 1 | ar3:
 2 |   joint_offsets:
 3 |     JointRightShoulderPan: 0.0
 4 |     JointRightShoulderTilt: 0.0
 5 |     JointRightUpperArmRoll: 0.0
 6 |     JointRightElbowFlex: 0.0
 7 |     JointRightForearmRoll: 0.0
 8 |     JointRightWristFlex: 0.0
 9 |     JointRightWristRoll: 0.0
10 |     JointRightGripper: 0.0
11 |     JointLeftShoulderPan: 0.0
12 |     JointLeftShoulderTilt: 0.0
13 |     JointLeftUpperArmRoll: 0.0
14 |     JointLeftElbowFlex: 0.0
15 |     JointLeftForearmRoll: 0.0
16 |     JointLeftWristFlex: 0.0
17 |     JointLeftWristRoll: 0.0
18 |     JointLeftGripper: 0.0
19 |   joint_read_ratio:
20 |     JointRightShoulderPan: 1.0
21 |     JointRightShoulderTilt: 1.0
22 |     JointRightUpperArmRoll: 1.0
23 |     JointRightElbowFlex: 1.0
24 |     JointRightForearmRoll: 1.0
25 |     JointRightWristFlex: 1.0
26 |     JointRightWristRoll: 1.0
27 |     JointRightGripper: 1.0
28 |     JointLeftShoulderPan: 1.0
29 |     JointLeftShoulderTilt: 1.0
30 |     JointLeftUpperArmRoll: 1.0
31 |     JointLeftElbowFlex: 1.0
32 |     JointLeftForearmRoll: 1.0
33 |     JointLeftWristFlex: 1.0
34 |     JointLeftWristRoll: 1.0
35 |     JointLeftGripper: 1.0
36 | 


--------------------------------------------------------------------------------
/.gitignore:
--------------------------------------------------------------------------------
 1 | # Prerequisites
 2 | *.d
 3 | 
 4 | # Compiled Object files
 5 | *.slo
 6 | *.lo
 7 | *.o
 8 | *.obj
 9 | 
10 | # Precompiled Headers
11 | *.gch
12 | *.pch
13 | 
14 | # Compiled Dynamic libraries
15 | *.so
16 | *.dylib
17 | *.dll
18 | 
19 | # Fortran module files
20 | *.mod
21 | *.smod
22 | 
23 | # Compiled Static libraries
24 | *.lai
25 | *.la
26 | *.a
27 | *.lib
28 | 
29 | # Executables
30 | *.exe
31 | *.out
32 | *.app
33 | 
34 | ## Ignore Visual Studio temporary files, build results, and
35 | ## files generated by popular Visual Studio add-ons.
36 | 
37 | # User-specific files
38 | *.suo
39 | *.user
40 | *.userosscache
41 | *.sln.docstates
42 | 
43 | # User-specific files (MonoDevelop/Xamarin Studio)
44 | *.userprefs
45 | 
46 | # Build results
47 | [Dd]ebug/
48 | [Dd]ebugPublic/
49 | [Rr]elease/
50 | [Rr]eleases/
51 | x64/
52 | x86/
53 | bld/
54 | [Bb]in/
55 | [Oo]bj/
56 | [Ll]og/
57 | 
58 | # Visual Studio 2015 cache/options directory
59 | .vs/
60 | # Uncomment if you have tasks that create the project's static files in wwwroot
61 | #wwwroot/
62 | 
63 | # MSTest test Results
64 | [Tt]est[Rr]esult*/
65 | [Bb]uild[Ll]og.*
66 | 
67 | # NUNIT
68 | *.VisualState.xml
69 | TestResult.xml


--------------------------------------------------------------------------------
/ar3_moveit_config/package.xml:
--------------------------------------------------------------------------------
 1 | <package>
 2 |   <name>ar3_moveit_config</name>
 3 |   <version>2.2.0</version>
 4 |   <description>
 5 |      An automatically generated package with all the configuration and launch files for using the ar3 with the MoveIt! Motion Planning Framework
 6 |   </description>
 7 |   <author>Edouard Renard</author>
 8 |   <maintainer email="e.rey-coquais@niryo.com">Etienne Rey-Coquais</maintainer>
 9 | 
10 |   <license>GPLv3</license>
11 | 
12 |   <url type="website">http://moveit.ros.org/</url>
13 |   <url type="bugtracker">https://github.com/ros-planning/moveit/issues</url>
14 |   <url type="repository">https://github.com/ros-planning/moveit</url>
15 | 
16 |   <buildtool_depend>catkin</buildtool_depend>
17 | 
18 |   <run_depend>moveit_ros_move_group</run_depend>
19 |   <run_depend>moveit_kinematics</run_depend>
20 |   <run_depend>moveit_planners_ompl</run_depend>
21 |   <run_depend>moveit_ros_visualization</run_depend>
22 |   <run_depend>joint_state_publisher</run_depend>
23 |   <run_depend>robot_state_publisher</run_depend>
24 |   <run_depend>xacro</run_depend>
25 |   <build_depend>ar3_description</build_depend>
26 |   <run_depend>ar3_description</run_depend>
27 | 
28 | </package>
29 | 


--------------------------------------------------------------------------------
/ar3_control/README.md:
--------------------------------------------------------------------------------
 1 | # ar3_control
 2 | This module contains sample code for motion planning with the MoveIt API. There is currently a demo for the move group interface which has been adapted for the AR3 from the tutorials. There is much more to the MoveIt API, do check out the [official MoveIt tutorials](https://ros-planning.github.io/moveit_tutorials/index.html).
 3 | 
 4 | ## Move Group Interface Demo
 5 | This demo is a simplified version of the official tutorial and covers various ways of specifying and planning for a goal - namely pose goals, joint-space goals, Cartesian paths and planning with path constraints. This demo has modified goals that are valid for the AR3 so that you can try it out with the simulator or your own arm. This should be sufficient to get started with basic tasks. The official tutorial also covers adding objects to the environment, which I've chosen to leave out, so you are encouraged to go through that as well.
 6 | 
 7 | * **rviz_tools_gui**  
 8 |   You will need to add the rviz_tools_gui to step through the demo. You can do so with `Panels -> Add New Panel -> RvizVisualToolsGui`.
 9 | 
10 | * **Marker Array**  
11 |   `Add -> By display type -> MarkerArray` to view the poses and trajectories.


--------------------------------------------------------------------------------
/ar3_moveit_config/launch/planning_context.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |   <!-- By default we do not overwrite the URDF. Change the following to true to change the default behavior -->
 3 |   <arg name="load_robot_description" default="false"/>
 4 | 
 5 |   <!-- The name of the parameter under which the URDF is loaded -->
 6 |   <arg name="robot_description" default="robot_description"/>
 7 | 
 8 |   <!-- Load universal robot description format (URDF) -->
 9 |   <param if="$(arg load_robot_description)" name="$(arg robot_description)" command="$(find xacro)/xacro --inorder $(find ar3_description)/urdf/ar3.urdf.xacro"/>
10 | 
11 |   <!-- The semantic description that corresponds to the URDF -->
12 |   <param name="$(arg robot_description)_semantic" textfile="$(find ar3_moveit_config)/config/ar3.srdf" />
13 |   
14 |   <!-- Load updated joint limits (override information from URDF) -->
15 |   <group ns="$(arg robot_description)_planning">
16 |     <rosparam command="load" file="$(find ar3_moveit_config)/config/joint_limits.yaml"/>
17 |   </group>
18 | 
19 |   <!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->
20 |   <group ns="$(arg robot_description)_kinematics">
21 |     <rosparam command="load" file="$(find ar3_moveit_config)/config/kinematics.yaml"/>
22 |   </group>
23 |   
24 | </launch>
25 | 


--------------------------------------------------------------------------------
/ar3_control/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.1.3)
 2 | project(ar3_control)
 3 | 
 4 | set(CMAKE_CXX_STANDARD 14)
 5 | set(CMAKE_CXX_EXTENSIONS OFF)
 6 | 
 7 | find_package(catkin REQUIRED
 8 |   COMPONENTS
 9 |     interactive_markers
10 |     moveit_core
11 |     moveit_visual_tools
12 |     moveit_ros_planning
13 |     moveit_ros_planning_interface
14 |     moveit_ros_perception
15 |     pluginlib
16 |     geometric_shapes
17 |     pcl_ros
18 |     pcl_conversions
19 |     rosbag
20 |     tf2_ros
21 |     tf2_eigen
22 |     tf2_geometry_msgs
23 | )
24 | 
25 | find_package(Eigen3 REQUIRED)
26 | find_package(Boost REQUIRED system filesystem date_time thread)
27 | 
28 | catkin_package(
29 |   LIBRARIES
30 |   INCLUDE_DIRS
31 |   CATKIN_DEPENDS
32 |     moveit_core
33 |     moveit_visual_tools
34 |     moveit_ros_planning_interface
35 |     interactive_markers
36 |     tf2_geometry_msgs
37 |   DEPENDS
38 |     EIGEN3
39 | )
40 | 
41 | ###########
42 | ## Build ##
43 | ###########
44 | 
45 | include_directories(${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIRS})
46 | 
47 | add_executable(move_group_demo src/move_group_demo.cpp)
48 | target_link_libraries(move_group_demo ${catkin_LIBRARIES} ${Boost_LIBRARIES})
49 | install(TARGETS move_group_demo DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
50 | 
51 | install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})


--------------------------------------------------------------------------------
/ar3_hardware_interface/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package>
 3 |   <name>ar3_hardware_interface</name>
 4 |   <version>0.0.1</version>
 5 |   <description>
 6 |     The ar3_hardware_interface package provides the hardware interface for
 7 |     running an AR3 robot using the ros_control software framework.
 8 |   </description>
 9 | 
10 |   <maintainer email="dexterong94@gmail.com">Dexter Ong</maintainer>
11 | 
12 |   <license>MIT</license>
13 | 
14 |   <author email="dexterong94@gmail.com">Dexter Ong</author>
15 | 
16 |   <buildtool_depend>catkin</buildtool_depend>
17 | 
18 |   <build_depend>hardware_interface</build_depend>
19 |   <build_depend>controller_manager</build_depend>
20 |   <build_depend>roscpp</build_depend>
21 |   <build_depend>transmission_interface</build_depend>
22 |   <build_depend>urdf</build_depend>
23 |   <build_depend>joint_limits_interface</build_depend>
24 |   <build_depend>ar3_hardware_drivers</build_depend>
25 | 
26 |   <run_depend>hardware_interface</run_depend>
27 |   <run_depend>controller_manager</run_depend>
28 |   <run_depend>ros_controllers</run_depend>
29 |   <run_depend>ar3_description</run_depend>
30 |   <run_depend>roscpp</run_depend>
31 |   <run_depend>transmission_interface</run_depend>
32 |   <run_depend>urdf</run_depend>
33 |   <run_depend>joint_limits_interface</run_depend>
34 |   <run_depend>ar3_hardware_drivers</run_depend>
35 | 
36 | </package>
37 | 


--------------------------------------------------------------------------------
/ar3_moveit_config/launch/trajectory_execution.launch.xml:
--------------------------------------------------------------------------------
 1 | <launch>
 2 | 
 3 |   <!-- This file makes it easy to include the settings for trajectory execution  -->  
 4 | 
 5 |   <!-- Flag indicating whether MoveIt! is allowed to load/unload  or switch controllers -->
 6 |   <arg name="moveit_manage_controllers" default="true"/>
 7 |   <param name="moveit_manage_controllers" value="$(arg moveit_manage_controllers)"/>
 8 | 
 9 |   <!-- When determining the expected duration of a trajectory, this multiplicative factor is applied to get the allowed duration of execution -->
10 |   <param name="trajectory_execution/allowed_execution_duration_scaling" value="1.2"/> <!-- default 1.2 -->
11 |   <!-- Allow more than the expected execution time before triggering a trajectory cancel (applied after scaling) -->
12 |   <param name="trajectory_execution/allowed_goal_duration_margin" value="0.5"/> <!-- default 0.5 -->
13 |   <!-- Allowed joint-value tolerance for validation that trajectory's first point matches current robot state -->
14 |   <param name="trajectory_execution/allowed_start_tolerance" value="0.0"/> <!-- default 0.01 -->
15 |   
16 |   <!-- Load the robot specific controller manager; this sets the moveit_controller_manager ROS parameter -->
17 |   <arg name="moveit_controller_manager" default="ar3" />
18 |   <include file="$(find ar3_moveit_config)/launch/$(arg moveit_controller_manager)_moveit_controller_manager.launch.xml" />
19 |   
20 | </launch>
21 | 


--------------------------------------------------------------------------------
/ar3_moveit_config/launch/ar3_moveit_bringup.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 | 
 3 |   <!-- By default, we do not start a database (it can be large) -->
 4 |   <arg name="db" default="false" />
 5 |   <!-- Allow user to specify database location -->
 6 |   <arg name="db_path" default="$(find ar3_moveit_config)/default_warehouse_mongo_db" />
 7 | 
 8 |   <!-- By default, we are not in debug mode -->
 9 |   <arg name="debug" default="false" />
10 | 
11 |   <!-- By default, don't load robot description -->
12 |   <arg name="load_robot_description" default="false"/>
13 | 
14 |   <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
15 |   <include file="$(find ar3_moveit_config)/launch/planning_context.launch">
16 |     <arg name="load_robot_description" value="$(arg load_robot_description)"/>
17 |   </include>
18 | 
19 |   <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
20 |   <include file="$(find ar3_moveit_config)/launch/move_group.launch">
21 |     <arg name="allow_trajectory_execution" value="true"/>
22 |     <arg name="fake_execution" value="false"/>
23 |     <arg name="info" value="true"/>
24 |     <arg name="debug" value="$(arg debug)"/>
25 |   </include>
26 | 
27 |   <!-- Run Rviz and load the default config to see the state of the move_group node -->
28 |   <include file="$(find ar3_moveit_config)/launch/moveit_rviz.launch">
29 |     <arg name="config" value="true"/>
30 |     <arg name="debug" value="$(arg debug)"/>
31 |   </include>
32 | 
33 |   <!-- If database loading was enabled, start mongodb as well -->
34 |   <include file="$(find ar3_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)">
35 |     <arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
36 |   </include>
37 | 
38 |   <!-- World to base transform -->
39 |   <node pkg="tf" type="static_transform_publisher" name="world_broadcaster" args = "0 0 0 0 0 0 world base_link 10" />
40 | 
41 | </launch>
42 | 


--------------------------------------------------------------------------------
/ar3_hardware_drivers/include/ar3_hardware_drivers/TeensyDriver.h:
--------------------------------------------------------------------------------
 1 | #ifndef TEENSY_DRIVER_H
 2 | #define TEENSY_DRIVER_H
 3 | 
 4 | #include <vector>
 5 | #include <unordered_map>
 6 | #include <string>
 7 | #include "math.h"
 8 | #include "time.h"
 9 | #include <ros/ros.h>
10 | #include <boost/asio.hpp>
11 | 
12 | namespace ar3_hardware_drivers {
13 | 
14 | class TeensyDriver {
15 |   public:
16 |     void init(std::string port, int baudrate, int num_joints, std::vector<double>& enc_steps_per_deg);
17 |     void setStepperSpeed(std::vector<double>& max_speed, std::vector<double>& max_accel);
18 |     void update(std::vector<double>& pos_commands, std::vector<double>& joint_states);
19 |     void getJointPositions(std::vector<double>& joint_positions);
20 |     void calibrateJoints();
21 | 
22 |     TeensyDriver();
23 | 
24 |   private:
25 |     bool initialised_;
26 |     std::string version_;
27 |     boost::asio::io_service io_service_;
28 |     boost::asio::serial_port serial_port_;
29 |     int num_joints_;
30 |     std::vector<int> enc_commands_;
31 |     std::vector<int> enc_steps_;
32 |     std::vector<double> enc_steps_per_deg_;
33 |     std::vector<int> enc_calibrations_;
34 | 
35 | 
36 |     // Comms with teensy
37 |     void exchange(std::string outMsg); // exchange joint commands/state
38 |     bool transmit(std::string outMsg, std::string& err);
39 |     bool receive(std::string &inMsg, std::string& err);
40 |     void sendCommand(std::string outMsg); // send arbitrary commands
41 | 
42 |     void checkInit(std::string msg);
43 |     void updateEncoderCalibrations(std::string msg);
44 |     void updateEncoderSteps(std::string msg);
45 | 
46 |     // Convert between encoder steps and joint angle degrees
47 |     void encStepsToJointPos(std::vector<int>& enc_steps, std::vector<double>& joint_positions);
48 |     void jointPosToEncSteps(std::vector<double>& joint_positions, std::vector<int>& enc_steps);
49 | };
50 | 
51 | } // namespace ar3_hardware_drivers
52 | 
53 | #endif // TEENSY_DRIVER
54 | 


--------------------------------------------------------------------------------
/ar3_control/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package>
 3 |   <name>ar3_control</name>
 4 |   <version>0.1.0</version>
 5 |   <description>AR3 control demo with the Move Group interface</description>
 6 |   <license>BSD</license>
 7 | 
 8 |   <author email="dexterong94@gmail.com">Dexter Ong</author>
 9 | 
10 |   <maintainer email="dexterong94@gmail.com">Dexter Ong</maintainer>
11 | 
12 |   <buildtool_depend>catkin</buildtool_depend>
13 | 
14 |   <build_depend>pluginlib</build_depend>
15 |   <build_depend>eigen</build_depend>
16 |   <build_depend>moveit_core</build_depend>
17 |   <build_depend>moveit_ros_planning</build_depend>
18 |   <build_depend>moveit_ros_planning_interface</build_depend>
19 |   <build_depend>moveit_ros_perception</build_depend>
20 |   <build_depend>interactive_markers</build_depend>
21 |   <build_depend>geometric_shapes</build_depend>
22 |   <build_depend>moveit_visual_tools</build_depend>
23 |   <build_depend>pcl_ros</build_depend>
24 |   <build_depend>pcl_conversions</build_depend>
25 |   <build_depend>rosbag</build_depend>
26 |   <build_depend>tf2_ros</build_depend>
27 |   <build_depend>tf2_eigen</build_depend>
28 |   <build_depend>tf2_geometry_msgs</build_depend>
29 | 
30 |   <run_depend>panda_moveit_config</run_depend>
31 |   <run_depend>pluginlib</run_depend>
32 |   <run_depend>moveit_core</run_depend>
33 |   <run_depend>moveit_commander</run_depend>
34 |   <run_depend>moveit_fake_controller_manager</run_depend>
35 |   <run_depend>moveit_ros_planning_interface</run_depend>
36 |   <run_depend>moveit_ros_perception</run_depend>
37 |   <run_depend>interactive_markers</run_depend>
38 |   <run_depend>moveit_visual_tools</run_depend>
39 |   <run_depend>joy</run_depend>
40 |   <run_depend>pcl_ros</run_depend>
41 |   <run_depend>pcl_conversions</run_depend>
42 |   <run_depend>rosbag</run_depend>
43 |   <run_depend>tf2_ros</run_depend>
44 |   <run_depend>tf2_eigen</run_depend>
45 |   <run_depend>tf2_geometry_msgs</run_depend>
46 | 
47 |   <test_depend>moveit_resources_panda_moveit_config</test_depend>
48 | 
49 | </package>
50 | 


--------------------------------------------------------------------------------
/ar3_hardware_interface/launch/ar3_hardware_bringup.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |     <arg name="robot_name" default="ar3"/>
 3 |     
 4 |     <!-- GDB functionality -->
 5 |     <arg name="debug" default="false" />
 6 |     <arg unless="$(arg debug)" name="launch_prefix" value="" />
 7 |     <arg if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
 8 |     <arg name="use_existing_calibrations" default="false" />
 9 | 
10 |     <param name="/ar3/hardware_interface/use_existing_calibrations" type="bool" value="$(arg use_existing_calibrations)"/>
11 | 
12 |     <!-- Load hardware interface and driver configs -->
13 |     <rosparam file="$(find ar3_hardware_interface)/config/hardware_interface.yaml" command="load"/>
14 |     <rosparam file="$(find ar3_hardware_interface)/config/controllers.yaml" command="load"/>
15 |     <rosparam file="$(find ar3_hardware_interface)/config/joint_offsets.yaml" command="load"/>
16 |     <rosparam file="$(find ar3_hardware_interface)/config/hardware_driver.yaml" command="load"/>
17 | 
18 |     <!-- Load moveit joint limits config in separate namespace to get velocity limits -->
19 |     <rosparam file="$(find ar3_moveit_config)/config/joint_limits.yaml" command="load" ns="ar3"/>
20 | 
21 |     <!-- Load universal robot description format (URDF) -->
22 |     <param name="robot_description" command="$(find xacro)/xacro --inorder $(find ar3_description)/urdf/ar3.urdf.xacro"/>
23 | 
24 |     <!-- The semantic description that corresponds to the URDF -->
25 |     <param name="robot_description_semantic" textfile="$(find ar3_moveit_config)/config/ar3.srdf" />
26 | 
27 |     <!-- Start hardware_interface node -->
28 |     <node name="ar3_hardware_interface" pkg="ar3_hardware_interface" type="ar3_hardware_interface_node" output="screen" launch-prefix="$(arg launch_prefix)"/>
29 | 
30 |     <!-- Start controllers -->
31 |     <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" ns="/" 
32 |           args="/ar3/controllers/state
33 | 			    /ar3/controllers/position"/>
34 | 
35 | </launch>
36 | 


--------------------------------------------------------------------------------
/ar3_hardware_interface/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(ar3_hardware_interface)
 3 | 
 4 | if(WIN32)
 5 |     macro(get_WIN32_WINNT version)
 6 |         if(CMAKE_SYSTEM_VERSION)
 7 |             set(ver ${CMAKE_SYSTEM_VERSION})
 8 |             string(REGEX MATCH "^([0-9]+).([0-9])" ver ${ver})
 9 |             string(REGEX MATCH "^([0-9]+)" verMajor ${ver})
10 |             # Check for Windows 10, b/c we'll need to convert to hex 'A'.
11 |             if("${verMajor}" MATCHES "10")
12 |                 set(verMajor "A")
13 |                 string(REGEX REPLACE "^([0-9]+)" ${verMajor} ver ${ver})
14 |             endif()
15 |             # Remove all remaining '.' characters.
16 |             string(REPLACE "." "" ver ${ver})
17 |             # Prepend each digit with a zero.
18 |             string(REGEX REPLACE "([0-9A-Z])" "0\\1" ver ${ver})
19 |             set(${version} "0x${ver}")
20 |         endif()
21 |     endmacro()
22 | 
23 |     get_WIN32_WINNT(ver)
24 |     add_definitions(-D_WIN32_WINNT=${ver})
25 | endif()
26 | 
27 | # Default to C++14
28 | if(NOT CMAKE_CXX_STANDARD)
29 |   set(CMAKE_CXX_STANDARD 14)
30 | endif()
31 | 
32 | find_package(catkin REQUIRED COMPONENTS
33 |   hardware_interface
34 |   controller_manager
35 |   roscpp
36 |   urdf
37 |   joint_limits_interface
38 |   ar3_hardware_drivers
39 | )
40 | 
41 | catkin_package(
42 |   INCLUDE_DIRS 
43 |     include
44 |   LIBRARIES 
45 |     ar3_hardware_interface
46 |   CATKIN_DEPENDS 
47 |     hardware_interface
48 |     controller_manager
49 |     roscpp
50 |     urdf
51 |     joint_limits_interface
52 |     ar3_hardware_drivers
53 | )
54 | 
55 | include_directories(
56 |   include/
57 |   ${catkin_INCLUDE_DIRS})
58 | 
59 | install(DIRECTORY include/${PROJECT_NAME}/
60 |   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
61 |   FILES_MATCHING PATTERN "*.h"
62 |   PATTERN ".svn" EXCLUDE)
63 | 
64 | add_library(ar3_hardware_interface src/ar3_hardware_interface.cpp)
65 | target_link_libraries(ar3_hardware_interface
66 | 	${catkin_LIBRARIES}
67 | )
68 | 
69 | add_executable(ar3_hardware_interface_node src/ar3_hardware_interface_node.cpp)
70 | add_dependencies(ar3_hardware_interface_node ar3_hardware_interface)
71 | 
72 | target_link_libraries(ar3_hardware_interface_node
73 | 	ar3_hardware_interface
74 | 	${catkin_LIBRARIES}
75 | )
76 | 


--------------------------------------------------------------------------------
/ar3_moveit_config/launch/demo.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 | 
 3 |   <!-- By default, we do not start a database (it can be large) -->
 4 |   <arg name="db" default="false" />
 5 |   <!-- Allow user to specify database location -->
 6 |   <arg name="db_path" default="$(find ar3_moveit_config)/default_warehouse_mongo_db" />
 7 | 
 8 |   <!-- By default, we are not in debug mode -->
 9 |   <arg name="debug" default="false" />
10 | 
11 |   <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
12 |   <include file="$(find ar3_moveit_config)/launch/planning_context.launch">
13 |     <arg name="load_robot_description" value="true"/>
14 |   </include>
15 | 
16 |   <!-- We do not have a robot connected, so publish fake joint states -->
17 |   <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
18 |     <param name="/use_gui" value="true"/>
19 |     <rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam>
20 |   </node>
21 | 
22 |   <!-- Given the published joint states, publish tf for the robot links -->
23 |   <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
24 | 
25 |   <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
26 |   <include file="$(find ar3_moveit_config)/launch/move_group.launch">
27 |     <arg name="allow_trajectory_execution" value="true"/>
28 |     <arg name="fake_execution" value="true"/>
29 |     <arg name="info" value="true"/>
30 |     <arg name="debug" value="$(arg debug)"/>
31 |   </include>
32 | 
33 |   <!-- Run Rviz and load the default config to see the state of the move_group node -->
34 |   <include file="$(find ar3_moveit_config)/launch/moveit_rviz.launch">
35 |     <arg name="config" value="true"/>
36 |     <arg name="debug" value="$(arg debug)"/>
37 |   </include>
38 | 
39 |   <!-- If database loading was enabled, start mongodb as well -->
40 |   <include file="$(find ar3_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)">
41 |     <arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
42 |   </include>
43 | 
44 |   <!-- World to base transform -->
45 |   <node pkg="tf" type="static_transform_publisher" name="world_broadcaster" args = "0 0 0 0 0 0 world base_link 10" />
46 | 
47 | </launch>
48 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | # toplevel CMakeLists.txt for a catkin workspace
 2 | # catkin/cmake/toplevel.cmake
 3 | 
 4 | cmake_minimum_required(VERSION 3.0.2)
 5 | 
 6 | project(Project)
 7 | 
 8 | set(CATKIN_TOPLEVEL TRUE)
 9 | 
10 | # search for catkin within the workspace
11 | set(_cmd "catkin_find_pkg" "catkin" "${CMAKE_SOURCE_DIR}")
12 | execute_process(COMMAND ${_cmd}
13 |   RESULT_VARIABLE _res
14 |   OUTPUT_VARIABLE _out
15 |   ERROR_VARIABLE _err
16 |   OUTPUT_STRIP_TRAILING_WHITESPACE
17 |   ERROR_STRIP_TRAILING_WHITESPACE
18 | )
19 | if(NOT _res EQUAL 0 AND NOT _res EQUAL 2)
20 |   # searching fot catkin resulted in an error
21 |   string(REPLACE ";" " " _cmd_str "${_cmd}")
22 |   message(FATAL_ERROR "Search for 'catkin' in workspace failed (${_cmd_str}): ${_err}")
23 | endif()
24 | 
25 | # include catkin from workspace or via find_package()
26 | if(_res EQUAL 0)
27 |   set(catkin_EXTRAS_DIR "${CMAKE_SOURCE_DIR}/${_out}/cmake")
28 |   # include all.cmake without add_subdirectory to let it operate in same scope
29 |   include(${catkin_EXTRAS_DIR}/all.cmake NO_POLICY_SCOPE)
30 |   add_subdirectory("${_out}")
31 | 
32 | else()
33 |   # use either CMAKE_PREFIX_PATH explicitly passed to CMake as a command line argument
34 |   # or CMAKE_PREFIX_PATH from the environment
35 |   if(NOT DEFINED CMAKE_PREFIX_PATH)
36 |     if(NOT "$ENV{CMAKE_PREFIX_PATH}" STREQUAL "")
37 |       if(NOT WIN32)
38 |         string(REPLACE ":" ";" CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH})
39 |       else()
40 |         set(CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH})
41 |       endif()
42 |     endif()
43 |   endif()
44 | 
45 |   # list of catkin workspaces
46 |   set(catkin_search_path "")
47 |   foreach(path ${CMAKE_PREFIX_PATH})
48 |     if(EXISTS "${path}/.catkin")
49 |       list(FIND catkin_search_path ${path} _index)
50 |       if(_index EQUAL -1)
51 |         list(APPEND catkin_search_path ${path})
52 |       endif()
53 |     endif()
54 |   endforeach()
55 | 
56 |   # search for catkin in all workspaces
57 |   set(CATKIN_TOPLEVEL_FIND_PACKAGE TRUE)
58 |   find_package(catkin QUIET
59 |     NO_POLICY_SCOPE
60 |     PATHS ${catkin_search_path}
61 |     NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH)
62 |   unset(CATKIN_TOPLEVEL_FIND_PACKAGE)
63 | 
64 |   if(NOT catkin_FOUND)
65 |     message(FATAL_ERROR "find_package(catkin) failed. catkin was neither found in the workspace nor in the CMAKE_PREFIX_PATH. One reason may be that no ROS setup.sh was sourced before.")
66 |   endif()
67 | endif()
68 | 
69 | catkin_workspace()
70 | 


--------------------------------------------------------------------------------
/ar3_moveit_config/config/joint_limits.yaml:
--------------------------------------------------------------------------------
 1 | # ar3:
 2 |   joint_limits: # degrees
 3 |     joint_1:
 4 |       has_position_limits: true
 5 |       min_position: -2.96706 # -170.0
 6 |       max_position: 2.96706 # 170.0
 7 |       has_velocity_limits: true
 8 |       max_velocity: 1.0
 9 |       has_acceleration_limits: true
10 |       max_acceleration: 0.1
11 |       has_jerk_limits: false
12 |       max_jerk: 0.0
13 |       has_effort_limits: false
14 |       max_effort: 0.0
15 |       min_effort: 0.0
16 | 
17 |     joint_2:
18 |       has_position_limits: true
19 |       min_position: -0.6911504 # -39.6
20 |       max_position: 1.57079632679 # 90.0
21 |       has_velocity_limits: true
22 |       max_velocity: 1.0
23 |       has_acceleration_limits: true
24 |       max_acceleration: 0.1
25 |       has_jerk_limits: false
26 |       max_jerk: 0.0
27 |       has_effort_limits: false
28 |       max_effort: 0.0
29 |       min_effort: 0.0
30 | 
31 |     joint_3:
32 |       has_position_limits: true
33 |       min_position: 0.0 # 0.0
34 |       max_position: 2.5080381 # default 143.7
35 |       has_velocity_limits: true
36 |       max_velocity: 1.0
37 |       has_acceleration_limits: true
38 |       max_acceleration: 0.1
39 |       has_jerk_limits: false
40 |       max_jerk: 0.0
41 |       has_effort_limits: false
42 |       max_effort: 0.0
43 |       min_effort: 0.0
44 | 
45 |     joint_4:
46 |       has_position_limits: true
47 |       min_position: -2.8710666 # -164.5
48 |       max_position: 2.8710666 # 164.5
49 |       has_velocity_limits: true
50 |       max_velocity: 1.0
51 |       has_acceleration_limits: true
52 |       max_acceleration: 0.1
53 |       has_jerk_limits: false
54 |       max_jerk: 0.0
55 |       has_effort_limits: false
56 |       max_effort: 0.0
57 |       min_effort: 0.0
58 | 
59 |     joint_5:
60 |       has_position_limits: true
61 |       min_position: -1.81776042 # -104.15
62 |       max_position: 1.81776042 # 104.15
63 |       has_velocity_limits: true
64 |       max_velocity: 1.0
65 |       has_acceleration_limits: true
66 |       max_acceleration: 0.1
67 |       has_jerk_limits: false
68 |       max_jerk: 0.0
69 |       has_effort_limits: false
70 |       max_effort: 0.0
71 |       min_effort: 0.0
72 | 
73 |     joint_6:
74 |       has_position_limits: true
75 |       min_position: -2.5848326 # -148.1
76 |       max_position: 2.5848326 # 148.1
77 |       has_velocity_limits: true
78 |       max_velocity: 1.0
79 |       has_acceleration_limits: true
80 |       max_acceleration: 0.1
81 |       has_jerk_limits: false
82 |       max_jerk: 0.0
83 |       has_effort_limits: false
84 |       max_effort: 0.0
85 |       min_effort: 0.0
86 | 


--------------------------------------------------------------------------------
/ar3_hardware_drivers/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.0.2)
 2 | project(ar3_hardware_drivers)
 3 | 
 4 | if(WIN32)
 5 |     macro(get_WIN32_WINNT version)
 6 |         if(CMAKE_SYSTEM_VERSION)
 7 |             set(ver ${CMAKE_SYSTEM_VERSION})
 8 |             string(REGEX MATCH "^([0-9]+).([0-9])" ver ${ver})
 9 |             string(REGEX MATCH "^([0-9]+)" verMajor ${ver})
10 |             # Check for Windows 10, b/c we'll need to convert to hex 'A'.
11 |             if("${verMajor}" MATCHES "10")
12 |                 set(verMajor "A")
13 |                 string(REGEX REPLACE "^([0-9]+)" ${verMajor} ver ${ver})
14 |             endif()
15 |             # Remove all remaining '.' characters.
16 |             string(REPLACE "." "" ver ${ver})
17 |             # Prepend each digit with a zero.
18 |             string(REGEX REPLACE "([0-9A-Z])" "0\\1" ver ${ver})
19 |             set(${version} "0x${ver}")
20 |         endif()
21 |     endmacro()
22 | 
23 |     get_WIN32_WINNT(ver)
24 |     add_definitions(-D_WIN32_WINNT=${ver})
25 | endif()
26 | 
27 | # Default to C++14
28 | if(NOT CMAKE_CXX_STANDARD)
29 |   set(CMAKE_CXX_STANDARD 14)
30 | endif()
31 | 
32 | if (POLICY CMP0074)
33 |   cmake_policy(SET CMP0074 NEW)
34 | endif()
35 | 
36 | ## Find catkin macros and libraries
37 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
38 | ## is used, also find other catkin packages
39 | find_package(catkin REQUIRED COMPONENTS
40 |   roscpp
41 |   rospy
42 |   std_msgs
43 | )
44 | 
45 | # boost
46 | find_package(Boost REQUIRED COMPONENTS system)
47 | include_directories(${Boost_INCLUDE_DIRS})
48 | 
49 | catkin_package(
50 |   INCLUDE_DIRS include
51 |   LIBRARIES ar3_hardware_drivers
52 |   CATKIN_DEPENDS roscpp rospy std_msgs
53 |   DEPENDS Boost
54 | )
55 | 
56 | include_directories(
57 |   include 
58 |   include/ar3_hardware_drivers 
59 |   ${catkin_INCLUDE_DIRS}
60 | )
61 | 
62 | add_library(${PROJECT_NAME}
63 |   src/TeensyDriver.cpp
64 | )
65 | 
66 | target_link_libraries(${PROJECT_NAME}
67 |   ${catkin_LIBRARIES}
68 |   ${Boost_LIBRARIES}
69 | )
70 | 
71 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
72 | 
73 | ## Mark executables and/or libraries for installation
74 | install(TARGETS ${PROJECT_NAME}
75 |  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
76 |  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
77 |  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
78 | )
79 | 
80 | ## Mark cpp header files for installation
81 | install(DIRECTORY include/
82 |   DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}
83 |   FILES_MATCHING PATTERN "*.h"
84 | )
85 | 
86 | ## Mark launch files for installation
87 | install(DIRECTORY launch/
88 |   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
89 | )
90 | 


--------------------------------------------------------------------------------
/ar3_hardware_interface/include/ar3_hardware_interface/ar3_hardware_interface.h:
--------------------------------------------------------------------------------
 1 | #ifndef AR3_HARDWARE_INTERFACE_H
 2 | #define AR3_HARDWARE_INTERFACE_H
 3 | 
 4 | #include <ros/ros.h>
 5 | #include <hardware_interface/joint_state_interface.h>
 6 | #include <hardware_interface/joint_command_interface.h>
 7 | #include <hardware_interface/robot_hw.h>
 8 | #include <joint_limits_interface/joint_limits_interface.h>
 9 | #include <joint_limits_interface/joint_limits.h>
10 | #include <joint_limits_interface/joint_limits_urdf.h>
11 | #include <joint_limits_interface/joint_limits_rosparam.h>
12 | #include <controller_manager/controller_manager.h>
13 | #include <boost/scoped_ptr.hpp>
14 | #include "ar3_hardware_drivers/TeensyDriver.h"
15 | 
16 | #include <chrono>
17 | #include <thread>
18 | 
19 | using namespace hardware_interface;
20 | using joint_limits_interface::JointLimits;
21 | using joint_limits_interface::SoftJointLimits;
22 | using joint_limits_interface::PositionJointSoftLimitsHandle;
23 | using joint_limits_interface::PositionJointSoftLimitsInterface;
24 | 
25 | namespace ar3_hardware_interface
26 | {
27 | 	class AR3HardwareInterface: public hardware_interface::RobotHW
28 | 	{
29 | 		public:
30 | 			AR3HardwareInterface(ros::NodeHandle& nh);
31 | 			~AR3HardwareInterface();
32 | 			void init();
33 | 			void update(const ros::TimerEvent& e);
34 | 			void read();
35 | 			void write(ros::Duration elapsed_time);
36 | 
37 | 		private:
38 | 			ros::NodeHandle nh_;
39 | 			ros::Timer non_realtime_loop_;
40 | 			ros::Duration control_period_;
41 | 			ros::Duration elapsed_time_;
42 | 			PositionJointInterface positionJointInterface;
43 | 			PositionJointSoftLimitsInterface positionJointSoftLimitsInterface;
44 | 			double loop_hz_;
45 | 			boost::shared_ptr<controller_manager::ControllerManager> controller_manager_;
46 | 			double p_error_, v_error_, e_error_;
47 | 
48 | 			// Motor driver
49 | 			ar3_hardware_drivers::TeensyDriver driver_;
50 | 			std::vector<double> actuator_commands_;
51 | 			std::vector<double> actuator_positions_;
52 | 
53 | 			// Interfaces
54 | 			hardware_interface::JointStateInterface joint_state_interface_;
55 | 			hardware_interface::PositionJointInterface position_joint_interface_;
56 | 
57 | 			joint_limits_interface::PositionJointSaturationInterface position_joint_saturation_interface_;
58 | 			joint_limits_interface::PositionJointSoftLimitsInterface position_joint_limits_interface_;
59 | 
60 | 			// Shared memory
61 | 			int num_joints_;
62 | 			std::vector<std::string> joint_names_;
63 | 			std::vector<double> joint_offsets_;
64 | 			std::vector<double> joint_positions_;
65 | 			std::vector<double> joint_velocities_;
66 | 			std::vector<double> joint_efforts_;
67 | 			std::vector<double> joint_position_commands_;
68 | 			std::vector<double> joint_velocity_commands_;
69 | 			std::vector<double> joint_effort_commands_;
70 | 			std::vector<double> joint_lower_limits_;
71 | 			std::vector<double> joint_upper_limits_;
72 | 			std::vector<double> velocity_limits_;
73 | 			std::vector<double> acceleration_limits_;
74 | 
75 | 			// Misc
76 | 			double degToRad(double deg);
77 | 			double radToDeg(double rad);
78 | 	};
79 | } // namespace ar3_hardware_interface
80 | 
81 | #endif // AR3_HARDWARE_INTERFACE_H
82 | 


--------------------------------------------------------------------------------
/ar3_moveit_config/config/ar3.srdf:
--------------------------------------------------------------------------------
 1 |   
 2 | <?xml version="1.0" ?>
 3 | <!--This does not replace URDF, and is not an extension of URDF.
 4 |     This is a format for representing semantic information about the robot structure.
 5 |     A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
 6 | -->
 7 | <robot name="ar3">
 8 |     <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
 9 |     <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
10 |     <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
11 |     <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
12 |     <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
13 |     <group name="manipulator">
14 |         <joint name="joint_1" />
15 |         <joint name="joint_2" />
16 |         <joint name="joint_3" />
17 |         <joint name="joint_4" />
18 |         <joint name="joint_5" />
19 |         <joint name="joint_6" />
20 |     </group>
21 |     <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
22 |     <group_state name="rest" group="manipulator">
23 |         <joint name="joint_1" value="0" />
24 |         <joint name="joint_2" value="0" />
25 |         <joint name="joint_3" value="0" />
26 |         <joint name="joint_4" value="0" />
27 |         <joint name="joint_5" value="0" />
28 |         <joint name="joint_6" value="0" />
29 |     </group_state>
30 |     <group_state name="ready" group="manipulator">
31 |         <joint name="joint_1" value="1.5708" />
32 |         <joint name="joint_2" value="0.785398" />
33 |         <joint name="joint_3" value="1.5708" />
34 |         <joint name="joint_4" value="0" />
35 |         <joint name="joint_5" value="-0.785398" />
36 |         <joint name="joint_6" value="0" />
37 |     </group_state>
38 |     <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
39 |     <end_effector name="gripper" parent_link="link_6" group="manipulator" />
40 |     <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
41 |     <disable_collisions link1="base_link" link2="link_1" reason="Adjacent" />
42 |     <disable_collisions link1="link_1" link2="link_2" reason="Adjacent" />
43 |     <disable_collisions link1="link_1" link2="link_3" reason="Never" />
44 |     <disable_collisions link1="link_2" link2="link_3" reason="Adjacent" />
45 |     <disable_collisions link1="link_3" link2="link_4" reason="Adjacent" />
46 |     <disable_collisions link1="link_3" link2="link_5" reason="Never" />
47 |     <disable_collisions link1="link_3" link2="link_6" reason="Never" />
48 |     <disable_collisions link1="link_4" link2="link_5" reason="Adjacent" />
49 |     <disable_collisions link1="link_5" link2="link_6" reason="Adjacent" />
50 | </robot>


--------------------------------------------------------------------------------
/ar3_moveit_config/launch/move_group.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 | 
 3 |   <include file="$(find ar3_moveit_config)/launch/planning_context.launch" />
 4 | 
 5 |   <!-- GDB Debug Option -->
 6 |   <arg name="debug" default="false" />
 7 |   <arg unless="$(arg debug)" name="launch_prefix" value="" />
 8 |   <arg     if="$(arg debug)" name="launch_prefix"
 9 | 	   value="gdb -x $(find ar3_moveit_config)/launch/gdb_settings.gdb --ex run --args" />
10 | 
11 |   <!-- Verbose Mode Option -->
12 |   <arg name="info" default="$(arg debug)" />
13 |   <arg unless="$(arg info)" name="command_args" value="" />
14 |   <arg     if="$(arg info)" name="command_args" value="--debug" />
15 | 
16 |   <!-- move_group settings -->
17 |   <arg name="allow_trajectory_execution" default="true"/>
18 |   <arg name="fake_execution" default="false"/>
19 |   <arg name="max_safe_path_cost" default="1"/>
20 |   <arg name="jiggle_fraction" default="0.05" />
21 |   <arg name="publish_monitored_planning_scene" default="true"/>
22 | 
23 |   <!-- Planning Functionality -->
24 |   <include ns="move_group" file="$(find ar3_moveit_config)/launch/planning_pipeline.launch.xml">
25 |     <arg name="pipeline" value="ompl" />
26 |   </include>
27 | 
28 |   <!-- Trajectory Execution Functionality -->
29 |   <include ns="move_group" file="$(find ar3_moveit_config)/launch/trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
30 |     <arg name="moveit_manage_controllers" value="true" />
31 |     <arg name="moveit_controller_manager" value="ar3" unless="$(arg fake_execution)"/>
32 |     <arg name="moveit_controller_manager" value="fake" if="$(arg fake_execution)"/>
33 |   </include>
34 | 
35 |   <!-- Sensors Functionality -->
36 |   <include ns="move_group" file="$(find ar3_moveit_config)/launch/sensor_manager.launch.xml" if="$(arg allow_trajectory_execution)">
37 |     <arg name="moveit_sensor_manager" value="ar3" />
38 |   </include>
39 | 
40 |   <!-- Start the actual move_group node/action server -->
41 |   <node name="move_group" launch-prefix="$(arg launch_prefix)" pkg="moveit_ros_move_group" type="move_group" respawn="false" output="screen" args="$(arg command_args)">
42 |     <!-- Set the display variable, in case OpenGL code is used internally -->
43 |     <env name="DISPLAY" value="$(optenv DISPLAY :0)" />
44 | 
45 |     <param name="allow_trajectory_execution" value="$(arg allow_trajectory_execution)"/>
46 |     <param name="max_safe_path_cost" value="$(arg max_safe_path_cost)"/>
47 |     <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
48 | 
49 |     <!-- load these non-default MoveGroup capabilities -->
50 |     <!--
51 |     <param name="capabilities" value="
52 |                   a_package/AwsomeMotionPlanningCapability
53 |                   another_package/GraspPlanningPipeline
54 |                   " />
55 |     -->
56 | 
57 |     <!-- inhibit these default MoveGroup capabilities -->
58 |     <!--
59 |     <param name="disable_capabilities" value="
60 |                   move_group/MoveGroupKinematicsService
61 |                   move_group/ClearOctomapService
62 |                   " />
63 |     -->
64 | 
65 |     <!-- Publish the planning scene of the physical robot so that rviz plugin can know actual robot -->
66 |     <param name="planning_scene_monitor/publish_planning_scene" value="$(arg publish_monitored_planning_scene)" />
67 |     <param name="planning_scene_monitor/publish_geometry_updates" value="$(arg publish_monitored_planning_scene)" />
68 |     <param name="planning_scene_monitor/publish_state_updates" value="$(arg publish_monitored_planning_scene)" />
69 |     <param name="planning_scene_monitor/publish_transforms_updates" value="$(arg publish_monitored_planning_scene)" />
70 |   </node>
71 | 
72 | </launch>
73 | 


--------------------------------------------------------------------------------
/ar3_hardware_interface/src/ar3_hardware_interface.cpp:
--------------------------------------------------------------------------------
  1 | #include <sstream>
  2 | #include <ar3_hardware_interface/ar3_hardware_interface.h>
  3 | #include <joint_limits_interface/joint_limits_interface.h>
  4 | #include <joint_limits_interface/joint_limits.h>
  5 | #include <joint_limits_interface/joint_limits_urdf.h>
  6 | #include <joint_limits_interface/joint_limits_rosparam.h>
  7 | 
  8 | 
  9 | using joint_limits_interface::JointLimits;
 10 | using joint_limits_interface::PositionJointSoftLimitsHandle;
 11 | using joint_limits_interface::PositionJointSoftLimitsInterface;
 12 | using joint_limits_interface::SoftJointLimits;
 13 | 
 14 | namespace ar3_hardware_interface
 15 | {
 16 | 
 17 | AR3HardwareInterface::AR3HardwareInterface(ros::NodeHandle &nh) : nh_(nh)
 18 | {
 19 | 	init();
 20 | 
 21 | 	// init ros controller manager
 22 | 	controller_manager_.reset(new controller_manager::ControllerManager(this, nh_));
 23 | 
 24 | 	nh_.param("/ar3/hardware_interface/loop_hz", loop_hz_, 0.1);
 25 | 	ROS_DEBUG_STREAM_NAMED("constructor", "Using loop freqency of " << loop_hz_ << " hz");
 26 | 	ros::Duration update_freq = ros::Duration(1.0 / loop_hz_);
 27 | 	non_realtime_loop_ = nh_.createTimer(update_freq, &AR3HardwareInterface::update, this);
 28 | 
 29 | 	// initialize controller
 30 | 	for (int i = 0; i < num_joints_; ++i)
 31 | 	{
 32 | 		ROS_DEBUG_STREAM_NAMED("constructor", "Loading joint name: " << joint_names_[i]);
 33 | 
 34 | 		// Create joint state interface
 35 | 		JointStateHandle jointStateHandle(joint_names_[i], &joint_positions_[i], &joint_velocities_[i], &joint_efforts_[i]);
 36 | 		joint_state_interface_.registerHandle(jointStateHandle);
 37 | 
 38 | 		// Create position joint interface
 39 | 		JointHandle jointPositionHandle(jointStateHandle, &joint_position_commands_[i]);
 40 | 		position_joint_interface_.registerHandle(jointPositionHandle);
 41 | 	}
 42 | 
 43 | 	// get encoder calibration
 44 | 	std::vector<double> enc_steps_per_deg(num_joints_);
 45 | 
 46 | 	for (int i = 0; i < num_joints_; ++i)
 47 | 	{
 48 | 		if(!nh_.getParam("/ar3/hardware_driver/encoder_steps_per_deg/" + joint_names_[i], enc_steps_per_deg[i]))
 49 | 		{
 50 | 			ROS_WARN("Failed to get params for /ar3/hardware_driver/encoder_steps_per_deg/");
 51 | 		}
 52 | 
 53 | 		if (!nh_.getParam("/ar3/hardware_interface/joint_offsets/" + joint_names_[i], joint_offsets_[i]))
 54 | 		{
 55 | 			ROS_WARN("Failed to get params for /ar3/hardware_interface/joint_offsets/");
 56 | 		}
 57 | 
 58 | 	}
 59 | 
 60 | 	// init motor driver
 61 | 	std::string serial_port;
 62 | 	int baudrate;
 63 | 	nh_.getParam("/ar3/hardware_driver/serial_port", serial_port);
 64 | 	nh_.getParam("/ar3/hardware_driver/baudrate", baudrate);
 65 | 	driver_.init(serial_port, baudrate, num_joints_, enc_steps_per_deg);
 66 | 
 67 | 	// set velocity limits
 68 | 	for (int i = 0; i < num_joints_; ++i)
 69 | 	{
 70 | 		nh_.getParam("/ar3/joint_limits/" + joint_names_[i] + "/max_velocity", velocity_limits_[i]);
 71 | 		nh_.getParam("/ar3/joint_limits/" + joint_names_[i] + "/max_acceleration", acceleration_limits_[i]);
 72 | 		velocity_limits_[i] = radToDeg(velocity_limits_[i]);
 73 | 		acceleration_limits_[i] = radToDeg(acceleration_limits_[i]);
 74 | 	}
 75 | 	driver_.setStepperSpeed(velocity_limits_, acceleration_limits_);
 76 | 
 77 | 	// calibrate joints if needed
 78 | 	bool use_existing_calibrations = false;
 79 | 	nh_.getParam("/ar3/hardware_interface/use_existing_calibrations", use_existing_calibrations);
 80 | 	if (!use_existing_calibrations)
 81 | 	{
 82 | 		// run calibration
 83 | 		ROS_INFO("Running joint calibration...");
 84 | 		driver_.calibrateJoints();
 85 | 	}
 86 | 
 87 | 	// init position commands at current positions
 88 | 	driver_.getJointPositions(actuator_positions_);
 89 | 	for (int i = 0; i < num_joints_; ++i)
 90 | 	{
 91 | 		// apply offsets, convert from deg to rad for moveit
 92 | 		joint_positions_[i] = degToRad(actuator_positions_[i] + joint_offsets_[i]);
 93 | 		joint_position_commands_[i] = joint_positions_[i];
 94 | 	}
 95 | 
 96 | 	registerInterface(&joint_state_interface_);
 97 | 	registerInterface(&position_joint_interface_);
 98 | }
 99 | 
100 | AR3HardwareInterface::~AR3HardwareInterface()
101 | {
102 | }
103 | 
104 | void AR3HardwareInterface::init()
105 | {
106 | 	// get joint names
107 | 	nh_.getParam("/ar3/hardware_interface/joints", joint_names_);
108 | 	if (joint_names_.size() == 0)
109 | 	{
110 | 		ROS_FATAL_STREAM_NAMED("init", "No joints found on parameter server for controller. Did you load the proper yaml file?");
111 | 	}
112 | 	num_joints_ = static_cast<int>(joint_names_.size());
113 | 
114 | 	// resize vectors
115 | 	actuator_commands_.resize(num_joints_);
116 | 	actuator_positions_.resize(num_joints_);
117 | 	joint_positions_.resize(num_joints_);
118 | 	joint_velocities_.resize(num_joints_);
119 | 	joint_efforts_.resize(num_joints_);
120 | 	joint_position_commands_.resize(num_joints_);
121 | 	joint_velocity_commands_.resize(num_joints_);
122 | 	joint_effort_commands_.resize(num_joints_);
123 | 	joint_offsets_.resize(num_joints_);
124 | 	joint_lower_limits_.resize(num_joints_);
125 | 	joint_upper_limits_.resize(num_joints_);
126 | 	velocity_limits_.resize(num_joints_);
127 | 	acceleration_limits_.resize(num_joints_);
128 | }
129 | 
130 | void AR3HardwareInterface::update(const ros::TimerEvent &e)
131 | {
132 | 	std::string logInfo = "\n";
133 | 	logInfo += "Joint Position Command:\n";
134 | 	for (int i = 0; i < num_joints_; i++)
135 | 	{
136 | 		std::ostringstream jointPositionStr;
137 | 		jointPositionStr << radToDeg(joint_position_commands_[i]);
138 | 		logInfo += "  " + joint_names_[i] + ": " + jointPositionStr.str() + "\n";
139 | 	}
140 | 
141 | 	elapsed_time_ = ros::Duration(e.current_real - e.last_real);
142 | 	
143 | 	write(elapsed_time_);
144 | 	read();
145 | 
146 | 	logInfo += "Joint Position State:\n";
147 | 	for (int i = 0; i < num_joints_; i++)
148 | 	{
149 | 		std::ostringstream jointPositionStr;
150 | 		jointPositionStr << radToDeg(joint_positions_[i]);
151 | 		logInfo += "  " + joint_names_[i] + ": " + jointPositionStr.str() + "\n";
152 | 	}
153 | 
154 | 	controller_manager_->update(ros::Time::now(), elapsed_time_);
155 | 
156 | 	ROS_INFO_STREAM(logInfo);
157 | }
158 | 
159 | void AR3HardwareInterface::read()
160 | {		
161 | 	driver_.update(actuator_commands_, actuator_positions_);
162 | 	for (int i = 0; i < num_joints_; ++i)
163 | 	{
164 | 		// apply offsets, convert from deg to rad for moveit
165 | 		joint_positions_[i] = degToRad(actuator_positions_[i] + joint_offsets_[i]);
166 | 	}
167 | }
168 | 
169 | void AR3HardwareInterface::write(ros::Duration elapsed_time)
170 | {
171 | 	for (int i = 0; i < num_joints_; ++i)
172 | 	{
173 | 		// convert from rad to deg, apply offsets
174 | 		actuator_commands_[i] = radToDeg(joint_position_commands_[i]) - joint_offsets_[i];
175 | 	}
176 | }
177 | 
178 | double AR3HardwareInterface::degToRad(double deg)
179 | {
180 | 	return deg / 180.0 * M_PI;
181 | }
182 | 
183 | double AR3HardwareInterface::radToDeg(double rad)
184 | {
185 | 	return rad / M_PI * 180.0;
186 | }
187 | 
188 | } // namespace ar3_hardware_interface
189 | 


--------------------------------------------------------------------------------
/ar3_description/urdf/ar3.urdf.xacro:
--------------------------------------------------------------------------------
  1 | <?xml version="1.0" encoding="utf-8"?>
  2 | <!-- =================================================================================== -->
  3 | <!-- |    This document was autogenerated by xacro from ar3.urdf                       | -->
  4 | <!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
  5 | <!-- =================================================================================== -->
  6 | <robot name="ar3">
  7 |   <link name="base_link">
  8 |     <inertial>
  9 |       <origin rpy="0 0 0" xyz="-4.6941E-06 0.054174 0.038824"/>
 10 |       <mass value="0.7102"/>
 11 |       <inertia ixx="0.0039943" ixy="3.697E-07" ixz="-5.7364E-08" iyy="0.0014946" iyz="-0.00036051" izz="0.0042554"/>
 12 |     </inertial>
 13 |     <visual>
 14 |       <origin rpy="0 0 0" xyz="0 0 0"/>
 15 |       <geometry>
 16 |         <mesh filename="package://ar3_description/meshes/base_link.STL"/>
 17 |       </geometry>
 18 |       <material name="">
 19 |         <color rgba="1 1 0 1"/>
 20 |       </material>
 21 |     </visual>
 22 |     <collision>
 23 |       <origin rpy="0 0 0" xyz="0 0 0"/>
 24 |       <geometry>
 25 |         <mesh filename="package://ar3_description/meshes/base_link.STL"/>
 26 |       </geometry>
 27 |     </collision>
 28 |   </link>
 29 |   <link name="link_1">
 30 |     <inertial>
 31 |       <origin rpy="0 0 0" xyz="-0.022706 0.04294 -0.12205"/>
 32 |       <mass value="0.88065"/>
 33 |       <inertia ixx="0.0034" ixy="0.00042296" ixz="-0.00089231" iyy="0.0041778" iyz="0.0010848" izz="0.0027077"/>
 34 |     </inertial>
 35 |     <visual>
 36 |       <origin rpy="0 0 0" xyz="0 0 0"/>
 37 |       <geometry>
 38 |         <mesh filename="package://ar3_description/meshes/link_1.STL"/>
 39 |       </geometry>
 40 |       <material name="">
 41 |         <color rgba="1 1 0 1"/>
 42 |       </material>
 43 |     </visual>
 44 |     <collision>
 45 |       <origin rpy="0 0 0" xyz="0 0 0"/>
 46 |       <geometry>
 47 |         <mesh filename="package://ar3_description/meshes/link_1.STL"/>
 48 |       </geometry>
 49 |     </collision>
 50 |   </link>
 51 |   <joint name="joint_1" type="revolute">
 52 |     <origin rpy="${pi} 0 0" xyz="0 0 0.003445"/>
 53 |     <parent link="base_link"/>
 54 |     <child link="link_1"/>
 55 |     <axis xyz="0 0 1"/>
 56 |     <limit lower="-2.96706" upper="2.96706" effort="0" velocity="0"/>
 57 |   </joint>
 58 |   <link name="link_2">
 59 |     <inertial>
 60 |       <origin rpy="0 0 0" xyz="0.064818 -0.11189 -0.038671"/>
 61 |       <mass value="0.57738"/>
 62 |       <inertia ixx="0.0047312" ixy="0.0022624" ixz="0.00032144" iyy="0.0020836" iyz="-0.00056569" izz="0.0056129"/>
 63 |     </inertial>
 64 |     <visual>
 65 |       <origin rpy="0 0 0" xyz="0 0 0"/>
 66 |       <geometry>
 67 |         <mesh filename="package://ar3_description/meshes/link_2.STL"/>
 68 |       </geometry>
 69 |       <material name="">
 70 |         <color rgba="1 1 0 1"/>
 71 |       </material>
 72 |     </visual>
 73 |     <collision>
 74 |       <origin rpy="0 0 0" xyz="0 0 0"/>
 75 |       <geometry>
 76 |         <mesh filename="package://ar3_description/meshes/link_2.STL"/>
 77 |       </geometry>
 78 |     </collision>
 79 |   </link>
 80 |   <joint name="joint_2" type="revolute">
 81 |     <origin rpy="1.5708 0.5236 -1.5708" xyz="0 0.064146 -0.16608"/>
 82 |     <parent link="link_1"/>
 83 |     <child link="link_2"/>
 84 |     <axis xyz="0 0 -1"/>
 85 |     <limit lower="${-39.6 / 180.0 * pi}" upper="${pi / 2.0}" effort="0" velocity="0"/>
 86 |   </joint>
 87 |   <link name="link_3">
 88 |     <inertial>
 89 |       <origin rpy="0 0 0" xyz="-0.00029765 -0.023661 -0.0019125"/>
 90 |       <mass value="0.1787"/>
 91 |       <inertia ixx="0.0001685" ixy="-2.7713E-05" ixz="5.6885E-06" iyy="0.00012865" iyz="2.9256E-05" izz="0.00020744"/>
 92 |     </inertial>
 93 |     <visual>
 94 |       <origin rpy="0 0 0" xyz="0 0 0"/>
 95 |       <geometry>
 96 |         <mesh filename="package://ar3_description/meshes/link_3.STL"/>
 97 |       </geometry>
 98 |       <material name="">
 99 |         <color rgba="1 1 0 1"/>
100 |       </material>
101 |     </visual>
102 |     <collision>
103 |       <origin rpy="0 0 0" xyz="0 0 0"/>
104 |       <geometry>
105 |         <mesh filename="package://ar3_description/meshes/link_3.STL"/>
106 |       </geometry>
107 |     </collision>
108 |   </link>
109 |   <joint name="joint_3" type="revolute">
110 |     <origin rpy="0 0 -1.04720367321" xyz="0.1525 -0.26414 0"/>
111 |     <parent link="link_2"/>
112 |     <child link="link_3"/>
113 |     <axis xyz="0 0 -1"/>
114 |     <limit lower="0.0174533" upper="2.5080381" effort="0" velocity="0"/>
115 |   </joint>
116 |   <link name="link_4">
117 |     <inertial>
118 |       <origin rpy="0 0 0" xyz="-0.0016798 -0.00057319 -0.074404"/>
119 |       <mass value="0.34936"/>
120 |       <inertia ixx="0.0030532" ixy="-1.8615E-05" ixz="-7.0047E-05" iyy="0.0031033" iyz="-2.3301E-05" izz="0.00022264"/>
121 |     </inertial>
122 |     <visual>
123 |       <origin rpy="0 0 0" xyz="0 0 0"/>
124 |       <geometry>
125 |         <mesh filename="package://ar3_description/meshes/link_4.STL"/>
126 |       </geometry>
127 |       <material name="">
128 |         <color rgba="1 1 0 1"/>
129 |       </material>
130 |     </visual>
131 |     <collision>
132 |       <origin rpy="0 0 0" xyz="0 0 0"/>
133 |       <geometry>
134 |         <mesh filename="package://ar3_description/meshes/link_4.STL"/>
135 |       </geometry>
136 |     </collision>
137 |   </link>
138 |   <joint name="joint_4" type="revolute">
139 |     <origin rpy="1.5708 -1.2554 -1.5708" xyz="0 0 0.00675"/>
140 |     <parent link="link_3"/>
141 |     <child link="link_4"/>
142 |     <axis xyz="0 0 -1"/>
143 |     <limit lower="-2.8710666" upper="2.8710666" effort="0" velocity="0"/>
144 |   </joint>
145 |   <link name="link_5">
146 |     <inertial>
147 |       <origin rpy="0 0 0" xyz="0.0015066 -1.3102E-05 -0.012585"/>
148 |       <mass value="0.11562"/>
149 |       <inertia ixx="5.5035E-05" ixy="-1.019E-08" ixz="-2.6243E-06" iyy="8.2921E-05" iyz="1.4437E-08" izz="5.2518E-05"/>
150 |     </inertial>
151 |     <visual>
152 |       <origin rpy="0 0 0" xyz="0 0 0"/>
153 |       <geometry>
154 |         <mesh filename="package://ar3_description/meshes/link_5.STL"/>
155 |       </geometry>
156 |       <material name="">
157 |         <color rgba="1 1 0 1"/>
158 |       </material>
159 |     </visual>
160 |     <collision>
161 |       <origin rpy="0 0 0" xyz="0 0 0"/>
162 |       <geometry>
163 |         <mesh filename="package://ar3_description/meshes/link_5.STL"/>
164 |       </geometry>
165 |     </collision>
166 |   </link>
167 |   <joint name="joint_5" type="revolute">
168 |     <origin rpy="${pi} 0 -2.8262" xyz="0 0 -0.22225"/>
169 |     <parent link="link_4"/>
170 |     <child link="link_5"/>
171 |     <axis xyz="1 0 0"/>
172 |     <limit lower="-1.81776042" upper="1.81776042" effort="0" velocity="0"/>
173 |   </joint>
174 |   <link name="link_6">
175 |     <inertial>
176 |       <origin rpy="0 0 0" xyz="2.9287E-10 -1.6472E-09 0.0091432"/>
177 |       <mass value="0.013863"/>
178 |       <inertia ixx="1.3596E-06" ixy="3.0585E-13" ixz="5.7102E-14" iyy="1.7157E-06" iyz="6.3369E-09" izz="2.4332E-06"/>
179 |     </inertial>
180 |     <visual>
181 |       <origin rpy="0 0 0" xyz="0 0 0"/>
182 |       <geometry>
183 |         <mesh filename="package://ar3_description/meshes/link_6.STL"/>
184 |       </geometry>
185 |       <material name="">
186 |         <color rgba="1 1 0 1"/>
187 |       </material>
188 |     </visual>
189 |     <collision>
190 |       <origin rpy="0 0 0" xyz="0 0 0"/>
191 |       <geometry>
192 |         <mesh filename="package://ar3_description/meshes/link_6.STL"/>
193 |       </geometry>
194 |     </collision>
195 |   </link>
196 |   <joint name="joint_6" type="revolute">
197 |     <origin rpy="0 0 3.1416" xyz="-0.000294 0 0.02117"/>
198 |     <parent link="link_5"/>
199 |     <child link="link_6"/>
200 |     <axis xyz="0 0 1"/>
201 |     <limit lower="-2.5848326" upper="2.5848326" effort="0" velocity="0"/>
202 |   </joint>
203 | </robot>
204 | 
205 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
  1 | # AR3 Core Software
  2 | This repository provides the software for control of the AR3 arm with ros_control and MoveIt. I hope to provide fellow robotic arm enthusiasts with a starting point to explore using ROS for controlling the arm. The baseline implementation is designed to accomodate the original hardware and firmware, including the message structures for communication. Moving forward, I will do my best to continue to keep it accessible. I plan to explore some interesting concepts that I have in mind for on-arm vision, grippers and human-robot interaction and I will share my projects here if possible.
  3 | 
  4 | You may also wish to check out some hardware modifications that I've made for joint encoders at [ar3_hardware_mods](https://github.com/ongdexter/ar3_hardware_mods).
  5 | 
  6 | * [Overview](#Overview)
  7 | * [Installation](#Installation)
  8 | * [Usage](#Usage)
  9 | 
 10 | [![](http://img.youtube.com/vi/6D4vdhJlLsQ/0.jpg)](http://www.youtube.com/watch?v=6D4vdhJlLsQ "AR3 with ROS and MoveIt")
 11 | 
 12 | ## Overview
 13 | + **ar3_control**
 14 |     + Controlling the arm through the MoveIt user interfaces
 15 |     + Provides demo for the move group interface
 16 |     
 17 | + **ar3_description**
 18 |     + Hardware description of arm, urdf etc.
 19 |     
 20 | + **ar3_hardware_interface**
 21 |     + ROS interface for the hardware driver, built on the ros_control framework
 22 |     + Manages joint offsets, limits and conversion between joint and actuator messages
 23 |     
 24 | + **ar3_hardware_drivers**
 25 |     + Handles communication with the motor controllers
 26 | 
 27 | + **ar3_microcontrollers**
 28 |     + Firmware for the motor controller ie. Teensy
 29 | 
 30 | + **ar3_moveit_config**
 31 |     + MoveIt module for motion planning
 32 |     + Controlling the arm through Rviz
 33 | 
 34 | + **ar3_gazebo**
 35 |     + Simulation on Gazebo
 36 | 
 37 | 
 38 | ## Installation
 39 | *Actively developed and tested on ROS Melodic on Windows 10 with limited testing on Ubuntu 18.04.*
 40 | 
 41 | * [Windows 10](#Windows)
 42 | * [Ubuntu 18.04](#Ubuntu)
 43 | 
 44 | 
 45 | ### Windows
 46 | * Install [ROS Melodic and MoveIt](https://moveit.ros.org/install/) for Windows 10
 47 | * Create the ROS workspace `ie. ar3_ws`
 48 |   ```
 49 |   md C:\ar3_ws\src && C:\ar3_ws\src
 50 |   ```
 51 | 
 52 | * Clone this repository into the workspace `src`:
 53 |   ```
 54 |   git clone https://github.com/ongdexter/ar3_core.git .
 55 |   ```
 56 |   The workspace directory should be as such:
 57 |   ```
 58 |   ar_ws
 59 |   +-- src
 60 |   |   +-- ar3_control
 61 |   |   +-- ar3_description
 62 |   |   +-- ...
 63 |   ```
 64 | * Build the workspace:
 65 |   ```
 66 |   cd ~/ar3_ws
 67 |   catkin_make
 68 |   ```
 69 | * Source the workspace
 70 | 
 71 |   ```
 72 |   call C:\ar3_ws\setup.bat
 73 |   ```
 74 |   You can add this to the ROS Command Window shortcut that you created during the ROS installation, by appending `&& c:\ar3_ws\devel\setup.bat` to the shortcut target, so that it is automatically run each time a new terminal is opened.
 75 | 
 76 | -----
 77 | 
 78 | ### Ubuntu
 79 | * Install [ROS Melodic and MoveIt](https://moveit.ros.org/install/) for Ubuntu 18.04
 80 | * Create the ROS workspace `ie. ar3_ws`
 81 |   ```
 82 |   mkdir -p ~/ar3_ws/src && cd "$_"
 83 |   ```
 84 |   
 85 | * Clone this repository into the workspace `src`:
 86 |   ```
 87 |   git clone https://github.com/ongdexter/ar3_core.git .
 88 |   ```
 89 |   The workspace directory should be as such:
 90 |   ```
 91 |   ar_ws
 92 |   +-- src
 93 |   |   +-- ar3_control
 94 |   |   +-- ar3_description
 95 |   |   +-- ...
 96 |   ```
 97 | * Install workspace dependencies:
 98 |   ```
 99 |   cd ~/ar3_ws
100 |   rosdep install --from-paths src --ignore-src -r -y
101 |   ```
102 | * Build the workspace:
103 |   ```
104 |   catkin_make
105 |   ```
106 | * Source the workspace:
107 |   ```
108 |   source ~/ar3_ws/devel/setup.bash
109 |   ```
110 |   You can add this to your .bashrc so that it is automatically run each time a new terminal is opened:
111 |   ```
112 |   echo "source ~/ar3_ws/devel/setup.bash" >> ~/.bashrc
113 |   ```
114 | * Enable serial port access if you haven't already done so:
115 |   ```
116 |   sudo addgroup $USER dialout
117 |   ```
118 |   You will need to log out and back in for changes to take effect.
119 | 
120 | ## Setup
121 | * **Hardware interface**  
122 |   - Set the serial port and baudrate in `ar3_hardware_interface/config/hardware_driver.yaml`
123 |   
124 | * **Teensy Sketch**  
125 |   - Both teensy sketches provided are compatible with the default hardware. Refer to the module for more information.
126 | 
127 | ## Usage
128 | There are two modules that you will always need to run:
129 | 
130 | 1. **Arm module** - this can be for either a real-world or simulated arm
131 |      + For controlling the real-world arm, you will need to run the `ar3_hardware_interface` module 
132 |      + For the simulated arm, you will need to run the `ar3_gazebo` module
133 |      + Either of the modules will load the necessary hardware descriptions for MoveIt
134 | 
135 | 2. **MoveIt module** - the `ar3_moveit_config` module provides the MoveIt interface and RViz GUI, and the `ar3_control` module provides the MoveIt user interface for programmatically setting goals
136 | 
137 | The various use cases of the modules and instructions to run them are described below:
138 | 
139 | -----
140 | 
141 | ### MoveIt Demo in RViz
142 | If you are unfamiliar with MoveIt, it is recommended to start with this to explore planning with MoveIt in RViz. This contains neither a real-world nor a simulated arm but just a model loaded within RViz for visualisation.
143 | * The robot description, moveit interface and RViz will all be loaded in the single demo launch file
144 |   ```
145 |   roslaunch ar3_moveit_config demo.launch
146 |   ```
147 | 
148 | -----  
149 | 
150 | ### Control real-world arm with MoveIt in RViz
151 | 
152 | * Start the `ar3_hardware_interface` module, which will load configs and the robot description
153 |   ```
154 |   roslaunch ar3_hardware_interface ar3_hardware_bringup.launch
155 |   ```
156 |   The hardware interface will also start the hardware driver and initialise communication with the Teensy. You can skip the joint encoder calibration sequence with the `use_existing_calibrations` argument when starting the node  
157 |   ie. `roslaunch ar3_hardware_interface ar3_hardware_bringup.launch use_existing_calibrations:=true`.
158 |   
159 |   
160 | * Start MoveIt and RViz
161 |   ```
162 |   roslaunch ar3_moveit_config ar3_moveit_bringup.launch
163 |   ```
164 | You can now plan in RViz and control the real-world arm. Joint commands and joint states will be updated through the hardware interface.
165 | 
166 | -----
167 | 
168 | ### Control simulated arm in Gazebo with MoveIt in RViz
169 | 
170 | * Start the `ar3_gazebo` module, which will start the Gazebo simulator and load the robot description
171 |   ```
172 |   roslaunch ar3_gazebo ar3_gazebo_bringup.launch
173 |   ```
174 | * Start Moveit and RViz
175 |   ```
176 |   roslaunch ar3_moveit_config ar3_moveit_bringup.launch
177 |   ```
178 | You can now plan in RViz and control the simulated arm.
179 | 
180 | -----
181 | ### Control arm with Move Group Interface
182 | 
183 | **It is recommended to run this demo with the simulated arm first to make sure that the programmed goals are safe for your environment (and your arm). Needless to say, the same applies when programming your own tasks.**
184 | 
185 | This is a demo modified from the official MoveIt tutorials. As opposed to manually setting goals through RViz, the move group interface allows us to programmatically plan and execute movements, and provides more functionality such as specifying path constraints and planning Cartesian movements. This also enables much more complex tasks, planning around obstacles etc.
186 | 
187 | * Start the `ar3_gazebo` module, which will start the Gazebo simulator and load the robot description  
188 | *For controlling the real-world arm, you will just need to run `ar3_hardware_interface` instead of `ar3_gazebo` as described above.*
189 |   ```
190 |   roslaunch ar3_gazebo ar3_gazebo_bringup.launch
191 |   ```
192 | * Start Moveit and RViz
193 |   ```
194 |   roslaunch ar3_moveit_config ar3_moveit_bringup.launch
195 |   ```
196 | * Start the move group demo
197 |   ```
198 |   roslaunch ar3_control move_group_demo.launch
199 |   ```
200 |   Follow the command-line instructions to step through the demo. See `ar3_control` for more details.
201 | 
202 | -----


--------------------------------------------------------------------------------
/ar3_moveit_config/launch/moveit.rviz:
--------------------------------------------------------------------------------
  1 | Panels:
  2 |   - Class: rviz/Displays
  3 |     Help Height: 84
  4 |     Name: Displays
  5 |     Property Tree Widget:
  6 |       Expanded:
  7 |         - /MotionPlanning1
  8 |         - /MotionPlanning1/Planned Path1
  9 |       Splitter Ratio: 0.7425600290298462
 10 |     Tree Height: 353
 11 |   - Class: rviz/Help
 12 |     Name: Help
 13 |   - Class: rviz/Views
 14 |     Expanded:
 15 |       - /Current View1
 16 |     Name: Views
 17 |     Splitter Ratio: 0.5
 18 | Preferences:
 19 |   PromptSaveOnExit: true
 20 | Toolbars:
 21 |   toolButtonStyle: 2
 22 | Visualization Manager:
 23 |   Class: ""
 24 |   Displays:
 25 |     - Alpha: 0.5
 26 |       Cell Size: 1
 27 |       Class: rviz/Grid
 28 |       Color: 160; 160; 164
 29 |       Enabled: true
 30 |       Line Style:
 31 |         Line Width: 0.029999999329447746
 32 |         Value: Lines
 33 |       Name: Grid
 34 |       Normal Cell Count: 0
 35 |       Offset:
 36 |         X: 0
 37 |         Y: 0
 38 |         Z: 0
 39 |       Plane: XY
 40 |       Plane Cell Count: 10
 41 |       Reference Frame: <Fixed Frame>
 42 |       Value: true
 43 |     - Acceleration_Scaling_Factor: 1
 44 |       Class: moveit_rviz_plugin/MotionPlanning
 45 |       Enabled: true
 46 |       Move Group Namespace: ""
 47 |       MoveIt_Allow_Approximate_IK: false
 48 |       MoveIt_Allow_External_Program: false
 49 |       MoveIt_Allow_Replanning: false
 50 |       MoveIt_Allow_Sensor_Positioning: false
 51 |       MoveIt_Goal_Tolerance: 0
 52 |       MoveIt_Planning_Attempts: 10
 53 |       MoveIt_Planning_Time: 5
 54 |       MoveIt_Use_Cartesian_Path: false
 55 |       MoveIt_Use_Constraint_Aware_IK: true
 56 |       MoveIt_Warehouse_Host: 127.0.0.1
 57 |       MoveIt_Warehouse_Port: 33829
 58 |       MoveIt_Workspace:
 59 |         Center:
 60 |           X: 0
 61 |           Y: 0
 62 |           Z: 0
 63 |         Size:
 64 |           X: 2
 65 |           Y: 2
 66 |           Z: 2
 67 |       Name: MotionPlanning
 68 |       Planned Path:
 69 |         Color Enabled: false
 70 |         Interrupt Display: false
 71 |         Links:
 72 |           All Links Enabled: true
 73 |           Expand Joint Details: false
 74 |           Expand Link Details: false
 75 |           Expand Tree: false
 76 |           Link Tree Style: Links in Alphabetic Order
 77 |           base_link:
 78 |             Alpha: 1
 79 |             Show Axes: false
 80 |             Show Trail: false
 81 |             Value: true
 82 |           link_1:
 83 |             Alpha: 1
 84 |             Show Axes: false
 85 |             Show Trail: false
 86 |             Value: true
 87 |           link_2:
 88 |             Alpha: 1
 89 |             Show Axes: false
 90 |             Show Trail: false
 91 |             Value: true
 92 |           link_3:
 93 |             Alpha: 1
 94 |             Show Axes: false
 95 |             Show Trail: false
 96 |             Value: true
 97 |           link_4:
 98 |             Alpha: 1
 99 |             Show Axes: false
100 |             Show Trail: false
101 |             Value: true
102 |           link_5:
103 |             Alpha: 1
104 |             Show Axes: false
105 |             Show Trail: false
106 |             Value: true
107 |           link_6:
108 |             Alpha: 1
109 |             Show Axes: false
110 |             Show Trail: false
111 |             Value: true
112 |         Loop Animation: true
113 |         Robot Alpha: 0.5
114 |         Robot Color: 150; 50; 150
115 |         Show Robot Collision: false
116 |         Show Robot Visual: false
117 |         Show Trail: false
118 |         State Display Time: 0.05 s
119 |         Trail Step Size: 1
120 |         Trajectory Topic: move_group/display_planned_path
121 |       Planning Metrics:
122 |         Payload: 1
123 |         Show Joint Torques: false
124 |         Show Manipulability: false
125 |         Show Manipulability Index: false
126 |         Show Weight Limit: false
127 |         TextHeight: 0.07999999821186066
128 |       Planning Request:
129 |         Colliding Link Color: 255; 0; 0
130 |         Goal State Alpha: 1
131 |         Goal State Color: 250; 128; 0
132 |         Interactive Marker Size: 0
133 |         Joint Violation Color: 255; 0; 255
134 |         Planning Group: manipulator
135 |         Query Goal State: true
136 |         Query Start State: false
137 |         Show Workspace: false
138 |         Start State Alpha: 1
139 |         Start State Color: 0; 255; 0
140 |       Planning Scene Topic: move_group/monitored_planning_scene
141 |       Robot Description: robot_description
142 |       Scene Geometry:
143 |         Scene Alpha: 1
144 |         Scene Color: 50; 230; 50
145 |         Scene Display Time: 0.20000000298023224
146 |         Show Scene Geometry: true
147 |         Voxel Coloring: Z-Axis
148 |         Voxel Rendering: Occupied Voxels
149 |       Scene Robot:
150 |         Attached Body Color: 150; 50; 150
151 |         Links:
152 |           All Links Enabled: true
153 |           Expand Joint Details: false
154 |           Expand Link Details: false
155 |           Expand Tree: false
156 |           Link Tree Style: Links in Alphabetic Order
157 |           base_link:
158 |             Alpha: 1
159 |             Show Axes: false
160 |             Show Trail: false
161 |             Value: true
162 |           link_1:
163 |             Alpha: 1
164 |             Show Axes: false
165 |             Show Trail: false
166 |             Value: true
167 |           link_2:
168 |             Alpha: 1
169 |             Show Axes: false
170 |             Show Trail: false
171 |             Value: true
172 |           link_3:
173 |             Alpha: 1
174 |             Show Axes: false
175 |             Show Trail: false
176 |             Value: true
177 |           link_4:
178 |             Alpha: 1
179 |             Show Axes: false
180 |             Show Trail: false
181 |             Value: true
182 |           link_5:
183 |             Alpha: 1
184 |             Show Axes: false
185 |             Show Trail: false
186 |             Value: true
187 |           link_6:
188 |             Alpha: 1
189 |             Show Axes: false
190 |             Show Trail: false
191 |             Value: true
192 |         Robot Alpha: 0.5
193 |         Show Robot Collision: false
194 |         Show Robot Visual: true
195 |       Value: true
196 |       Velocity_Scaling_Factor: 1
197 |   Enabled: true
198 |   Global Options:
199 |     Background Color: 48; 48; 48
200 |     Default Light: true
201 |     Fixed Frame: world
202 |     Frame Rate: 30
203 |   Name: root
204 |   Tools:
205 |     - Class: rviz/Interact
206 |       Hide Inactive Objects: true
207 |     - Class: rviz/MoveCamera
208 |     - Class: rviz/Select
209 |   Value: true
210 |   Views:
211 |     Current:
212 |       Class: rviz/XYOrbit
213 |       Distance: 1.5685498714447021
214 |       Enable Stereo Rendering:
215 |         Stereo Eye Separation: 0.05999999865889549
216 |         Stereo Focal Distance: 1
217 |         Swap Stereo Eyes: false
218 |         Value: false
219 |       Focal Point:
220 |         X: -0.08989071100950241
221 |         Y: 0.15271839499473572
222 |         Z: 3.427272758926847e-7
223 |       Focal Shape Fixed Size: true
224 |       Focal Shape Size: 0.05000000074505806
225 |       Invert Z Axis: false
226 |       Name: Current View
227 |       Near Clip Distance: 0.009999999776482582
228 |       Pitch: -0.00020243600010871887
229 |       Target Frame: world
230 |       Value: XYOrbit (rviz)
231 |       Yaw: 4.624979019165039
232 |     Saved: ~
233 | Window Geometry:
234 |   Displays:
235 |     collapsed: false
236 |   Height: 1057
237 |   Help:
238 |     collapsed: false
239 |   Hide Left Dock: false
240 |   Hide Right Dock: false
241 |   MotionPlanning:
242 |     collapsed: false
243 |   MotionPlanning - Trajectory Slider:
244 |     collapsed: false
245 |   QMainWindow State: 000000ff00000000fd0000000100000000000002a2000003d0fc0200000007fb000000100044006900730070006c0061007900730100000037000001f1000000ca00fffffffb0000000800480065006c00700000000342000000bb0000006f00fffffffb0000000a0056006900650077007300000003b0000000b0000000a100fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004300fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000022c000001db0000015400ffffff000004ba000003d000000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
246 |   Views:
247 |     collapsed: false
248 |   Width: 1888
249 |   X: 24
250 |   Y: -8
251 | 


--------------------------------------------------------------------------------
/ar3_hardware_drivers/src/TeensyDriver.cpp:
--------------------------------------------------------------------------------
  1 | #include "ar3_hardware_drivers/TeensyDriver.h"
  2 | 
  3 | #include <thread>
  4 | #include <chrono>
  5 | 
  6 | #define FW_VERSION "0.0.1"
  7 | 
  8 | namespace ar3_hardware_drivers {
  9 | 
 10 | void TeensyDriver::init(std::string port, int baudrate, int num_joints, std::vector<double>& enc_steps_per_deg)
 11 | {
 12 |     // @TODO read version from config
 13 |     version_ = FW_VERSION;
 14 | 
 15 |     // establish connection with teensy board
 16 |     boost::system::error_code ec;
 17 |     serial_port_.open(port, ec);
 18 | 
 19 |     if (ec)
 20 |     {
 21 |         ROS_WARN("Failed to connect to serial port %s", port.c_str());
 22 |         return;
 23 |     }
 24 |     else
 25 |     {
 26 |         serial_port_.set_option(boost::asio::serial_port_base::baud_rate(static_cast<uint32_t>(baudrate)));
 27 |         serial_port_.set_option(boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none));
 28 |         ROS_INFO("Successfully connected to serial port %s", port.c_str());
 29 |     }
 30 |     
 31 |     initialised_ = false;
 32 |     std::string msg = "STA" + version_ + "\n";
 33 | 
 34 |     while (!initialised_)
 35 |     {
 36 |         ROS_INFO("Waiting for response from Teensy on port %s", port.c_str());
 37 |         std::this_thread::sleep_for(std::chrono::milliseconds(1000));
 38 |         exchange(msg);
 39 |     }
 40 |     ROS_INFO("Successfully initialised driver on port %s", port.c_str());
 41 | 
 42 |     // initialise joint and encoder calibration
 43 |     num_joints_ = num_joints;
 44 |     enc_commands_.resize(num_joints_);
 45 |     enc_steps_.resize(num_joints_);
 46 |     enc_steps_per_deg_.resize(num_joints_);
 47 |     enc_calibrations_.resize(num_joints_);
 48 |     for (int i = 0; i < num_joints_; ++i)
 49 |     {
 50 |         enc_steps_per_deg_[i] = enc_steps_per_deg[i];
 51 |     }
 52 | 
 53 |     // get current joint positions
 54 |     msg = "JP\n";
 55 |     exchange(msg);
 56 |     for (int i = 0; i < num_joints_; ++i)
 57 |     {
 58 |         enc_commands_[i] = enc_steps_[i];
 59 |     }
 60 | }
 61 | 
 62 | TeensyDriver::TeensyDriver()
 63 | : serial_port_(io_service_)
 64 | {
 65 | }
 66 | 
 67 | void TeensyDriver::setStepperSpeed(std::vector<double>& max_speed, std::vector<double>& max_accel)
 68 | {
 69 |     std::string outMsg = "SS";
 70 |     for (int i = 0, charIdx = 0; i < num_joints_; ++i, charIdx += 2)
 71 |     {
 72 |         outMsg += 'A' + charIdx;
 73 |         outMsg += std::to_string(max_speed[i]);
 74 |         outMsg += 'A' + charIdx + 1;
 75 |         outMsg += std::to_string(max_accel[i]);
 76 |     }
 77 |     outMsg += "\n";
 78 |     exchange(outMsg);
 79 | }
 80 | 
 81 | // Update between hardware interface and hardware driver
 82 | void TeensyDriver::update(std::vector<double>& pos_commands, std::vector<double>& joint_positions)
 83 | {
 84 |     // get updated position commands
 85 |     jointPosToEncSteps(pos_commands, enc_commands_);
 86 | 
 87 |     // construct update message
 88 |     std::string outMsg = "MT";
 89 |     for (int i = 0; i < num_joints_; ++i)
 90 |     {
 91 |         outMsg += 'A' + i;
 92 |         outMsg += std::to_string(enc_commands_[i]);
 93 |     }
 94 |     outMsg += "\n";
 95 | 
 96 |     // run the communication with board
 97 |     exchange(outMsg);
 98 | 
 99 |     // return updated joint_positions
100 |     encStepsToJointPos(enc_steps_ , joint_positions);
101 | }
102 | 
103 | void TeensyDriver::calibrateJoints()
104 | {
105 |     std::string outMsg = "JC\n";
106 |     sendCommand(outMsg);
107 | }
108 | 
109 | void TeensyDriver::getJointPositions(std::vector<double>& joint_positions)
110 | {
111 |     // get current joint positions
112 |     std::string msg = "JP\n";
113 |     exchange(msg);
114 |     encStepsToJointPos(enc_steps_, joint_positions);
115 | }
116 | 
117 | // Send specific commands
118 | void TeensyDriver::sendCommand(std::string outMsg)
119 | {
120 |     exchange(outMsg);
121 | }
122 | 
123 | // Send msg to board and collect data
124 | void TeensyDriver::exchange(std::string outMsg)
125 | {
126 |     std::string inMsg;
127 |     std::string errTransmit = "";
128 |     std::string errReceive = "";
129 |     
130 |     if (!transmit(outMsg, errTransmit))
131 |     {
132 |         // print err
133 |     }
134 | 
135 |     if (!receive(inMsg, errReceive))
136 |     {
137 |         // print err
138 |     }
139 |     // parse msg
140 |     std::string header = inMsg.substr(0, 2);
141 |     if (header == "ST")
142 |     {
143 |         // init acknowledgement
144 |         checkInit(inMsg);   
145 |     }
146 |     else if (header == "JC")
147 |     {
148 |         // encoder calibration values
149 |         updateEncoderCalibrations(inMsg);
150 |     }
151 |     else if (header == "JP")
152 |     {
153 |         // encoder steps
154 |         updateEncoderSteps(inMsg);
155 |     }
156 |     else
157 |     {
158 |         // unknown header
159 |         ROS_WARN("Unknown header %s", header);
160 |     } 
161 | }
162 | 
163 | bool TeensyDriver::transmit(std::string msg, std::string& err)
164 | {
165 |     boost::system::error_code ec;
166 |     const auto sendBuffer = boost::asio::buffer(msg.c_str(), msg.size());
167 | 
168 |     boost::asio::write(serial_port_, sendBuffer, ec);
169 | 
170 |     if(!ec)
171 |     {
172 |         return true;
173 |     }
174 |     else
175 |     {
176 |         err = "Error in transmit";
177 |         return false;
178 |     }    
179 | }
180 | 
181 | bool TeensyDriver::receive(std::string& inMsg, std::string& err)
182 | {
183 |     char c;
184 |     std::string msg = "";
185 |     bool eol = false;
186 |     while (!eol)
187 |     {
188 |         boost::asio::read(serial_port_, boost::asio::buffer(&c, 1));
189 |         switch(c)
190 |         {
191 |             case '\r':
192 |                 break;
193 |             case '\n':
194 |                 eol = true;
195 |             default:
196 |                 msg += c;
197 |         }
198 |     }
199 |     inMsg = msg;
200 |     return true;
201 | }
202 | 
203 | void TeensyDriver::checkInit(std::string msg)
204 | {
205 |     std::size_t ack_idx = msg.find("A", 2) + 1;
206 |     std::size_t version_idx = msg.find("B", 2) + 1;
207 |     int ack = std::stoi(msg.substr(ack_idx, version_idx));
208 |     if (ack)
209 |     {
210 |         initialised_ = true;
211 |     }
212 |     else
213 |     {
214 |         std::string version = msg.substr(version_idx);
215 |         ROS_ERROR("Firmware version mismatch %s", version);
216 |     }  
217 | }
218 | 
219 | void TeensyDriver::updateEncoderCalibrations(std::string msg)
220 | {
221 |     size_t idx1 = msg.find("A", 2) + 1;
222 |     size_t idx2 = msg.find("B", 2) + 1;
223 |     size_t idx3 = msg.find("C", 2) + 1;
224 |     size_t idx4 = msg.find("D", 2) + 1;
225 |     size_t idx5 = msg.find("E", 2) + 1;
226 |     size_t idx6 = msg.find("F", 2) + 1;
227 |     enc_calibrations_[0] = std::stoi(msg.substr(idx1, idx2 - idx1));
228 |     enc_calibrations_[1] = std::stoi(msg.substr(idx2, idx3 - idx2));
229 |     enc_calibrations_[2] = std::stoi(msg.substr(idx3, idx4 - idx3));
230 |     enc_calibrations_[3] = std::stoi(msg.substr(idx4, idx5 - idx4));
231 |     enc_calibrations_[4] = std::stoi(msg.substr(idx5, idx6 - idx5));
232 |     enc_calibrations_[5] = std::stoi(msg.substr(idx6));
233 | 
234 |     // @TODO update config file
235 |     ROS_INFO("Successfully updated encoder calibrations");
236 | }
237 | 
238 | void TeensyDriver::updateEncoderSteps(std::string msg)
239 | {
240 |     size_t idx1 = msg.find("A", 2) + 1;
241 |     size_t idx2 = msg.find("B", 2) + 1;
242 |     size_t idx3 = msg.find("C", 2) + 1;
243 |     size_t idx4 = msg.find("D", 2) + 1;
244 |     size_t idx5 = msg.find("E", 2) + 1;
245 |     size_t idx6 = msg.find("F", 2) + 1;
246 |     enc_steps_[0] = std::stoi(msg.substr(idx1, idx2 - idx1));
247 |     enc_steps_[1] = std::stoi(msg.substr(idx2, idx3 - idx2));
248 |     enc_steps_[2] = std::stoi(msg.substr(idx3, idx4 - idx3));
249 |     enc_steps_[3] = std::stoi(msg.substr(idx4, idx5 - idx4));
250 |     enc_steps_[4] = std::stoi(msg.substr(idx5, idx6 - idx5));
251 |     enc_steps_[5] = std::stoi(msg.substr(idx6));
252 | }
253 | 
254 | void TeensyDriver::encStepsToJointPos(std::vector<int>& enc_steps, std::vector<double>& joint_positions)
255 | {
256 |     for (int i = 0; i < enc_steps.size(); ++i)
257 |     {
258 |         // convert enc steps to joint deg
259 |         joint_positions[i] = static_cast<double>(enc_steps[i]) / enc_steps_per_deg_[i];
260 |     }
261 | }
262 | 
263 | void TeensyDriver::jointPosToEncSteps(std::vector<double>& joint_positions, std::vector<int>& enc_steps)
264 | {
265 |     for (int i = 0; i < joint_positions.size(); ++i)
266 |     {
267 |         // convert joint deg to enc steps
268 |         enc_steps[i] = static_cast<int>(joint_positions[i] * enc_steps_per_deg_[i]);
269 |     }
270 | }
271 | 
272 | } // namespace ar3_hardware_drivers
273 | 


--------------------------------------------------------------------------------
/ar3_description/urdf/ar3.urdf:
--------------------------------------------------------------------------------
  1 | <robot
  2 |   name="ar3">
  3 |   <link
  4 |     name="base_link">
  5 |     <inertial>
  6 |       <origin
  7 |         xyz="-4.6941E-06 0.054174 0.038824"
  8 |         rpy="0 0 0" />
  9 |       <mass
 10 |         value="0.7102" />
 11 |       <inertia
 12 |         ixx="0.0039943"
 13 |         ixy="3.697E-07"
 14 |         ixz="-5.7364E-08"
 15 |         iyy="0.0014946"
 16 |         iyz="-0.00036051"
 17 |         izz="0.0042554" />
 18 |     </inertial>
 19 |     <visual>
 20 |       <origin
 21 |         xyz="0 0 0"
 22 |         rpy="0 0 0" />
 23 |       <geometry>
 24 |         <mesh
 25 |           filename="package://ar3_description/meshes/base_link.STL" />
 26 |       </geometry>
 27 |       <material
 28 |         name="">
 29 |         <color
 30 |           rgba="1 1 0 1" />
 31 |       </material>
 32 |     </visual>
 33 |     <collision>
 34 |       <origin
 35 |         xyz="0 0 0"
 36 |         rpy="0 0 0" />
 37 |       <geometry>
 38 |         <mesh
 39 |           filename="package://ar3_description/meshes/base_link.STL" />
 40 |       </geometry>
 41 |     </collision>
 42 |   </link>
 43 |   <link
 44 |     name="link_1">
 45 |     <inertial>
 46 |       <origin
 47 |         xyz="-0.022706 0.04294 -0.12205"
 48 |         rpy="0 0 0" />
 49 |       <mass
 50 |         value="0.88065" />
 51 |       <inertia
 52 |         ixx="0.0034"
 53 |         ixy="0.00042296"
 54 |         ixz="-0.00089231"
 55 |         iyy="0.0041778"
 56 |         iyz="0.0010848"
 57 |         izz="0.0027077" />
 58 |     </inertial>
 59 |     <visual>
 60 |       <origin
 61 |         xyz="0 0 0"
 62 |         rpy="0 0 0" />
 63 |       <geometry>
 64 |         <mesh
 65 |           filename="package://ar3_description/meshes/link_1.STL" />
 66 |       </geometry>
 67 |       <material
 68 |         name="">
 69 |         <color
 70 |           rgba="1 1 0 1" />
 71 |       </material>
 72 |     </visual>
 73 |     <collision>
 74 |       <origin
 75 |         xyz="0 0 0"
 76 |         rpy="0 0 0" />
 77 |       <geometry>
 78 |         <mesh
 79 |           filename="package://ar3_description/meshes/link_1.STL" />
 80 |       </geometry>
 81 |     </collision>
 82 |   </link>
 83 |   <joint
 84 |     name="joint_1"
 85 |     type="continuous">
 86 |     <origin
 87 |       xyz="0 0 0.003445"
 88 |       rpy="3.1416 -1.0248E-21 -1.6736E-05" />
 89 |     <parent
 90 |       link="base_link" />
 91 |     <child
 92 |       link="link_1" />
 93 |     <axis
 94 |       xyz="0 0 1" />
 95 |   </joint>
 96 |   <link
 97 |     name="link_2">
 98 |     <inertial>
 99 |       <origin
100 |         xyz="0.064818 -0.11189 -0.038671"
101 |         rpy="0 0 0" />
102 |       <mass
103 |         value="0.57738" />
104 |       <inertia
105 |         ixx="0.0047312"
106 |         ixy="0.0022624"
107 |         ixz="0.00032144"
108 |         iyy="0.0020836"
109 |         iyz="-0.00056569"
110 |         izz="0.0056129" />
111 |     </inertial>
112 |     <visual>
113 |       <origin
114 |         xyz="0 0 0"
115 |         rpy="0 0 0" />
116 |       <geometry>
117 |         <mesh
118 |           filename="package://ar3_description/meshes/link_2.STL" />
119 |       </geometry>
120 |       <material
121 |         name="">
122 |         <color
123 |           rgba="1 1 0 1" />
124 |       </material>
125 |     </visual>
126 |     <collision>
127 |       <origin
128 |         xyz="0 0 0"
129 |         rpy="0 0 0" />
130 |       <geometry>
131 |         <mesh
132 |           filename="package://ar3_description/meshes/link_2.STL" />
133 |       </geometry>
134 |     </collision>
135 |   </link>
136 |   <joint
137 |     name="joint_2"
138 |     type="continuous">
139 |     <origin
140 |       xyz="0 0.064146 -0.16608"
141 |       rpy="1.5708 0.5236 -1.5708" />
142 |     <parent
143 |       link="link_1" />
144 |     <child
145 |       link="link_2" />
146 |     <axis
147 |       xyz="0 0 -1" />
148 |   </joint>
149 |   <link
150 |     name="link_3">
151 |     <inertial>
152 |       <origin
153 |         xyz="-0.00029765 -0.023661 -1.04720367321"
154 |         rpy="0 0 0" />
155 |       <mass
156 |         value="0.1787" />
157 |       <inertia
158 |         ixx="0.0001685"
159 |         ixy="-2.7713E-05"
160 |         ixz="5.6885E-06"
161 |         iyy="0.00012865"
162 |         iyz="2.9256E-05"
163 |         izz="0.00020744" />
164 |     </inertial>
165 |     <visual>
166 |       <origin
167 |         xyz="0 0 0"
168 |         rpy="0 0 0" />
169 |       <geometry>
170 |         <mesh
171 |           filename="package://ar3_description/meshes/link_3.STL" />
172 |       </geometry>
173 |       <material
174 |         name="">
175 |         <color
176 |           rgba="1 1 0 1" />
177 |       </material>
178 |     </visual>
179 |     <collision>
180 |       <origin
181 |         xyz="0 0 0"
182 |         rpy="0 0 0" />
183 |       <geometry>
184 |         <mesh
185 |           filename="package://ar3_description/meshes/link_3.STL" />
186 |       </geometry>
187 |     </collision>
188 |   </link>
189 |   <joint
190 |     name="joint_3"
191 |     type="continuous">
192 |     <origin
193 |       xyz="0.1525 -0.26414 0"
194 |       rpy="3.3431E-16 -1.4164E-16 -1.4953816339" />
195 |     <parent
196 |       link="link_2" />
197 |     <child
198 |       link="link_3" />
199 |     <axis
200 |       xyz="0 0 -1" />
201 |   </joint>
202 |   <link
203 |     name="link_4">
204 |     <inertial>
205 |       <origin
206 |         xyz="-0.0016798 -0.00057319 -0.074404"
207 |         rpy="0 0 0" />
208 |       <mass
209 |         value="0.34936" />
210 |       <inertia
211 |         ixx="0.0030532"
212 |         ixy="-1.8615E-05"
213 |         ixz="-7.0047E-05"
214 |         iyy="0.0031033"
215 |         iyz="-2.3301E-05"
216 |         izz="0.00022264" />
217 |     </inertial>
218 |     <visual>
219 |       <origin
220 |         xyz="0 0 0"
221 |         rpy="0 0 0" />
222 |       <geometry>
223 |         <mesh
224 |           filename="package://ar3_description/meshes/link_4.STL" />
225 |       </geometry>
226 |       <material
227 |         name="">
228 |         <color
229 |           rgba="1 1 0 1" />
230 |       </material>
231 |     </visual>
232 |     <collision>
233 |       <origin
234 |         xyz="0 0 0"
235 |         rpy="0 0 0" />
236 |       <geometry>
237 |         <mesh
238 |           filename="package://ar3_description/meshes/link_4.STL" />
239 |       </geometry>
240 |     </collision>
241 |   </link>
242 |   <joint
243 |     name="joint_4"
244 |     type="continuous">
245 |     <origin
246 |       xyz="0 0 0.00675"
247 |       rpy="1.5708 -1.2554 -1.5708" />
248 |     <parent
249 |       link="link_3" />
250 |     <child
251 |       link="link_4" />
252 |     <axis
253 |       xyz="0 0 -1" />
254 |   </joint>
255 |   <link
256 |     name="link_5">
257 |     <inertial>
258 |       <origin
259 |         xyz="0.0015066 -1.3102E-05 -0.012585"
260 |         rpy="0 0 0" />
261 |       <mass
262 |         value="0.11562" />
263 |       <inertia
264 |         ixx="5.5035E-05"
265 |         ixy="-1.019E-08"
266 |         ixz="-2.6243E-06"
267 |         iyy="8.2921E-05"
268 |         iyz="1.4437E-08"
269 |         izz="5.2518E-05" />
270 |     </inertial>
271 |     <visual>
272 |       <origin
273 |         xyz="0 0 0"
274 |         rpy="0 0 0" />
275 |       <geometry>
276 |         <mesh
277 |           filename="package://ar3_description/meshes/link_5.STL" />
278 |       </geometry>
279 |       <material
280 |         name="">
281 |         <color
282 |           rgba="1 1 0 1" />
283 |       </material>
284 |     </visual>
285 |     <collision>
286 |       <origin
287 |         xyz="0 0 0"
288 |         rpy="0 0 0" />
289 |       <geometry>
290 |         <mesh
291 |           filename="package://ar3_description/meshes/link_5.STL" />
292 |       </geometry>
293 |     </collision>
294 |   </link>
295 |   <joint
296 |     name="joint_5"
297 |     type="continuous">
298 |     <origin
299 |       xyz="0 0 -0.22225"
300 |       rpy="3.1416 0 -2.8262" />
301 |     <parent
302 |       link="link_4" />
303 |     <child
304 |       link="link_5" />
305 |     <axis
306 |       xyz="-1 0 0" />
307 |   </joint>
308 |   <link
309 |     name="link_6">
310 |     <inertial>
311 |       <origin
312 |         xyz="2.9287E-10 -1.6472E-09 0.0091432"
313 |         rpy="0 0 0" />
314 |       <mass
315 |         value="0.013863" />
316 |       <inertia
317 |         ixx="1.3596E-06"
318 |         ixy="3.0585E-13"
319 |         ixz="5.7102E-14"
320 |         iyy="1.7157E-06"
321 |         iyz="6.3369E-09"
322 |         izz="2.4332E-06" />
323 |     </inertial>
324 |     <visual>
325 |       <origin
326 |         xyz="0 0 0"
327 |         rpy="0 0 0" />
328 |       <geometry>
329 |         <mesh
330 |           filename="package://ar3_description/meshes/link_6.STL" />
331 |       </geometry>
332 |       <material
333 |         name="">
334 |         <color
335 |           rgba="1 1 0 1" />
336 |       </material>
337 |     </visual>
338 |     <collision>
339 |       <origin
340 |         xyz="0 0 0"
341 |         rpy="0 0 0" />
342 |       <geometry>
343 |         <mesh
344 |           filename="package://ar3_description/meshes/link_6.STL" />
345 |       </geometry>
346 |     </collision>
347 |   </link>
348 |   <joint
349 |     name="joint_6"
350 |     type="continuous">
351 |     <origin
352 |       xyz="-0.000294 0 0.02117"
353 |       rpy="0 0 3.1416" />
354 |     <parent
355 |       link="link_5" />
356 |     <child
357 |       link="link_6" />
358 |     <axis
359 |       xyz="0 0 1" />
360 |   </joint>
361 | </robot>


--------------------------------------------------------------------------------
/ar3_moveit_config/config/ompl_planning.yaml:
--------------------------------------------------------------------------------
  1 | planner_configs:
  2 |   SBLkConfigDefault:
  3 |     type: geometric::SBL
  4 |     range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
  5 |   ESTkConfigDefault:
  6 |     type: geometric::EST
  7 |     range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup()
  8 |     goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
  9 |   LBKPIECEkConfigDefault:
 10 |     type: geometric::LBKPIECE
 11 |     range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
 12 |     border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9
 13 |     min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
 14 |   BKPIECEkConfigDefault:
 15 |     type: geometric::BKPIECE
 16 |     range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
 17 |     border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9
 18 |     failed_expansion_score_factor: 0.5  # When extending motion fails, scale score by factor. default: 0.5
 19 |     min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
 20 |   KPIECEkConfigDefault:
 21 |     type: geometric::KPIECE
 22 |     range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
 23 |     goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05 
 24 |     border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9 (0.0,1.]
 25 |     failed_expansion_score_factor: 0.5  # When extending motion fails, scale score by factor. default: 0.5
 26 |     min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
 27 |   RRTkConfigDefault:
 28 |     type: geometric::RRT
 29 |     range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
 30 |     goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
 31 |   RRTConnectkConfigDefault:
 32 |     type: geometric::RRTConnect
 33 |     range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
 34 |   RRTstarkConfigDefault:
 35 |     type: geometric::RRTstar
 36 |     range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
 37 |     goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
 38 |     delay_collision_checking: 1  # Stop collision checking as soon as C-free parent found. default 1
 39 |   TRRTkConfigDefault:
 40 |     type: geometric::TRRT
 41 |     range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
 42 |     goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
 43 |     max_states_failed: 10  # when to start increasing temp. default: 10
 44 |     temp_change_factor: 2.0  # how much to increase or decrease temp. default: 2.0
 45 |     min_temperature: 10e-10  # lower limit of temp change. default: 10e-10
 46 |     init_temperature: 10e-6  # initial temperature. default: 10e-6
 47 |     frountier_threshold: 0.0  # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() 
 48 |     frountierNodeRatio: 0.1  # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
 49 |     k_constant: 0.0  # value used to normalize expresssion. default: 0.0 set in setup()
 50 |   PRMkConfigDefault:
 51 |     type: geometric::PRM
 52 |     max_nearest_neighbors: 10  # use k nearest neighbors. default: 10
 53 |   PRMstarkConfigDefault:
 54 |     type: geometric::PRMstar
 55 |   FMTkConfigDefault:
 56 |     type: geometric::FMT
 57 |     num_samples: 1000  # number of states that the planner should sample. default: 1000
 58 |     radius_multiplier: 1.1  # multiplier used for the nearest neighbors search radius. default: 1.1
 59 |     nearest_k: 1  # use Knearest strategy. default: 1
 60 |     cache_cc: 1  # use collision checking cache. default: 1
 61 |     heuristics: 0  # activate cost to go heuristics. default: 0
 62 |     extended_fmt: 1  # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1
 63 |   BFMTkConfigDefault:
 64 |     type: geometric::BFMT
 65 |     num_samples: 1000  # number of states that the planner should sample. default: 1000
 66 |     radius_multiplier: 1.0  # multiplier used for the nearest neighbors search radius. default: 1.0
 67 |     nearest_k: 1  # use the Knearest strategy. default: 1
 68 |     balanced: 0  # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1
 69 |     optimality: 1  # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1
 70 |     heuristics: 1  # activates cost to go heuristics. default: 1
 71 |     cache_cc: 1  # use the collision checking cache. default: 1
 72 |     extended_fmt: 1  # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1
 73 |   PDSTkConfigDefault:
 74 |     type: geometric::PDST
 75 |   STRIDEkConfigDefault:
 76 |     type: geometric::STRIDE
 77 |     range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
 78 |     goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05 
 79 |     use_projected_distance: 0  # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0
 80 |     degree: 16  # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 
 81 |     max_degree: 18  # max degree of a node in the GNAT. default: 12
 82 |     min_degree: 12  # min degree of a node in the GNAT. default: 12
 83 |     max_pts_per_leaf: 6  # max points per leaf in the GNAT. default: 6
 84 |     estimated_dimension: 0.0  # estimated dimension of the free space. default: 0.0
 85 |     min_valid_path_fraction: 0.2  # Accept partially valid moves above fraction. default: 0.2
 86 |   BiTRRTkConfigDefault:
 87 |     type: geometric::BiTRRT
 88 |     range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
 89 |     temp_change_factor: 0.1  # how much to increase or decrease temp. default: 0.1
 90 |     init_temperature: 100  # initial temperature. default: 100
 91 |     frountier_threshold: 0.0  # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() 
 92 |     frountier_node_ratio: 0.1  # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
 93 |     cost_threshold: 1e300  # the cost threshold. Any motion cost that is not better will not be expanded. default: inf
 94 |   LBTRRTkConfigDefault:
 95 |     type: geometric::LBTRRT
 96 |     range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
 97 |     goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05 
 98 |     epsilon: 0.4  # optimality approximation factor. default: 0.4
 99 |   BiESTkConfigDefault:
100 |     type: geometric::BiEST
101 |     range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
102 |   ProjESTkConfigDefault:
103 |     type: geometric::ProjEST
104 |     range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
105 |     goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05 
106 |   LazyPRMkConfigDefault:
107 |     type: geometric::LazyPRM
108 |     range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
109 |   LazyPRMstarkConfigDefault:
110 |     type: geometric::LazyPRMstar
111 |   SPARSkConfigDefault:
112 |     type: geometric::SPARS
113 |     stretch_factor: 3.0  # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
114 |     sparse_delta_fraction: 0.25  # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
115 |     dense_delta_fraction: 0.001  # delta fraction for interface detection. default: 0.001
116 |     max_failures: 1000  # maximum consecutive failure limit. default: 1000
117 |   SPARStwokConfigDefault:
118 |     type: geometric::SPARStwo
119 |     stretch_factor: 3.0  # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
120 |     sparse_delta_fraction: 0.25  # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
121 |     dense_delta_fraction: 0.001  # delta fraction for interface detection. default: 0.001
122 |     max_failures: 5000  # maximum consecutive failure limit. default: 5000
123 | manipulator:
124 |   planner_configs:
125 |     - SBLkConfigDefault
126 |     - ESTkConfigDefault
127 |     - LBKPIECEkConfigDefault
128 |     - BKPIECEkConfigDefault
129 |     - KPIECEkConfigDefault
130 |     - RRTkConfigDefault
131 |     - RRTConnectkConfigDefault
132 |     - RRTstarkConfigDefault
133 |     - TRRTkConfigDefault
134 |     - PRMkConfigDefault
135 |     - PRMstarkConfigDefault
136 |     - FMTkConfigDefault
137 |     - BFMTkConfigDefault
138 |     - PDSTkConfigDefault
139 |     - STRIDEkConfigDefault
140 |     - BiTRRTkConfigDefault
141 |     - LBTRRTkConfigDefault
142 |     - BiESTkConfigDefault
143 |     - ProjESTkConfigDefault
144 |     - LazyPRMkConfigDefault
145 |     - LazyPRMstarkConfigDefault
146 |     - SPARSkConfigDefault
147 |     - SPARStwokConfigDefault
148 |   projection_evaluator: joints(joint_1,joint_2)
149 |   longest_valid_segment_fraction: 0.005
150 | 


--------------------------------------------------------------------------------
/ar3_description/urdf/ar3_gazebo.urdf.xacro:
--------------------------------------------------------------------------------
  1 | <?xml version="1.0" encoding="utf-8"?>
  2 | <!-- =================================================================================== -->
  3 | <!-- |    This document was autogenerated by xacro from ar3.urdf                       | -->
  4 | <!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED but it was unfortunately        | -->
  5 | <!-- =================================================================================== -->
  6 | <robot name="ar3">
  7 |   <link name="world" />
  8 |   <link name="base_link">
  9 |     <inertial>
 10 |       <origin rpy="0 0 0" xyz="-4.6941E-06 0.054174 0.038824"/>
 11 |       <mass value="0.7102"/>
 12 |       <inertia ixx="0.0039943" ixy="3.697E-07" ixz="-5.7364E-08" iyy="0.0014946" iyz="-0.00036051" izz="0.0042554"/>
 13 |     </inertial>
 14 |     <visual>
 15 |       <origin rpy="0 0 0" xyz="0 0 0"/>
 16 |       <geometry>
 17 |         <mesh filename="package://ar3_description/meshes/base_link.STL"/>
 18 |       </geometry>
 19 |       <material name="">
 20 |         <color rgba="1 1 0 1"/>
 21 |       </material>
 22 |     </visual>
 23 |     <collision>
 24 |       <origin rpy="0 0 0" xyz="0 0 0"/>
 25 |       <geometry>
 26 |         <mesh filename="package://ar3_description/meshes/base_link.STL"/>
 27 |       </geometry>
 28 |     </collision>
 29 |   </link>
 30 |   <joint name="joint_world" type="fixed">
 31 |     <parent link="world"/>
 32 |     <child link="base_link"/>
 33 |     <origin xyz="0 0 0" rpy="0 0 0" />
 34 |   </joint>
 35 |   <link name="link_1">
 36 |     <inertial>
 37 |       <origin rpy="0 0 0" xyz="-0.022706 0.04294 -0.12205"/>
 38 |       <mass value="0.88065"/>
 39 |       <inertia ixx="0.0034" ixy="0.00042296" ixz="-0.00089231" iyy="0.0041778" iyz="0.0010848" izz="0.0027077"/>
 40 |     </inertial>
 41 |     <visual>
 42 |       <origin rpy="0 0 0" xyz="0 0 0"/>
 43 |       <geometry>
 44 |         <mesh filename="package://ar3_description/meshes/link_1.STL"/>
 45 |       </geometry>
 46 |       <material name="">
 47 |         <color rgba="1 1 0 1"/>
 48 |       </material>
 49 |     </visual>
 50 |     <collision>
 51 |       <origin rpy="0 0 0" xyz="0 0 0"/>
 52 |       <geometry>
 53 |         <mesh filename="package://ar3_description/meshes/link_1.STL"/>
 54 |       </geometry>
 55 |     </collision>
 56 |   </link>
 57 |   <joint name="joint_1" type="revolute">
 58 |     <origin rpy="${pi} 0 0" xyz="0 0 0.003445"/>
 59 |     <parent link="base_link"/>
 60 |     <child link="link_1"/>
 61 |     <axis xyz="0 0 1"/>
 62 |     <limit lower="-2.96706" upper="2.96706" effort="100" velocity="100"/>
 63 |   </joint>
 64 |   <link name="link_2">
 65 |     <inertial>
 66 |       <origin rpy="0 0 0" xyz="0.064818 -0.11189 -0.038671"/>
 67 |       <mass value="0.57738"/>
 68 |       <inertia ixx="0.0047312" ixy="0.0022624" ixz="0.00032144" iyy="0.0020836" iyz="-0.00056569" izz="0.0056129"/>
 69 |     </inertial>
 70 |     <visual>
 71 |       <origin rpy="0 0 0" xyz="0 0 0"/>
 72 |       <geometry>
 73 |         <mesh filename="package://ar3_description/meshes/link_2.STL"/>
 74 |       </geometry>
 75 |       <material name="">
 76 |         <color rgba="1 1 0 1"/>
 77 |       </material>
 78 |     </visual>
 79 |     <collision>
 80 |       <origin rpy="0 0 0" xyz="0 0 0"/>
 81 |       <geometry>
 82 |         <mesh filename="package://ar3_description/meshes/link_2.STL"/>
 83 |       </geometry>
 84 |     </collision>
 85 |   </link>
 86 |   <joint name="joint_2" type="revolute">
 87 |     <origin rpy="1.5708 0.5236 -1.5708" xyz="0 0.064146 -0.16608"/>
 88 |     <parent link="link_1"/>
 89 |     <child link="link_2"/>
 90 |     <axis xyz="0 0 -1"/>
 91 |     <limit lower="${-39.6 / 180.0 * pi}" upper="${pi / 2.0}" effort="100" velocity="100"/>
 92 |   </joint>
 93 |   <link name="link_3">
 94 |     <inertial>
 95 |       <origin rpy="0 0 0" xyz="-0.00029765 -0.023661 -0.0019125"/>
 96 |       <mass value="0.1787"/>
 97 |       <inertia ixx="0.0001685" ixy="-2.7713E-05" ixz="5.6885E-06" iyy="0.00012865" iyz="2.9256E-05" izz="0.00020744"/>
 98 |     </inertial>
 99 |     <visual>
100 |       <origin rpy="0 0 0" xyz="0 0 0"/>
101 |       <geometry>
102 |         <mesh filename="package://ar3_description/meshes/link_3.STL"/>
103 |       </geometry>
104 |       <material name="">
105 |         <color rgba="1 1 0 1"/>
106 |       </material>
107 |     </visual>
108 |     <collision>
109 |       <origin rpy="0 0 0" xyz="0 0 0"/>
110 |       <geometry>
111 |         <mesh filename="package://ar3_description/meshes/link_3.STL"/>
112 |       </geometry>
113 |     </collision>
114 |   </link>
115 |   <joint name="joint_3" type="revolute">
116 |     <origin rpy="0 0 -1.04720367321" xyz="0.1525 -0.26414 0"/>
117 |     <parent link="link_2"/>
118 |     <child link="link_3"/>
119 |     <axis xyz="0 0 -1"/>
120 |     <limit lower="0.0174533" upper="2.5080381" effort="100" velocity="100"/>
121 |   </joint>
122 |   <link name="link_4">
123 |     <inertial>
124 |       <origin rpy="0 0 0" xyz="-0.0016798 -0.00057319 -0.074404"/>
125 |       <mass value="0.34936"/>
126 |       <inertia ixx="0.0030532" ixy="-1.8615E-05" ixz="-7.0047E-05" iyy="0.0031033" iyz="-2.3301E-05" izz="0.00022264"/>
127 |     </inertial>
128 |     <visual>
129 |       <origin rpy="0 0 0" xyz="0 0 0"/>
130 |       <geometry>
131 |         <mesh filename="package://ar3_description/meshes/link_4.STL"/>
132 |       </geometry>
133 |       <material name="">
134 |         <color rgba="1 1 0 1"/>
135 |       </material>
136 |     </visual>
137 |     <collision>
138 |       <origin rpy="0 0 0" xyz="0 0 0"/>
139 |       <geometry>
140 |         <mesh filename="package://ar3_description/meshes/link_4.STL"/>
141 |       </geometry>
142 |     </collision>
143 |   </link>  
144 |   <joint name="joint_4" type="revolute">
145 |     <origin rpy="1.5708 -1.2554 -1.5708" xyz="0 0 0.00675"/>
146 |     <parent link="link_3"/>
147 |     <child link="link_4"/>
148 |     <axis xyz="0 0 -1"/>
149 |     <limit lower="-2.8710666" upper="2.8710666" effort="100" velocity="100"/>
150 |   </joint>
151 |   <link name="link_5">
152 |     <inertial>
153 |       <origin rpy="0 0 0" xyz="0.0015066 -1.3102E-05 -0.012585"/>
154 |       <mass value="0.11562"/>
155 |       <inertia ixx="5.5035E-05" ixy="-1.019E-08" ixz="-2.6243E-06" iyy="8.2921E-05" iyz="1.4437E-08" izz="5.2518E-05"/>
156 |     </inertial>
157 |     <visual>
158 |       <origin rpy="0 0 0" xyz="0 0 0"/>
159 |       <geometry>
160 |         <mesh filename="package://ar3_description/meshes/link_5.STL"/>
161 |       </geometry>
162 |       <material name="">
163 |         <color rgba="1 1 0 1"/>
164 |       </material>
165 |     </visual>
166 |     <collision>
167 |       <origin rpy="0 0 0" xyz="0 0 0"/>
168 |       <geometry>
169 |         <mesh filename="package://ar3_description/meshes/link_5.STL"/>
170 |       </geometry>
171 |     </collision>
172 |   </link>
173 |   <joint name="joint_5" type="revolute">
174 |     <origin rpy="${pi} 0 -2.8262" xyz="0 0 -0.22225"/>
175 |     <parent link="link_4"/>
176 |     <child link="link_5"/>
177 |     <axis xyz="1 0 0"/>
178 |     <limit lower="-1.81776042" upper="1.81776042" effort="100" velocity="100"/>
179 |   </joint>
180 |   <link name="link_6">
181 |     <inertial>
182 |       <origin rpy="0 0 0" xyz="2.9287E-10 -1.6472E-09 0.0091432"/>
183 |       <mass value="0.013863"/>
184 |       <inertia ixx="1.3596E-06" ixy="3.0585E-13" ixz="5.7102E-14" iyy="1.7157E-06" iyz="6.3369E-09" izz="2.4332E-06"/>
185 |     </inertial>
186 |     <visual>
187 |       <origin rpy="0 0 0" xyz="0 0 0"/>
188 |       <geometry>
189 |         <mesh filename="package://ar3_description/meshes/link_6.STL"/>
190 |       </geometry>
191 |       <material name="">
192 |         <color rgba="1 1 0 1"/>
193 |       </material>
194 |     </visual>
195 |     <collision>
196 |       <origin rpy="0 0 0" xyz="0 0 0"/>
197 |       <geometry>
198 |         <mesh filename="package://ar3_description/meshes/link_6.STL"/>
199 |       </geometry>
200 |     </collision>
201 |   </link>
202 |   <joint name="joint_6" type="revolute">
203 |     <origin rpy="0 0 3.1416" xyz="-0.000294 0 0.02117"/>
204 |     <parent link="link_5"/>
205 |     <child link="link_6"/>
206 |     <axis xyz="0 0 1"/>
207 |     <limit lower="-2.5848326" upper="2.5848326" effort="100" velocity="100"/>
208 |   </joint>  
209 |   <transmission name="transmission_1">
210 |     <type>transmission_interface/SimpleTransmission</type>
211 |     <joint name="joint_1">
212 |         <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
213 |     </joint>
214 |     <actuator name="motor_1">
215 |         <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
216 |         <mechanicalReduction>1</mechanicalReduction>
217 |     </actuator>
218 |   </transmission>
219 |   <transmission name="transmission_2">
220 |       <type>transmission_interface/SimpleTransmission</type>
221 |       <joint name="joint_2">
222 |           <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
223 |       </joint>
224 |       <actuator name="motor_2">
225 |           <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
226 |           <mechanicalReduction>1</mechanicalReduction>
227 |       </actuator>
228 |   </transmission>
229 |   <transmission name="transmission_3">
230 |       <type>transmission_interface/SimpleTransmission</type>
231 |       <joint name="joint_3">
232 |           <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
233 |       </joint>
234 |       <actuator name="motor_3">
235 |           <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
236 |           <mechanicalReduction>1</mechanicalReduction>
237 |       </actuator>
238 |   </transmission>
239 |   <transmission name="transmission_4">
240 |       <type>transmission_interface/SimpleTransmission</type>
241 |       <joint name="joint_4">
242 |           <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
243 |       </joint>
244 |       <actuator name="motor_4">
245 |           <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
246 |           <mechanicalReduction>1</mechanicalReduction>
247 |       </actuator>
248 |   </transmission>
249 |   <transmission name="transmission_5">
250 |       <type>transmission_interface/SimpleTransmission</type>
251 |       <joint name="joint_5">
252 |           <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
253 |       </joint>
254 |       <actuator name="motor_5">
255 |           <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
256 |           <mechanicalReduction>1</mechanicalReduction>
257 |       </actuator>
258 |   </transmission>
259 |   <transmission name="transmission_6">
260 |       <type>transmission_interface/SimpleTransmission</type>
261 |       <joint name="joint_6">
262 |           <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
263 |       </joint>
264 |       <actuator name="motor_6">
265 |           <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
266 |           <mechanicalReduction>1</mechanicalReduction>
267 |       </actuator>
268 |   </transmission>  
269 |   <gazebo>
270 |     <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
271 |     </plugin>
272 |   </gazebo>
273 |   <gazebo reference="base_link">
274 |       <selfCollide>true</selfCollide>
275 |   </gazebo>
276 |   <gazebo reference="link_1">
277 |       <selfCollide>true</selfCollide>
278 |   </gazebo>
279 |   <gazebo reference="link_2">
280 |       <selfCollide>true</selfCollide>
281 |   </gazebo>
282 |   <gazebo reference="link_3">
283 |       <selfCollide>true</selfCollide>
284 |   </gazebo>
285 |   <gazebo reference="link_4">
286 |       <selfCollide>true</selfCollide>
287 |   </gazebo>
288 |   <gazebo reference="link_5">
289 |       <selfCollide>true</selfCollide>
290 |   </gazebo>
291 |   <gazebo reference="link_6">
292 |       <selfCollide>true</selfCollide>
293 |   </gazebo>
294 | </robot>
295 | 
296 | 


--------------------------------------------------------------------------------
/ar3_control/src/move_group_demo.cpp:
--------------------------------------------------------------------------------
  1 | /*********************************************************************
  2 |  * Software License Agreement (BSD License)
  3 |  *
  4 |  *  Copyright (c) 2013, SRI International
  5 |  *  All rights reserved.
  6 |  *
  7 |  *  Redistribution and use in source and binary forms, with or without
  8 |  *  modification, are permitted provided that the following conditions
  9 |  *  are met:
 10 |  *
 11 |  *   * Redistributions of source code must retain the above copyright
 12 |  *     notice, this list of conditions and the following disclaimer.
 13 |  *   * Redistributions in binary form must reproduce the above
 14 |  *     copyright notice, this list of conditions and the following
 15 |  *     disclaimer in the documentation and/or other materials provided
 16 |  *     with the distribution.
 17 |  *   * Neither the name of SRI International nor the names of its
 18 |  *     contributors may be used to endorse or promote products derived
 19 |  *     from this software without specific prior written permission.
 20 |  *
 21 |  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 22 |  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 23 |  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 24 |  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 25 |  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 26 |  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 27 |  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 28 |  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 29 |  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 30 |  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 31 |  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 32 |  *  POSSIBILITY OF SUCH DAMAGE.
 33 |  *********************************************************************/
 34 | 
 35 | /* Author: Sachin Chitta, Dave Coleman, Mike Lautman */
 36 | 
 37 | #include <moveit/move_group_interface/move_group_interface.h>
 38 | #include <moveit/planning_scene_interface/planning_scene_interface.h>
 39 | 
 40 | #include <moveit_msgs/DisplayRobotState.h>
 41 | #include <moveit_msgs/DisplayTrajectory.h>
 42 | 
 43 | #include <moveit_msgs/AttachedCollisionObject.h>
 44 | #include <moveit_msgs/CollisionObject.h>
 45 | 
 46 | #include <moveit_visual_tools/moveit_visual_tools.h>
 47 | 
 48 | int main(int argc, char** argv)
 49 | {
 50 |   ros::init(argc, argv, "ar3_control_demo");
 51 |   ros::NodeHandle node_handle;
 52 |   ros::AsyncSpinner spinner(1);
 53 |   spinner.start();
 54 | 
 55 |   // BEGIN_TUTORIAL
 56 |   //
 57 |   // Setup
 58 |   // ^^^^^
 59 |   //
 60 |   // MoveIt operates on sets of joints called "planning groups" and stores them in an object called
 61 |   // the `JointModelGroup`. Throughout MoveIt the terms "planning group" and "joint model group"
 62 |   // are used interchangably.
 63 |   static const std::string PLANNING_GROUP = "manipulator";
 64 | 
 65 |   // The :move_group_interface:`MoveGroupInterface` class can be easily
 66 |   // setup using just the name of the planning group you would like to control and plan for.
 67 |   moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
 68 | 
 69 |   // We will use the :planning_scene_interface:`PlanningSceneInterface`
 70 |   // class to add and remove collision objects in our "virtual world" scene
 71 |   moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
 72 | 
 73 |   // Raw pointers are frequently used to refer to the planning group for improved performance.
 74 |   const robot_state::JointModelGroup* joint_model_group =
 75 |       move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
 76 | 
 77 |   // Visualization
 78 |   // ^^^^^^^^^^^^^
 79 |   //
 80 |   // The package MoveItVisualTools provides many capabilties for visualizing objects, robots,
 81 |   // and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script
 82 |   namespace rvt = rviz_visual_tools;
 83 |   moveit_visual_tools::MoveItVisualTools visual_tools("base_link");
 84 |   visual_tools.deleteAllMarkers();
 85 | 
 86 |   // Remote control is an introspection tool that allows users to step through a high level script
 87 |   // via buttons and keyboard shortcuts in RViz
 88 |   visual_tools.loadRemoteControl();
 89 | 
 90 |   // RViz provides many types of markers, in this demo we will use text, cylinders, and spheres
 91 |   Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
 92 |   text_pose.translation().z() = 1.00;
 93 |   visual_tools.publishText(text_pose, "MoveGroupInterface Demo", rvt::WHITE, rvt::XLARGE);
 94 | 
 95 |   // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
 96 |   visual_tools.trigger();
 97 | 
 98 |   // Getting Basic Information
 99 |   // ^^^^^^^^^^^^^^^^^^^^^^^^^
100 |   //
101 |   // We can print the name of the reference frame for this robot.
102 |   ROS_INFO_NAMED("tutorial", "Planning frame: %s", move_group.getPlanningFrame().c_str());
103 | 
104 |   // We can also print the name of the end-effector link for this group.
105 |   ROS_INFO_NAMED("tutorial", "End effector link: %s", move_group.getEndEffectorLink().c_str());
106 | 
107 |   // We can get a list of all the groups in the robot:
108 |   ROS_INFO_NAMED("tutorial", "Available Planning Groups:");
109 |   std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
110 |             std::ostream_iterator<std::string>(std::cout, ", "));
111 | 
112 |   // Start the demo
113 |   // ^^^^^^^^^^^^^^^^^^^^^^^^^
114 |   visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");
115 | 
116 |   // Planning to a Pose goal
117 |   // ^^^^^^^^^^^^^^^^^^^^^^^
118 |   // We can plan a motion for this group to a desired pose for the
119 |   // end-effector.
120 |   geometry_msgs::Pose target_pose1;
121 |   target_pose1.orientation.w = 0.5;
122 |   target_pose1.orientation.x = -0.5;
123 |   target_pose1.orientation.y = -0.5;
124 |   target_pose1.orientation.z = 0.5;
125 |   target_pose1.position.x = 0.0;
126 |   target_pose1.position.y = -0.40;
127 |   target_pose1.position.z = 0.30;
128 |   move_group.setPoseTarget(target_pose1);
129 | 
130 |   // Now, we call the planner to compute the plan and visualize it.
131 |   // Note that we are just planning, not asking move_group
132 |   // to actually move the robot.
133 |   moveit::planning_interface::MoveGroupInterface::Plan my_plan;
134 | 
135 |   bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
136 | 
137 |   ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
138 | 
139 |   // Visualizing plans
140 |   // ^^^^^^^^^^^^^^^^^
141 |   // We can also visualize the plan as a line with markers in RViz.
142 |   ROS_INFO_NAMED("tutorial", "Visualizing plan 1 as trajectory line");
143 |   visual_tools.publishAxisLabeled(target_pose1, "pose1");
144 |   visual_tools.publishText(text_pose, "Pose Goal", rvt::WHITE, rvt::XLARGE);
145 |   visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group->getLinkModel("link_6"), joint_model_group, rvt::LIME_GREEN);
146 |   visual_tools.trigger();
147 |   visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to execute pose goal");
148 | 
149 |   // Moving to a pose goal
150 |   // ^^^^^^^^^^^^^^^^^^^^^
151 |   //
152 |   // Moving to a pose goal is similar to the step above
153 |   // except we now use the move() function. Note that
154 |   // the pose goal we had set earlier is still active
155 |   // and so the robot will try to move to that goal. We will
156 |   // not use that function in this tutorial since it is
157 |   // a blocking function and requires a controller to be active
158 |   // and report success on execution of a trajectory.
159 | 
160 |   move_group.move();
161 | 
162 |   // Planning to a joint-space goal
163 |   // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
164 |   //
165 |   // Let's set a joint space goal and move towards it.  This will replace the
166 |   // pose target we set above.
167 |   //
168 |   // To start, we'll create an pointer that references the current robot's state.
169 |   // RobotState is the object that contains all the current position/velocity/acceleration data.
170 |   moveit::core::RobotStatePtr current_state = move_group.getCurrentState();
171 |   //
172 |   // Next get the current set of joint values for the group.
173 |   std::vector<double> joint_group_positions;
174 |   current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);
175 | 
176 |   // Now, let's modify one of the joints, plan to the new joint space goal and visualize the plan.
177 |   joint_group_positions[0] = 1.57;  // radians
178 |   joint_group_positions[1] = 0.785;  // radians
179 |   joint_group_positions[2] = 1.57;  // radians
180 |   joint_group_positions[3] = 0.0;  // radians
181 |   joint_group_positions[4] = -0.785;  // radians
182 |   joint_group_positions[5] = 0.0;  // radians
183 |   move_group.setJointValueTarget(joint_group_positions);
184 | 
185 |   success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
186 |   ROS_INFO_NAMED("tutorial", "Visualizing plan 2 (joint space goal) %s", success ? "" : "FAILED");
187 | 
188 |   // Visualize the plan in RViz
189 |   visual_tools.deleteAllMarkers();
190 |   visual_tools.publishText(text_pose, "Joint Space Goal", rvt::WHITE, rvt::XLARGE);
191 |   visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group->getLinkModel("link_6"), joint_model_group, rvt::LIME_GREEN);
192 |   visual_tools.trigger();
193 |   visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to execute joint-space goal");
194 | 
195 |   move_group.move();
196 | 
197 |     // Planning with Path Constraints
198 |   // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
199 |   //
200 |   // Path constraints can easily be specified for a link on the robot.
201 |   // Let's specify a path constraint and a pose goal for our group.
202 |   // First define the path constraint.
203 |   moveit_msgs::OrientationConstraint ocm;
204 |   ocm.link_name = "link_6";
205 |   ocm.header.frame_id = "base_link";
206 |   ocm.orientation.w = 0.5;
207 |   ocm.orientation.x = -0.5;
208 |   ocm.orientation.y = -0.5;
209 |   ocm.orientation.z = 0.5;
210 |   ocm.absolute_x_axis_tolerance = 0.1;
211 |   ocm.absolute_y_axis_tolerance = 0.1;
212 |   ocm.absolute_z_axis_tolerance = 0.1;
213 |   ocm.weight = 1.0;
214 | 
215 |   // Now, set it as the path constraint for the group.
216 |   moveit_msgs::Constraints test_constraints;
217 |   test_constraints.orientation_constraints.push_back(ocm);
218 |   move_group.setPathConstraints(test_constraints);
219 | 
220 |   // Enforce Planning in Joint Space
221 |   // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
222 |   //
223 |   // Depending on the planning problem MoveIt chooses between
224 |   // ``joint space`` and ``cartesian space`` for problem representation.
225 |   // Setting the group parameter ``enforce_joint_model_state_space:true`` in
226 |   // the ompl_planning.yaml file enforces the use of ``joint space`` for all plans.
227 |   //
228 |   // By default planning requests with orientation path constraints
229 |   // are sampled in ``cartesian space`` so that invoking IK serves as a
230 |   // generative sampler.
231 |   //
232 |   // By enforcing ``joint space`` the planning process will use rejection
233 |   // sampling to find valid requests. Please note that this might
234 |   // increase planning time considerably.
235 |   //
236 |   // We will reuse the old goal that we had and plan to it.
237 |   // Note that this will only work if the current state already
238 |   // satisfies the path constraints. So we need to set the start
239 |   // state to a new pose.
240 |   moveit::core::RobotState start_state(*move_group.getCurrentState());
241 |   geometry_msgs::Pose start_pose2;
242 |   start_pose2.orientation.w = 0.5;
243 |   start_pose2.orientation.x = -0.5;
244 |   start_pose2.orientation.y = -0.5;
245 |   start_pose2.orientation.z = 0.5;
246 |   start_pose2.position.x = -0.40;
247 |   start_pose2.position.y = 0.0;
248 |   start_pose2.position.z = 0.20;
249 | 
250 |   // Now we will plan to the earlier pose target from the new
251 |   // start state that we have just created.
252 |   move_group.setPoseTarget(target_pose1);
253 | 
254 |   // Planning with constraints can be slow because every sample must call an inverse kinematics solver.
255 |   // Lets increase the planning time from the default 5 seconds to be sure the planner has enough time to succeed.
256 |   move_group.setPlanningTime(10.0);
257 | 
258 |   success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
259 |   ROS_INFO_NAMED("tutorial", "Visualizing plan 3 (constraints) %s", success ? "" : "FAILED");
260 | 
261 |   // Visualize the plan in RViz
262 |   visual_tools.deleteAllMarkers();
263 |   visual_tools.publishAxisLabeled(start_pose2, "start");
264 |   visual_tools.publishAxisLabeled(target_pose1, "goal");
265 |   visual_tools.publishText(text_pose, "Constrained Goal", rvt::WHITE, rvt::XLARGE);
266 |   visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group->getLinkModel("link_6"), joint_model_group, rvt::LIME_GREEN);
267 |   visual_tools.trigger();
268 | 
269 |   visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to execute path-constrained pose goal");
270 | 
271 |   move_group.move();
272 | 
273 |   // When done with the path constraint be sure to clear it.
274 |   move_group.clearPathConstraints();
275 | 
276 |   // Cartesian Paths
277 |   // ^^^^^^^^^^^^^^^
278 |   // You can plan a Cartesian path directly by specifying a list of waypoints
279 |   // for the end-effector to go through. Note that we are starting
280 |   // from the new start state above.  The initial pose (start state) does not
281 |   // need to be added to the waypoint list but adding it can help with visualizations
282 | 
283 |   std::vector<geometry_msgs::Pose> waypoints;
284 |   waypoints.push_back(start_pose2);
285 | 
286 |   geometry_msgs::Pose target_pose3 = start_pose2;
287 | 
288 |   target_pose3.position.z -= 0.1;
289 |   waypoints.push_back(target_pose3);  // down
290 | 
291 |   target_pose3.position.y -= 0.1;
292 |   waypoints.push_back(target_pose3);  // right
293 | 
294 |   target_pose3.position.z += 0.1;
295 |   target_pose3.position.y += 0.1;
296 |   target_pose3.position.x -= 0.1;
297 |   waypoints.push_back(target_pose3);  // up and left
298 | 
299 |   // Cartesian motions are frequently needed to be slower for actions such as approach and retreat
300 |   // grasp motions. Here we demonstrate how to reduce the speed of the robot arm via a scaling factor
301 |   // of the maxiumum speed of each joint. Note this is not the speed of the end effector point.
302 |   move_group.setMaxVelocityScalingFactor(0.1);
303 | 
304 |   // We want the Cartesian path to be interpolated at a resolution of 1 cm
305 |   // which is why we will specify 0.01 as the max step in Cartesian
306 |   // translation.  We will specify the jump threshold as 0.0, effectively disabling it.
307 |   // Warning - disabling the jump threshold while operating real hardware can cause
308 |   // large unpredictable motions of redundant joints and could be a safety issue
309 |   moveit_msgs::RobotTrajectory trajectory;
310 |   const double jump_threshold = 0.0;
311 |   const double eef_step = 0.01;
312 |   double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
313 |   ROS_INFO_NAMED("tutorial", "Visualizing plan 4 (Cartesian path) (%.2f%% acheived)", fraction * 100.0);
314 | 
315 |   // Visualize the plan in RViz
316 |   visual_tools.deleteAllMarkers();
317 |   visual_tools.publishText(text_pose, "Cartesian Path", rvt::WHITE, rvt::XLARGE);
318 |   visual_tools.publishPath(waypoints, rvt::LIME_GREEN, rvt::SMALL);
319 |   for (std::size_t i = 0; i < waypoints.size(); ++i)
320 |     visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL);
321 |   visual_tools.trigger();
322 |   visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to execute cartesian path");
323 | 
324 |   my_plan.trajectory_ = trajectory;
325 |   move_group.execute(my_plan);
326 | 
327 |   visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to end the demo");
328 | 
329 |   ros::shutdown();
330 |   return 0;
331 | }
332 | 


--------------------------------------------------------------------------------
/ar3_microcontrollers/teensy/baseline_dev/baseline_dev.ino:
--------------------------------------------------------------------------------
  1 | #include <Encoder.h>
  2 | #include <avr/pgmspace.h>
  3 | 
  4 | #include <AccelStepper.h>
  5 | 
  6 | // Firmware version
  7 | const char* VERSION = "0.0.1";
  8 | 
  9 | const int NUM_JOINTS = 6;
 10 | 
 11 | /*
 12 |   MOTOR DIRECTION - motor directions can be changed on the caibration page in the software but can also
 13 |   be changed here: example using DM542T driver(CW) set to 1 - if using ST6600 or DM320T driver(CCW) set to 0
 14 |   DEFAULT = 111011   */
 15 | 
 16 | const int J1rotdir = 1;
 17 | const int J2rotdir = 1;
 18 | const int J3rotdir = 1;
 19 | const int J4rotdir = 0;
 20 | const int J5rotdir = 1;
 21 | const int J6rotdir = 1;
 22 | const int ROT_DIRS[] = { J1rotdir, J2rotdir, J3rotdir, J4rotdir, J5rotdir, J6rotdir };
 23 | 
 24 | // approx encoder counts at rest position, 0 degree joint angle
 25 | // DEFAULTS 38681, 11264, 0, 36652, 5839, 15671
 26 | // J2 and J6 joint enc { 38681, 902, 0, 36652, 2198, 15671 }
 27 | const int REST_ENC_POSITIONS[] = { 38681, 11264, 0, 36652, 5839, 15671 };
 28 | 
 29 | const int J1stepPin = 0;
 30 | const int J1dirPin = 1;
 31 | const int J2stepPin = 2;
 32 | const int J2dirPin = 3;
 33 | const int J3stepPin = 4;
 34 | const int J3dirPin = 5;
 35 | const int J4stepPin = 6;
 36 | const int J4dirPin = 7;
 37 | const int J5stepPin = 8;
 38 | const int J5dirPin = 9;
 39 | const int J6stepPin = 10;
 40 | const int J6dirPin = 11;
 41 | const int TRstepPin = 12;
 42 | const int TRdirPin = 13;
 43 | const int STEP_PINS[] = { J1stepPin, J2stepPin, J3stepPin, J4stepPin, J5stepPin, J6stepPin };
 44 | const int DIR_PINS[] = { J1dirPin, J2dirPin, J3dirPin, J4dirPin, J5dirPin, J6dirPin };
 45 | 
 46 | // set encoder pins
 47 | Encoder J1encPos(14, 15);
 48 | Encoder J2encPos(16, 17);
 49 | Encoder J3encPos(18, 19);
 50 | Encoder J4encPos(20, 21);
 51 | Encoder J5encPos(22, 23);
 52 | Encoder J6encPos(24, 25);
 53 | Encoder JOINT_ENCODERS[] = { J1encPos, J2encPos, J3encPos, J4encPos, J5encPos, J6encPos };
 54 | // DEFAULTS { 1, 1, 1, 1, 1, 1 }
 55 | // J2 joint enc  { 1, -1, 1, 1, 1, 1 }
 56 | int ENC_DIR[] = { 1, 1, 1, 1, 1, 1 }; // +1 if encoder direction matches motor direction
 57 | 
 58 | // set calibration limit switch pins
 59 | const int J1calPin = 26;
 60 | const int J2calPin = 27;
 61 | const int J3calPin = 28;
 62 | const int J4calPin = 29;
 63 | const int J5calPin = 30;
 64 | const int J6calPin = 31;
 65 | const int CAL_PINS[] = { J1calPin, J2calPin, J3calPin, J4calPin, J5calPin, J6calPin };
 66 | const int LIMIT_SWITCH_HIGH[] = { 1, 1, 1, 1, 1, 1 }; // to account for both NC and NO limit switches
 67 | const int CAL_DIR[] = { -1, -1, 1, -1, -1, 1 }; // joint rotation direction to limit switch
 68 | const int CAL_SPEED = 600; // motor steps per second
 69 | // DEFAULT { 1, 1, 1, 2, 1, 1 }
 70 | // J2 joint enc { 1, 10, 1, 2, 1, 1 }
 71 | const int CAL_SPEED_MULT[] = { 1, 1, 1, 2, 1, 1 }; // multiplier to account for transmission ratios
 72 | 
 73 | // motor and encoder steps per revolution
 74 | // DEFAULTS { 400, 400, 400, 400, 800, 400 }
 75 | // J2 motor 4000 steps per rev { 400, 4000, 400, 400, 800, 400 }
 76 | const int MOTOR_STEPS_PER_REV[] = { 400, 400, 400, 400, 800, 400 };
 77 | 
 78 | // num of steps in range of motion of joint, may vary depending on your limit switch
 79 | // DEFAULT { 77363, 36854, 40878, 71967, 23347, 32358 }
 80 | // J2 joint enc { 77363, 2949, 40878, 71967, 4740, 32358 }
 81 | const int ENC_RANGE_STEPS[] = { 77363, 36854, 40878, 71967, 23347, 32358 };
 82 | 
 83 | //set encoder multiplier, encoder steps per motor step
 84 | // DEFAULT { 5.12, 5.12, 5.12, 5.12, 2.56, 5.12 }
 85 | // J2 and J5 joint enc { 5.12, 0.04096, 5.12, 5.12, 0.5197, 5.12 }
 86 | const float ENC_MULT[] = { 5.12, 5.12, 5.12, 5.12, 2.56, 5.12 };
 87 | // DEFAULT { 227.5555555555556, 284.4444444444444, 284.4444444444444, 223.0044444444444, 56.04224675948152, 108.0888888888889 }
 88 | // J2 and J5 joint enc { 227.5555555555556, 22.75555555555556, 284.4444444444444, 223.0044444444444, 22.75555555555556, 108.0888888888889 }
 89 | const float ENC_STEPS_PER_DEG[] = { 227.5555555555556, 284.4444444444444, 284.4444444444444, 223.0044444444444, 56.04224675948152, 108.0888888888889 };
 90 | 
 91 | // speed and acceleration settings
 92 | float JOINT_MAX_SPEED[] = { 20.0, 20.0, 20.0, 20.0, 20.0, 20.0 }; // deg/s
 93 | float JOINT_MAX_ACCEL[] = { 5.0, 5.0, 5.0, 5.0, 5.0, 5.0 }; // deg/s^2
 94 | // DEFAULT { 1500, 1500, 1500, 2000, 1500, 1500 }
 95 | // J2 4000 steps/rev { 1500, 15000, 1500, 2000, 1500, 1500 }
 96 | int MOTOR_MAX_SPEED[] = { 1500, 1500, 1500, 2000, 1500, 1500 }; // motor steps per sec
 97 | // DEFAULT { 250, 250, 250, 250, 250, 250 }
 98 | // J2 4000 steps/rev { 250, 2500, 250, 250, 250, 250 }
 99 | int MOTOR_MAX_ACCEL[] = { 250, 250, 250, 250, 250, 250 }; // motor steps per sec^2
100 | 
101 | // played around with this a little but I don't see a need to use this at the moment
102 | float MOTOR_SPEED_MULT[] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 }; // for tuning position control
103 | float MOTOR_ACCEL_MULT[] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 }; // for tuning position control
104 | 
105 | AccelStepper stepperJoints[NUM_JOINTS];
106 | 
107 | enum SM { STATE_INIT, STATE_TRAJ, STATE_ERR };
108 | SM STATE = STATE_INIT;
109 | 
110 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
111 | //MAIN
112 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
113 | 
114 | void setup() {
115 |   // run once:
116 |   Serial.begin(115200);
117 | 
118 |   for (int i = 0; i < NUM_JOINTS; ++i)
119 |   {
120 |     JOINT_ENCODERS[i].write(REST_ENC_POSITIONS[i]);
121 |   }
122 | 
123 |   pinMode(TRstepPin, OUTPUT);
124 |   pinMode(TRdirPin, OUTPUT);
125 |   pinMode(J1stepPin, OUTPUT);
126 |   pinMode(J1dirPin, OUTPUT);
127 |   pinMode(J2stepPin, OUTPUT);
128 |   pinMode(J2dirPin, OUTPUT);
129 |   pinMode(J3stepPin, OUTPUT);
130 |   pinMode(J3dirPin, OUTPUT);
131 |   pinMode(J4stepPin, OUTPUT);
132 |   pinMode(J4dirPin, OUTPUT);
133 |   pinMode(J5stepPin, OUTPUT);
134 |   pinMode(J5dirPin, OUTPUT);
135 |   pinMode(J6stepPin, OUTPUT);
136 |   pinMode(J6dirPin, OUTPUT);
137 | 
138 |   pinMode(J1calPin, INPUT_PULLUP);
139 |   pinMode(J2calPin, INPUT_PULLUP);
140 |   pinMode(J3calPin, INPUT_PULLUP);
141 |   pinMode(J4calPin, INPUT_PULLUP);
142 |   pinMode(J5calPin, INPUT_PULLUP);
143 |   pinMode(J6calPin, INPUT_PULLUP);
144 | 
145 |   digitalWrite(TRstepPin, HIGH);
146 |   digitalWrite(J1stepPin, HIGH);
147 |   digitalWrite(J2stepPin, HIGH);
148 |   digitalWrite(J3stepPin, HIGH);
149 |   digitalWrite(J4stepPin, HIGH);
150 |   digitalWrite(J5stepPin, HIGH);
151 |   digitalWrite(J6stepPin, HIGH);
152 | 
153 | }
154 | 
155 | bool initStateTraj(String inData)
156 | {
157 |   // parse initialisation message
158 |   int idxVersion = inData.indexOf('A');
159 |   String softwareVersion = inData.substring(idxVersion + 1, inData.length() - 1);
160 |   int versionMatches = (softwareVersion == VERSION);
161 | 
162 |   // return acknowledgement with result
163 |   String msg = String("ST") + String("A") + String(versionMatches) + String("B") + String(VERSION) + String("\n");
164 |   Serial.print(msg);
165 | 
166 |   return versionMatches ? true : false;
167 | }
168 | 
169 | void readEncPos(int* encPos)
170 | {
171 |   encPos[0] = J1encPos.read() * ENC_DIR[0];
172 |   encPos[1] = J2encPos.read() * ENC_DIR[1];
173 |   encPos[2] = J3encPos.read() * ENC_DIR[2];
174 |   encPos[3] = J4encPos.read() * ENC_DIR[3];
175 |   encPos[4] = J5encPos.read() * ENC_DIR[4];
176 |   encPos[5] = J6encPos.read() * ENC_DIR[5];
177 | }
178 | 
179 | void updateStepperSpeed(String inData)
180 | {
181 |   int idxSpeedJ1 = inData.indexOf('A');
182 |   int idxAccelJ1 = inData.indexOf('B');
183 |   int idxSpeedJ2 = inData.indexOf('C');
184 |   int idxAccelJ2 = inData.indexOf('D');
185 |   int idxSpeedJ3 = inData.indexOf('E');
186 |   int idxAccelJ3 = inData.indexOf('F');
187 |   int idxSpeedJ4 = inData.indexOf('G');
188 |   int idxAccelJ4 = inData.indexOf('H');
189 |   int idxSpeedJ5 = inData.indexOf('I');
190 |   int idxAccelJ5 = inData.indexOf('J');
191 |   int idxSpeedJ6 = inData.indexOf('K');
192 |   int idxAccelJ6 = inData.indexOf('L');
193 | 
194 |   JOINT_MAX_SPEED[0] = inData.substring(idxSpeedJ1 + 1, idxAccelJ1).toFloat();
195 |   JOINT_MAX_ACCEL[0] = inData.substring(idxAccelJ1 + 1, idxSpeedJ2).toFloat();
196 |   JOINT_MAX_SPEED[1] = inData.substring(idxSpeedJ2 + 1, idxAccelJ2).toFloat();
197 |   JOINT_MAX_ACCEL[1] = inData.substring(idxAccelJ2 + 1, idxSpeedJ3).toFloat();
198 |   JOINT_MAX_SPEED[2] = inData.substring(idxSpeedJ3 + 1, idxAccelJ3).toFloat();
199 |   JOINT_MAX_ACCEL[2] = inData.substring(idxAccelJ3 + 1, idxSpeedJ4).toFloat();
200 |   JOINT_MAX_SPEED[3] = inData.substring(idxSpeedJ4 + 1, idxAccelJ4).toFloat();
201 |   JOINT_MAX_ACCEL[3] = inData.substring(idxAccelJ4 + 1, idxSpeedJ5).toFloat();
202 |   JOINT_MAX_SPEED[4] = inData.substring(idxSpeedJ5 + 1, idxAccelJ5).toFloat();
203 |   JOINT_MAX_ACCEL[4] = inData.substring(idxAccelJ5 + 1, idxSpeedJ6).toFloat();
204 |   JOINT_MAX_SPEED[5] = inData.substring(idxSpeedJ6 + 1, idxAccelJ6).toFloat();
205 |   JOINT_MAX_ACCEL[5] = inData.substring(idxAccelJ6 + 1).toFloat();
206 | }
207 | 
208 | void calibrateJoints(int* calJoints)
209 | {
210 |   // check which joints to calibrate
211 |   bool calAllDone = false;
212 |   bool calJointsDone[NUM_JOINTS];
213 |   for (int i = 0; i < NUM_JOINTS; ++i)
214 |   {
215 |     calJointsDone[i] = !calJoints[i];
216 |   }
217 |   
218 |   // first pass of calibration, fast speed
219 |   for (int i = 0; i < NUM_JOINTS; i++)
220 |   {
221 |     stepperJoints[i].setSpeed(CAL_SPEED * CAL_SPEED_MULT[i] * CAL_DIR[i]);
222 |   }
223 |   while (!calAllDone)
224 |   {
225 |     calAllDone = true;
226 |     for (int i = 0; i < NUM_JOINTS; ++i)
227 |     {
228 |       // if joint is not calibrated yet
229 |       if (!calJointsDone[i])
230 |       {
231 |         // check limit switches
232 |         if (!reachedLimitSwitch(i))
233 |         {
234 |           // limit switch not reached, continue moving
235 |           stepperJoints[i].runSpeed();
236 |           calAllDone = false;
237 |         }
238 |         else
239 |         {
240 |           // limit switch reached
241 |           stepperJoints[i].setSpeed(0); // redundancy
242 |           calJointsDone[i] = true;
243 |         }   
244 |       }   
245 |     } 
246 |   }
247 |   delay(2000);
248 | 
249 |   return;
250 | }
251 | 
252 | bool reachedLimitSwitch(int joint)
253 | {
254 |   int pin = CAL_PINS[joint];
255 |   // check multiple times to deal with noise
256 |   // possibly EMI from motor cables?
257 |   if (digitalRead(pin) == LIMIT_SWITCH_HIGH[joint])
258 |   {
259 |     if (digitalRead(pin) == LIMIT_SWITCH_HIGH[joint])
260 |     {
261 |       if (digitalRead(pin) == LIMIT_SWITCH_HIGH[joint])
262 |       {
263 |         if (digitalRead(pin) == LIMIT_SWITCH_HIGH[joint])
264 |         {
265 |           if (digitalRead(pin) == LIMIT_SWITCH_HIGH[joint])
266 |           {
267 |             return true;
268 |           }
269 |         }
270 |       }
271 |     }
272 |   }
273 |   return false;
274 | }
275 | 
276 | void stateTRAJ()
277 | {
278 |   // initialise joint steps
279 |   int curEncSteps[NUM_JOINTS];
280 |   readEncPos(curEncSteps);
281 | 
282 |   int cmdEncSteps[NUM_JOINTS];
283 |   for (int i = 0; i < NUM_JOINTS; ++i)
284 |   {
285 |     cmdEncSteps[i] = curEncSteps[i];
286 |   }
287 | 
288 |   // initialise AccelStepper instance
289 |   for (int i = 0; i < NUM_JOINTS; ++i)
290 |   {
291 |     stepperJoints[i] = AccelStepper(1, STEP_PINS[i], DIR_PINS[i]);
292 |     stepperJoints[i].setPinsInverted(true, false, false); // DM542T CW
293 |     stepperJoints[i].setAcceleration(MOTOR_MAX_ACCEL[i]);
294 |     stepperJoints[i].setMaxSpeed(MOTOR_MAX_SPEED[i]);
295 |   }
296 |   stepperJoints[3].setPinsInverted(false, false, false); // J4 DM320T CCW
297 | 
298 |   // message
299 |   String inData = "";
300 | 
301 |   // start loop
302 |   while (STATE == STATE_TRAJ)
303 |   {
304 |     char received = "";
305 |     // check for message from host
306 |     if (Serial.available())
307 |     {
308 |       received = Serial.read();
309 |       inData += received;
310 |     }
311 | 
312 |     // process message when new line character is received
313 |     if (received == '\n')
314 |     {
315 |       String function = inData.substring(0, 2);
316 |       // update trajectory information
317 |       if (function == "MT")
318 |       {
319 |         // read current joint positions
320 |         readEncPos(curEncSteps);
321 | 
322 |         // update host with joint positions
323 |         String msg = String("JP") + String("A") + String(curEncSteps[0]) + String("B") + String(curEncSteps[1]) + String("C") + String(curEncSteps[2])
324 |                    + String("D") + String(curEncSteps[3]) + String("E") + String(curEncSteps[4]) + String("F") + String(curEncSteps[5]) + String("\n");
325 |         Serial.print(msg);
326 | 
327 |         // get new position commands
328 |         int msgIdxJ1 = inData.indexOf('A');
329 |         int msgIdxJ2 = inData.indexOf('B');
330 |         int msgIdxJ3 = inData.indexOf('C');
331 |         int msgIdxJ4 = inData.indexOf('D');
332 |         int msgIdxJ5 = inData.indexOf('E');
333 |         int msgIdxJ6 = inData.indexOf('F');
334 |         cmdEncSteps[0] = inData.substring(msgIdxJ1 + 1, msgIdxJ2).toInt();
335 |         cmdEncSteps[1] = inData.substring(msgIdxJ2 + 1, msgIdxJ3).toInt();
336 |         cmdEncSteps[2] = inData.substring(msgIdxJ3 + 1, msgIdxJ4).toInt();
337 |         cmdEncSteps[3] = inData.substring(msgIdxJ4 + 1, msgIdxJ5).toInt();
338 |         cmdEncSteps[4] = inData.substring(msgIdxJ5 + 1, msgIdxJ6).toInt();
339 |         cmdEncSteps[5] = inData.substring(msgIdxJ6 + 1).toInt();
340 | 
341 |         // update target joint positions
342 |         readEncPos(curEncSteps);
343 |         for (int i = 0; i < NUM_JOINTS; ++i)
344 |         { 
345 |           curEncSteps[1] = J2encPos.read() * ENC_DIR[1];
346 |           int diffEncSteps = cmdEncSteps[i] - curEncSteps[i];
347 |           if (abs(diffEncSteps) > 2)
348 |           {
349 |             int diffMotSteps = diffEncSteps / ENC_MULT[i];
350 |             if (diffMotSteps < MOTOR_STEPS_PER_REV[i])
351 |             {
352 |               // for the last rev of motor, introduce artificial decceleration
353 |               // to help prevent overshoot
354 |               diffMotSteps = diffMotSteps / 2;
355 |             }
356 |             stepperJoints[i].move(diffMotSteps);
357 |             stepperJoints[i].run();
358 |           }
359 |         }
360 |       }
361 |       else if (function == "JC")
362 |       {
363 |         // calibrate joint 6
364 |         int calJoint6[] = { 0, 0, 0, 0, 0, 1 }; // 000001
365 |         calibrateJoints(calJoint6);
366 | 
367 |         // record encoder steps
368 |         int calStepJ6 = J6encPos.read() * ENC_DIR[5];
369 | 
370 |         // calibrate joints 1 to 5
371 |         int calJoints[] = { 1, 1, 1, 1, 1, 0 }; // 111110
372 |         calibrateJoints(calJoints);
373 | 
374 |         // record encoder steps
375 |         int calStepJ1 = J1encPos.read() * ENC_DIR[0];
376 |         int calStepJ2 = J2encPos.read() * ENC_DIR[1];
377 |         int calStepJ3 = J3encPos.read() * ENC_DIR[2];
378 |         int calStepJ4 = J4encPos.read() * ENC_DIR[3];
379 |         int calStepJ5 = J5encPos.read() * ENC_DIR[4];
380 | 
381 |         // if limit switch at lower end, set encoder to 0
382 |         // otherwise set to encoder upper limit
383 |         J1encPos.write(0);
384 |         J2encPos.write(0);
385 |         J3encPos.write(ENC_RANGE_STEPS[2]);
386 |         J4encPos.write(0);
387 |         J5encPos.write(0);
388 |         J6encPos.write(ENC_RANGE_STEPS[5]);
389 | 
390 |         // read current joint positions
391 |         readEncPos(curEncSteps);
392 | 
393 |         // return to original position
394 |         for (int i = 0; i < NUM_JOINTS; ++i)
395 |         {
396 |             stepperJoints[i].setAcceleration(MOTOR_MAX_ACCEL[i]);
397 |             stepperJoints[i].setMaxSpeed(MOTOR_MAX_SPEED[i]);
398 |             cmdEncSteps[i] = REST_ENC_POSITIONS[i];
399 |             stepperJoints[i].move((REST_ENC_POSITIONS[i] - curEncSteps[i]) / ENC_MULT[i]);
400 |         }
401 | 
402 |         bool restPosReached = false;
403 |         while (!restPosReached)
404 |         {
405 |           restPosReached = true;
406 |           // read current joint positions
407 |           readEncPos(curEncSteps);
408 |           for (int i = 0; i < NUM_JOINTS; ++i)
409 |           {
410 |             if (abs(REST_ENC_POSITIONS[i] - curEncSteps[i]) > 5)
411 |             {
412 |               restPosReached = false;
413 |               stepperJoints[i].move((REST_ENC_POSITIONS[i] - curEncSteps[i]) / ENC_MULT[i]);
414 |               stepperJoints[i].run();
415 |             }
416 |           }
417 |         }
418 |         
419 |         // calibration done, send calibration values
420 |         String msg = String("JC") + String("A") + String(calStepJ1) + String("B") + String(calStepJ2) + String("C") + String(calStepJ3)
421 |                   + String("D") + String(calStepJ4) + String("E") + String(calStepJ5) + String("F") + String(calStepJ6) + String("\n");
422 |         Serial.print(msg);
423 | 
424 |         for (int i = 0; i < NUM_JOINTS; ++i)
425 |         {
426 |             stepperJoints[i].setAcceleration(MOTOR_MAX_ACCEL[i]);
427 |             stepperJoints[i].setMaxSpeed(MOTOR_MAX_SPEED[i]);
428 |         }
429 |       }
430 |       else if (function == "JP")
431 |       {
432 |         // read current joint positions
433 |         readEncPos(curEncSteps);
434 | 
435 |         // update host with joint positions
436 |         String msg = String("JP") + String("A") + String(curEncSteps[0]) + String("B") + String(curEncSteps[1]) + String("C") + String(curEncSteps[2])
437 |                    + String("D") + String(curEncSteps[3]) + String("E") + String(curEncSteps[4]) + String("F") + String(curEncSteps[5]) + String("\n");
438 |         Serial.print(msg);
439 |       }
440 |       else if (function == "SS")
441 |       {
442 |         updateStepperSpeed(inData);
443 |         // set motor speed and acceleration
444 |         for (int i = 0; i < NUM_JOINTS; ++i)
445 |         {
446 |           MOTOR_MAX_SPEED[i] = JOINT_MAX_SPEED[i] * ENC_STEPS_PER_DEG[i] / ENC_MULT[i];
447 |           MOTOR_MAX_ACCEL[i] = JOINT_MAX_ACCEL[i] * ENC_STEPS_PER_DEG[i] / ENC_MULT[i];
448 |           stepperJoints[i].setAcceleration(MOTOR_MAX_ACCEL[i] * MOTOR_ACCEL_MULT[i]);
449 |           stepperJoints[i].setMaxSpeed(MOTOR_MAX_SPEED[i] * MOTOR_SPEED_MULT[i]);
450 |         }
451 |         // read current joint positions
452 |         readEncPos(curEncSteps);
453 | 
454 |         // update host with joint positions
455 |         String msg = String("JP") + String("A") + String(curEncSteps[0]) + String("B") + String(curEncSteps[1]) + String("C") + String(curEncSteps[2])
456 |                    + String("D") + String(curEncSteps[3]) + String("E") + String(curEncSteps[4]) + String("F") + String(curEncSteps[5]) + String("\n");
457 |         Serial.print(msg);
458 |       }
459 |       else if (function == "ST")
460 |       {
461 |         if (!initStateTraj(inData))
462 |         {
463 |           STATE = STATE_INIT;
464 |           return;
465 |         }
466 |       }
467 |       
468 |       // clear message
469 |       inData = "";
470 |     }
471 |     // execute motor commands
472 |     for (int i = 0; i < NUM_JOINTS; ++i)
473 |     {
474 |       // target joint positions are already updated, just call run()
475 |       stepperJoints[i].run();
476 |     }
477 |   }
478 | }
479 | 
480 | void stateINIT()
481 | {
482 |   // message
483 |   String inData = "";
484 | 
485 |   // start loop
486 |   while (STATE == STATE_INIT)
487 |   {
488 |     char received = "";
489 |     // check for message from host
490 |     if (Serial.available())
491 |     {
492 |       received = Serial.read();
493 |       inData += received;
494 |     }
495 | 
496 |     // process message when new line character is received
497 |     if (received == '\n')
498 |     {
499 |       String function = inData.substring(0, 2);
500 |       // update trajectory information
501 |       if (function == "ST")
502 |       {
503 |         if (initStateTraj(inData))
504 |         {
505 |           STATE = STATE_TRAJ;
506 |           return;
507 |         }
508 |       }
509 |       inData = "";
510 |     }
511 |   }
512 | }
513 | 
514 | void stateERR()
515 | {
516 |   // enter holding state
517 |   digitalWrite(J1stepPin, LOW);
518 |   digitalWrite(J2stepPin, LOW);
519 |   digitalWrite(J3stepPin, LOW);
520 |   digitalWrite(J4stepPin, LOW);
521 |   digitalWrite(J5stepPin, LOW);
522 |   digitalWrite(J6stepPin, LOW);
523 | 
524 |   // do recovery @TODO
525 |   while (STATE == STATE_ERR) {}
526 | }
527 | 
528 | void loop() 
529 | {  
530 |   // state control
531 |   switch (STATE)
532 |   {
533 |     case STATE_TRAJ:
534 |       stateTRAJ();
535 |       break;
536 |     case STATE_ERR:
537 |       stateERR();
538 |       break;
539 |     default:
540 |       stateINIT();
541 |       break;
542 |   }
543 | }
544 | 


--------------------------------------------------------------------------------