Update documentation

config/cartesian_space_example/trajectory_publisher.yml

@@ -1,13 +1,24 @@
 /cartesian_position_controller/trajectory:
  ros__parameters:
+ # Amplitude of the circular trajectory in x-direction
  amplitude_x: 0.1
+ # Amplitude of the circular trajectory in y-direction
  amplitude_y: 0.1
+ # Frequency of the circular trajectory
  frequency: 0.3
+ # Plane in which the trajectory should be executed, cyn be one of [xy,yz,xz]
  plane: "xy"
+ # Initial position x of the robot
  init_pos_x: 0.067
+ # Initial position y of the robot
  init_pos_y: 0.0
+ # Initial position z of the robot
  init_pos_z: 1.138
+ # Initial orientation qx of the robot
  init_ori_qx: 0.0
+ # Initial orientation qy of the robot
  init_ori_qy: 0.0
+ # Initial orientation qz of the robot
  init_ori_qz: 0.0
+ # Initial orientation qw of the robot
  init_ori_qw: 1.0

config/joint_space_example/trajectory_publisher.yml

@@ -1,8 +1,12 @@
 /joint_position_controller/trajectory:
  ros__parameters:
+ # Amplitude of the sine/cosine curve
  amplitude: 0.3
+ # Frequency of the sine/cosine curve
  frequency: 3.0
+ # Joint names used in the trajectory
  joint_names: ["joint_a1", "joint_a2", "joint_a3", "joint_a4", "joint_a5", "joint_a6", "joint_a7"]
+ # Initial position of all joints
  initial_positions:
  joint_a1: 0.0
  joint_a2: 0.7854

config/null_space_example/iiwa_controllers.yaml

@@ -1,65 +1,121 @@
 controller_manager:
  ros__parameters:
+ # Update rate in Hz. This is the update rate of the controller manager. However, if you don' t configure the update rate for any of the
+ # controllers, every controller will run at this rate
  update_rate: 1000 # Hz
+ # Type configuration for the joint state broadcaster
  joint_state_broadcaster:
- type: joint_state_broadcaster/JointStateBroadcaster
+ type: joint_state_broadcaster/JointStateBroadcaste
+ # Type configuration for the whole body controllerr
  whole_body_controller:
  type: wbc_ros/WholeBodyController
+ # Type configuration for the ee pose controller
  ee_pose_controller:
  type: wbc_ros/CartesianPositionController
+ # Type configuration for the elbow pose controller
  elbow_pose_controller:
  type: wbc_ros/CartesianPositionController
 
 whole_body_controller:
  ros__parameters:
+ # Control mode of the wbc. Can be one of [velocity,acceleration].
  control_mode: velocity
+ # Command interfaces to claim from hardware. Can be one of [position,velocity,effort]
  command_interfaces: ["position"]
+ # Names of the tasks to be configured in task configuration. Each task will be a summand in the QP solver's cost function.
  task_names: ["ee_pose", "elbow_pose"]
+ # The joint weights control the contribution of each individual joint to the solution. Values have to be within [0,1].
+ # A zero means here that the joint is not used at all. Size has to be same as number of robot joints.
  joint_weights: [1.0,1.0,1.0,1.0,1.0,1.0,1.0]
  robot_model:
+ # Absolute path to plain URDF file of the robot
  file: install/wbc_ros/share/wbc_ros/models/urdf/iiwa.urdf
+ # Robot model type. Must be the exact name of one of the registered robot model plugins. See src/robot_models for all available plugins. Default is rbdl
  type: rbdl
  tasks:
  ee_pose:
+ # Task type, can be one of 'jnt' (joint space task), 'cart' (Cartesian task) or 'com' (Center of Mass task).
  type: cart
- priority: 1
+ # Priority of this task. 0 corresponds to the highest priority. Prioritization is only supported by the hls solver.
+ priority: 0
+ # Only Cartesian tasks: Root frame of the kinematic chain associated with this task. Has to be the name of a valid link in the robot model.
+ # Only robot_model.type=kdl supports root frames, which are not equal to the root of the URDF tree.
  root: world
+ # Only Cartesian tasks: Tip frame of the kinematic chain associated with this task. Has to be the name of a valid link in the robot model.
  tip: tool0
+ # Only Cartesian tasks: Reference frame of the task input (frame wrt which the input is expressed).
+ # This has to be the name of a valid link in robot model. If ref_frame != root the input will be transformed to the root frame.
  ref_frame: world
+ # Initial weights for this task. Size has to be same as number of task variables, e.g. number of joint names in joint space tasks.
+ # and 6 in case of a Cartesian task. All entries have to be >= 0. Can be used to balance contributions of the task variables.
+ # A value of 0 means that the reference of the corresponding task variable will be ignored while computing the solution.
  weights: [1.0,1.0,1.0,1.0,1.0,1.0]
+ # Initial activation for this task. Has to be within 0 and 1. Can be used to enable(1)/disable(0) the whole task,
+ # or to apply a smooth activation function.
  activation: 1.0
  elbow_pose:
+ # Task type, can be one of 'jnt' (joint space task), 'cart' (Cartesian task) or 'com' (Center of Mass task).
  type: cart
+ # Priority of this task. 0 corresponds to the highest priority. Prioritization is only supported by the hls solver.
  priority: 0
+ # Only Cartesian tasks: Root frame of the kinematic chain associated with this task. Has to be the name of a valid link in the robot model.
+ # Only robot_model.type=kdl supports root frames, which are not equal to the root of the URDF tree.
  root: world
+ # Only Cartesian tasks: Tip frame of the kinematic chain associated with this task. Has to be the name of a valid link in the robot model.
  tip: link_4
+ # Only Cartesian tasks: Reference frame of the task input (frame wrt which the input is expressed).
+ # This has to be the name of a valid link in robot model. If ref_frame != root the input will be transformed to the root frame.
  ref_frame: world
+ # Initial weights for this task. Size has to be same as number of task variables, e.g. number of joint names in joint space tasks.
+ # and 6 in case of a Cartesian task. All entries have to be >= 0. Can be used to balance contributions of the task variables.
+ # A value of 0 means that the reference of the corresponding task variable will be ignored while computing the solution.
  weights: [0.0,1.0,0.0,0.0,0.0,0.0]
+ # Initial activation for this task. Has to be within 0 and 1. Can be used to enable(1)/disable(0) the whole task,
+ # or to apply a smooth activation function.
  activation: 1.0
  solver:
+ # QP Solver type. Must be the exact name of one of the registered QP solver plugins. See src/solvers for all available plugins. Default is qpoases
  type: hls
  scene:
+ # Scene type. Must be the exact name of one of the registered scene plugins. See src/scenes for all available plugins.
  type: velocity
+ # Do numerical integration on the solver output, e.g. if output is acceleration type, integrate twice to get velocity and position
  integrate: true
 
 ee_pose_controller:
  ros__parameters:
+ # Control mode of the wbc. Can be one of [velocity,acceleration].
  control_mode: velocity
+ # Name of the WBC task this controller is assigned to.
  task_name: ee_pose
+ # Node name of the WBC controller.
  wbc_name: whole_body_controller
- p_gain: [5.0,5.0,5.0,0.0,0.0,0.0]
+ # P-Gain of the controller, size has to be 6.
+ p_gain: [5.0,5.0,5.0,5.0,5.0,5.0]
+ # D-Gain of the controller, size has to be 6. In case of velcity control mode, this is the feed forward gain
  d_gain: [0.0,0.0,0.0,0.0,0.0,0.0]
+ # Feed forward gain of the controller, size has to be 6.
  ff_gain: [0.0,0.0,0.0,0.0,0.0,0.0]
- max_control_output: [10.0,10.0,10.0,0.0,0.0,0.0]
+ # Controller output saturation per element. Size has to be 6.
+ max_control_output: [10.0,10.0,10.0,10.0,10.0,10.0]
+ # Dead zone for the position error per element. Size has to be 6.
  dead_zone: [0.0,0.0,0.0,0.0,0.0,0.0]
 
 elbow_pose_controller:
  ros__parameters:
+ # Control mode of the wbc. Can be one of [velocity,acceleration].
  control_mode: velocity
+ # Name of the WBC task this controller is assigned to.
  task_name: elbow_pose
+ # Node name of the WBC controller.
  wbc_name: whole_body_controller
+ # P-Gain of the controller, size has to be 6.
  p_gain: [5.0,5.0,5.0,5.0,5.0,5.0]
+ # D-Gain of the controller, size has to be 6. In case of velcity control mode, this is the feed forward gain
  d_gain: [0.0,0.0,0.0,0.0,0.0,0.0]
+ # Feed forward gain of the controller, size has to be 6.
  ff_gain: [0.0,0.0,0.0,0.0,0.0,0.0]
+ # Controller output saturation per element. Size has to be 6.
  max_control_output: [10.0,10.0,10.0,10.0,10.0,10.0]
+ # Dead zone for the position error per element. Size has to be 6.
  dead_zone: [0.0,0.0,0.0,0.0,0.0,0.0]

config/null_space_example/pose_publisher.yml

@@ -1,9 +1,16 @@
 /ee_pose_controller/pose:
  ros__parameters:
+ # Init position in x
  pos_x: 0.0
+ # Init position in y
  pos_y: 0.0
+ # Init position in z
  pos_z: 1.0
+ # Init orientation in qx
  ori_qx: 0.0
+ # Init orientation in qy
  ori_qy: 0.0
+ # Init orientation in qz
  ori_qz: 0.0
+ # Init orientation in qw
  ori_qw: 1.0

config/null_space_example/trajectory_publisher.yml

@@ -1,13 +1,24 @@
 /elbow_pose_controller/trajectory:
  ros__parameters:
+ # Amplitude of the circular trajectory in x-direction
  amplitude_x: 0.2
+ # Amplitude of the circular trajectory in y-direction
  amplitude_y: 0.1
+ # Frequency of the circular trajectory
  frequency: 0.3
+ # Plane in which the trajectory should be executed, cyn be one of [xy,yz,xz]
  plane: "xy"
+ # Initial position x of the robot
  init_pos_x: 0.3
+ # Initial position y of the robot
  init_pos_y: 0.0
+ # Initial position z of the robot
  init_pos_z: 0.7
+ # Initial orientation qx of the robot
  init_ori_qx: 0.0
+ # Initial orientation qy of the robot
  init_ori_qy: 0.0
+ # Initial orientation qz of the robot
  init_ori_qz: 0.0
+ # Initial orientation qw of the robot
  init_ori_qw: 1.0

launch/joint_space_example.launch.py

@@ -9,9 +9,9 @@
 import os
 
 def generate_launch_description():
- # Load the controller confguration file for the joint space example. This contains the parameters for all ros2 controllers. 
+ # Load the controller confguration file for the joint space example. This contains the parameters for all ros2 controllers.
  iiwa_controllers = PathJoinSubstitution([FindPackageShare('wbc_ros'), 'config', 'joint_space_example', 'iiwa_controllers.yaml'])
- # Load the controller confguration file for the Cartesian space example. This contains the parameters for all ros2 controllers. 
+ # Load the trajectory configuration file
  trajectory_publisher_config = os.path.join(get_package_share_directory('wbc_ros'),'config','joint_space_example','trajectory_publisher.yml')
 
  # Create the robot description parameter (URDF) from the iiwa.config.xacro file. Use the "fake" flag, which means that the input command is mirrored to the

launch/null_space_example.launch.py

@@ -9,10 +9,15 @@
 import os
 
 def generate_launch_description():
+ # Load the controller confguration file for the joint space example. This contains the parameters for all ros2 controllers.
  iiwa_controllers = PathJoinSubstitution([FindPackageShare('wbc_ros'), 'config', 'null_space_example', 'iiwa_controllers.yaml'])
+ # Load the trajectory configuration file
  trajectory_publisher_config = os.path.join(get_package_share_directory('wbc_ros'),'config','null_space_example','trajectory_publisher.yml')
+ # Load the pose configuration file
  pose_publisher_config = os.path.join(get_package_share_directory('wbc_ros'),'config','null_space_example','pose_publisher.yml')
 
+ # Create the robot description parameter (URDF) from the iiwa.config.xacro file. Use the "fake" flag, which means that the input command is mirrored to the
+ # robot state, producing a simple mini simulation
  robot_description = Command([
  PathJoinSubstitution([FindExecutable(name='xacro')]),' ',
  PathJoinSubstitution([FindPackageShare('wbc_ros'), 'models', 'urdf', 'iiwa.config.xacro']),' ',
@@ -24,25 +29,31 @@ def generate_launch_description():
  'namespace:=', '/'])
  robot_description = {'robot_description': robot_description}
 
+ # Load the controller manager (ros2_control_node). This is resposible for loading, configuring, activating and deactivating the controllers
  controller_manager = Node(
  package='controller_manager',
  executable='ros2_control_node',
  parameters=[robot_description, iiwa_controllers],
  output='both',
  namespace='/'
  )
+
+ # Spawn the whole-body controller (WBC). The WBC receive the control output from all controllers in task space (in this case only the joint position controller)
+ # and integrates into a coherent control signal in joint space (in this case joint position commands), which are written to hardware interface directly
  whole_body_controller_spawner = Node(
  package='controller_manager',
  executable='spawner',
  arguments=['whole_body_controller', '--controller-manager', ['/', 'controller_manager']],
  namespace='/')
 
+ # Spawn the ee controller. This controller stabilizes the desired end effector pose in cartesian space
  ee_pose_controller_spawner = Node(
  package='controller_manager',
  executable='spawner',
  arguments=['ee_pose_controller', '--controller-manager', ['/', 'controller_manager']],
  namespace='/')
 
+ # Spawn the elbow controller. This controller stabilizes the desired elbow trajectory in cartesian space
  elbow_pose_controller_spawner = Node(
  package='controller_manager',
  executable='spawner',
@@ -63,25 +74,29 @@ def generate_launch_description():
  target_action=whole_body_controller_spawner,
  on_exit=[ee_pose_controller_spawner])))
 
+ # The joint state broadcaster is actually not really a controller, it simply collects the joint states and publishes them
  joint_state_broadcaster = Node(
  package='controller_manager',
  executable='spawner',
  arguments=['joint_state_broadcaster', '--controller-manager', ['/', 'controller_manager']])
 
+ # This node publishes a fixed pose and sends it to the ee controller
  cartesian_pose_publisher = Node(
  package='wbc_ros',
  executable='cartesian_pose_publisher',
  name='pose',
  namespace='/ee_pose_controller',
  parameters=[pose_publisher_config])
 
+ # This node publishes a circular trajectory and sends it to the elbow controller
  cartesian_trajectory_publisher = Node(
  package='wbc_ros',
  executable='cartesian_trajectory_publisher',
  name='trajectory',
  namespace='/elbow_pose_controller',
  parameters=[trajectory_publisher_config])
 
+ # The robot state publisher computes the transform between all robot links given the joint_status topic to visualize the robot in rviz
  robot_state_publisher = Node(
  package='robot_state_publisher',
  executable='robot_state_publisher',

src/whole_body_controller.cpp

@@ -18,7 +18,7 @@ controller_interface::InterfaceConfiguration WholeBodyController::command_interf
  vector<string> iface_names;
  for(const string &joint_name : robot_model->jointNames()){
  for(const string &iface_name : params.command_interfaces)
- iface_names.push_back(joint_name + "/" + iface_name);
+ iface_names.push_back("/" + joint_name + "/" + iface_name);
  }
  return { controller_interface::interface_configuration_type::INDIVIDUAL, iface_names};
 }