Note: This tutorial was tested with ROS Melodic and assumes that you have some basic knowledge of ROS and CANopen. It is recommended to follow the first tutorial on Profile Operating Modes before going through this tutorial. Take care of the wiring and safety instructions mentioned by the "Hardware Reference" of your EPOS4 controller!.
(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Using maxon EPOS4 in Cyclic Synchronous Operating Modes

Description: This tutorial explains how to use Cyclic Synchronous Modes of maxon EPOS4 controller such as Cyclic Synchronous Position Mode with ros_canopen and MoveIt for a pair of motors.

Keywords: canopen, ros_canopen, epos4, maxon, cyclic synchronous position, CSP

Tutorial Level: ADVANCED

Please adapt the code to the needs of your concrete application. Any warranty for proper functionality is excluded and has to be ensured based on tests within the concrete system environment. Take care of the wiring and safety instructions mentioned by the "Hardware Reference" of the corresponding controller!

When you have a robot with a kinematic chain it might be better to use a motion planner and set the controllers in Cyclic Synchronous Operating Mode to guaranty that all joints are kept synchronized with the CAN master.

This tutorial explains how to use maxon EPOS4 controller with ros_canopen and MoveIt in Cyclic Synchronous Position (CSP). It should work with any combination of EPOS4 version and motor, as long as the motor dependent parameters are set properly. For simplification, only a pair of EPOS4 controllers will be used. However the following configuration files can be easily extended to more complex robotic systems.

For more information and specific use cases using NVIDIA Jetson TX2 and Raspberry Pi, along with troubleshooting tips please visit this github repository.

EPOS4 setup

Please follow the information from the previous tutorial about Profile Operating Modes

ROS packages setup

We will start from the package you created during the previous tutorial (Profile Operating Modes) and make the necessary modifications to use CSP.

ros_canopen config package

CANopen Bus parameters

The can.yaml file can be kept the same. There is nonetheless a very important parameter that you might need to tune, which is the sync: interval_ms and which has to correspond to the "Interpolation time period value" as seen later in the node.yaml file. It is recommended to set it at 10ms for non real-time system.

Controller parameters

For the controller.yaml file, you need to add a second joint, define a joint_trajectory_controller and set the required_drive_mode to 8 for CSP as explained in canopen_402 wiki page:

joint_names: [base_link1_joint, link1_link2_joint]

joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50

joint_trajectory_controller:
  type: position_controllers/JointTrajectoryController
  joints:
    - base_link1_joint
    - link1_link2_joint
  required_drive_mode: 8
  constraints:
    stopped_velocity_tolerance: 20
    base_link1_joint: {trajectory: 50, goal: 50}
    link1_link2_joint: {trajectory: 50, goal: 50}

The joint trajectory controller holds together all the joints that need to move as a group, following a motion planner trajectory, like the ones used with MoveIt.

Node parameters

For the node.yaml file you need to add the second node, set some parameters for CSP and make sure the unit conversions are correct so that MoveIt can send proper commands to your EPOS4 controller and read correct motor data:

nodes:
  node1:
    id: 1
    name: base_link1_joint
    eds_pkg: maxon_epos4_ros_canopen # optionals package  name for relative path
    eds_file: "config/epos4_50_15_can_ec90flat_gp52c_mile800_node1.dcf" # path to EDS/DCF file
  node2:
    id: 2
    name: link1_link2_joint
    eds_pkg: maxon_epos4_ros_canopen # optionals package  name for relative path
    eds_file: "config/epos4_50_15_can_ec90flat_gp52c_mile800_node2.dcf" # path to EDS/DCF file

defaults: # optional, all defaults can be overwritten per node
#  eds_pkg: my_config_package # optional package  name for relative path
#  eds_file: "my_config.dcf" # path to EDS/DCF file
  dcf_overlay: # "ObjectID": "ParameterValue" (both as strings)
    "60C2sub1": "10" # Interpolation time period value, to be set as the sync/interval_ms value present in canopen_bus_layer.yaml file, 10ms is recommended 

# canopen_chain_node settings ..
  motor_allocator: canopen::Motor402::Allocator # select allocator for motor layer
#   motor_layer: settings passed to motor layer (plugin-specific)
  switching_state: 2 # (Operation_Enable), state for mode switching. Drive mode of operation from canopen_402 wiki
  pos_to_device: "rint(rad2deg(pos)*3200*26/360)" # rad -> inc, for a MILE 800 CPT encoder and a 26:1 gearbox, 3200 = 4*800
  pos_from_device: "deg2rad(obj6064*360/3200/26)" # actual position [inc] -> rad
  vel_to_device: "vel" # rad/s -> mdeg/s
  vel_from_device: "obj606C" # actual velocity [mdeg/s] -> rad/s
  eff_to_device: "rint(eff)" # just round to integer
  eff_from_device: "0" # unset

You can see that the dcf_overlay of object "Interpolation time period value" (60C2sub1) is defined to 10, which correspond to the sync: interval_ms defined in can.yaml file.

This is very important to achieve a smooth motion in between consecutive “Target Position” data sent by the motion library such as MoveIt.

Finally as mentioned above, you need to take a special care to the unit conversions. Let's take a closer look at the following calculation :

pos_to_device: "rint(rad2deg(pos)*3200*26/360)" # rad -> inc, for a MILE 800 CPT encoder and a 26:1 gearbox, 3200 = 4*800

The MoveIt planner is going to send a joint angle position command expressed in radian. The EPOS4 is expecting an incremental encoder position. First, the rad2deg(pos) function converts the position in radian to a position in degree. It is then multiplied by 3200 which corresponds to the encoder cpt (counts per turn) times the number of quadcounts (distinctive states per pulse) of the MILE 800 CPT encoder used in this example. We need to take into account the gearbox reduction of 26:1, hence the multiplication by 26, corresponding to the GP52 C used in the tests. We also need to relate that to a whole turn which is 360 degrees. Finally the rint function rounds the calculation to the nearest integer. The pos_from_device is calculated in a similar way, changing an incremental encoder position to an angle in radian.

You need to look at the datasheet of your encoder and gearbox and change the unit calculation accordingly!

URDF file

You'll need to edit the previous URDF file to add a link and joint. The joint limits are defined in radian and the maximum velocity in rpm:

<?xml version="1.0"?>
<!-- maxon EPOS4 example with 2 DOF -->
<robot name="maxon_epos4" xmlns:xacro="http://www.ros.org/wiki/xacro">

  <!-- Base link -->
  <link name="base_link">
  </link>

  <!-- Link1 link -->
  <link name="link1_link">
  </link>

  <!-- Joint between Base link and Link1 link -->
  <joint name="base_link1_joint" type="revolute">
    <parent link="base_link"/>
    <child link="link1_link"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <axis xyz="1 0 0"/>
    <dynamics damping="0.7"/>
    <limit effort="100.0" velocity="1300" lower="${-pi}" upper="${pi}"/>
  </joint>

  <!-- Link2 Link -->
  <link name="link2_link">
  </link>

  <!-- Joint between Link1 link and Link2 link -->
  <joint name="link1_link2_joint" type="revolute">
    <parent link="link1_link"/>
    <child link="link2_link"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <axis xyz="1 0 0"/>
    <dynamics damping="0.7"/>
    <limit effort="100.0" velocity="1300" lower="${-pi}" upper="${pi}"/>
  </joint>

  <transmission name="transmission_base_link1">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="base_link1_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="link1_maxon_motor">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
      <mechanicalReduction>26</mechanicalReduction>
    </actuator>
  </transmission>

  <transmission name="transmission_link1_link2">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="link1_link2_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="link2_maxon_motor">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
      <mechanicalReduction>26</mechanicalReduction>
    </actuator>
  </transmission>

</robot>

Launch file

For the launch file you just need to define correctly the controllers:

<!-- Load the controllers -->
  <node name="controller_spawner" pkg="controller_manager" type="controller_manager" respawn="false"
    output="screen" args="spawn 
    /maxon/canopen_motor/joint_state_controller
    /maxon/canopen_motor/joint_trajectory_controller
  "/>

MoveIt config package

It is recommended to generate MoveIt configuration package using the Setup Assistant tool so that it fits your project. However, for the purpose of this tutorial using 2 EPOS4 controllers, you can clone the following package. This package has been simplified to use only one motion planning library (OMPL) and has the minimum files and settings to interconnect both ros_canopen and MoveIt nodes.

Configuration files

  • The joint_limits.yaml file can be used to overwrite the joint velocity limits defined in the URDF file. Explanation on Time Parameterization can be read on the following link.

  • The kinematics.yaml file defines which kinematic solver is being used, as explained here.

  • The maxon_epos4.srdf file is generated by the MoveIt Setup Assistant from the URDF file. You can find moreinformation about SRDF here. You can define your own named states that you will be able to use later when setting motion goals. Joint angles are expressed in radian.

  • The ompl_planning.yaml file contains the settings for all the algorithms used by the Open Motion Planning Library, as described here.

  • The ros_controllers.yaml file defines the robot controllers to create a link to the ones defined in the maxon_epos4_ros_canopen package. More information can be found here.

Launch files

  • demo.launch: This file runs the main MoveIt demo to be used with maxon_epos4.launch file from epos4_canopen package as explained later in this tutorial. It launches the robot_state_publisher node, calls the move_group.launch file and the moveit_rviz.launch file for the Rviz GUI (ROS Visualizer).

  • maxon_epos4_moveit_controller_manager.launch.xml: This file mainly loads the ros_controllers.yaml configuration file into the ROS parameter server.

  • move_group.launch: This file includes other files such as planning_context.launch, planning_pipeline.launch.xml, trajectory_execution.launch.xml and runs the move_group node.

  • moveit.rviz: This file contains the Rviz GUI settings used when the application opens.

  • moveit_rviz.launch: This file is included in the demo.launch and it opens the Rviz GUI using moveit.rviz settings.

  • ompl_planning_pipeline.launch.xml: This file loads the ompl_planning.yaml configuration file.

  • planning_context.launch: This file loads the maxon_epos4.srdf, joint_limits.yaml and kinematics.yaml configuration files.

You need to adapt the following line to your ros_canopen configuration package name:

<!-- Load universal robot description format (URDF) -->
<param if="$(arg load_robot_description)" name="$(arg robot_description)" command="xacro  '$(find epos4_canopen)/urdf/maxon_epos4.xacro'"/>
  • planning_pipeline.launch.xml: This file includes the ompl_planning_pipeline.launch.xml file.

  • ros_controllers.launch: This file loads the ros_controllers.yaml configuration file. The controllers themselves are already loaded by the maxon_epos4.launch file from the epos4_canopen package, so the corresponding section has been commented out.

  • setup_assistant.launch: This file can be used to re-generate the whole maxon_epos4_moveit_config package with different settings. Be careful when using it as it may overwrite specific settings that connects to epos4_canopen.

  • trajectory_execution.launch.xml: This file includes maxon_epos4_moveit_controller_manager.launch.xml file.

Basic demo

Installation

To install MoveIt you can follow the official website instructions and then install these additional packages:

$ sudo apt-get install ros-melodic-moveit-planners ros-melodic-moveit-plugins ros-melodic-joint-trajectory-*

Setup your CAN interface as explained in the previous tutorial.

Executing a path

Open a terminal and launch your ros_canopen launch file:

$ roslaunch epos4_canopen maxon_epos4.launch

In another terminal, call the initialization service of your driver:

$ rosservice call /maxon/driver/init

It should output "success: True" if it worked properly.

In a third terminal you can launch the MoveIt demo:

$ roslaunch epos4_moveit_config demo.launch rviz_tutorial:=true

It should open the Rviz window with the MotionPlanning tab on the bottom left. The “Planning” tab should be selected by default. In the “Query” sub-panel, click on the “Goal State” drop-down list and choose “pose1”. This means the planner will calculate a path going from the current state to the state called “pose1”, which has been defined previously. You can then click on “Plan” and see if it could plan the path successfully. You can then click on “Execute” to send the motor commands in CSP Mode.

Wiki: ros_canopen/Tutorials/UsingMaxonEPOS4inCyclicSynchronousOperatingModes (last edited 2022-03-16 11:21:02 by CyrilJourdan)