diff --git a/config/cartesian_space_example/trajectory_publisher.yml b/config/cartesian_space_example/trajectory_publisher.yml index b2139ea..8f72852 100644 --- a/config/cartesian_space_example/trajectory_publisher.yml +++ b/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 diff --git a/config/joint_space_example/trajectory_publisher.yml b/config/joint_space_example/trajectory_publisher.yml index 6627684..26f06c1 100644 --- a/config/joint_space_example/trajectory_publisher.yml +++ b/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 diff --git a/config/null_space_example/iiwa_controllers.yaml b/config/null_space_example/iiwa_controllers.yaml index 27c5430..02d1a89 100644 --- a/config/null_space_example/iiwa_controllers.yaml +++ b/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] diff --git a/config/null_space_example/pose_publisher.yml b/config/null_space_example/pose_publisher.yml index 5a4f47d..4ff677a 100644 --- a/config/null_space_example/pose_publisher.yml +++ b/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 diff --git a/config/null_space_example/trajectory_publisher.yml b/config/null_space_example/trajectory_publisher.yml index 4fcd27c..3c64870 100644 --- a/config/null_space_example/trajectory_publisher.yml +++ b/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 diff --git a/launch/joint_space_example.launch.py b/launch/joint_space_example.launch.py index a30831c..9599f8d 100644 --- a/launch/joint_space_example.launch.py +++ b/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 diff --git a/launch/null_space_example.launch.py b/launch/null_space_example.launch.py index 8dc39b4..22063d9 100644 --- a/launch/null_space_example.launch.py +++ b/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,6 +29,7 @@ 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', @@ -31,18 +37,23 @@ def generate_launch_description(): 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,11 +74,13 @@ 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', @@ -75,6 +88,7 @@ def generate_launch_description(): 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', @@ -82,6 +96,7 @@ def generate_launch_description(): 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', diff --git a/src/whole_body_controller.cpp b/src/whole_body_controller.cpp index cc6c09a..7172550 100644 --- a/src/whole_body_controller.cpp +++ b/src/whole_body_controller.cpp @@ -18,7 +18,7 @@ controller_interface::InterfaceConfiguration WholeBodyController::command_interf vector 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}; }