ctrl_lib.orogen

name "ctrl_lib"

using_library "wbc-controllers"
import_types_from "wbc/controllers/PotentialFieldInfo.hpp"
import_types_from "wbc/controllers/ActivationFunction.hpp"
import_types_from "base"
import_types_from "base/samples/RigidBodyStateSE3.hpp" # Workaround until RigidBodyStateSE3 is in base/orogen/types
import_types_from "wbc/controllers/PIDCtrlParams.hpp"

#
# Base class for all controllers. State machine in RUNNING state:
#  1. Update Properties
#  2. Read Feedback term.
#  3. If a feedback term is available, read setpoint. Once there is a setpoint, control output
#     will be written at all times.
#  3. Compute and write control output, depending on the implementation of the controller (derived task)
#  4. Compute activation. The activation indicates how much influence a control output has, compared to the other control outputs. Activation
#     values will be within 0..1. The activation ports can e.g. be connected to WBC in order to deactivate constraint variables and make unneeded
#     dof available for other tasks. A typical example is Joint limits avoidance. Usually one wants to activate the avoidance behavior only when
#     being close to the joint limits and not disturb other tasks when moving freely. Different activation functions can be chosen (e.g. linear, quadratic, ...)
#     in order to achieve smooth transitions.
#
task_context "ControllerTask" do
   abstract
   needs_configuration

   runtime_states "NO_SETPOINT", # Never got a setpoint, no control output will be written
                  "NO_FEEDBACK"  # Never got feedback, no control output will be written

   # Unique name for each variable, e.g. joint names in case of a joint space controller.
   property("field_names", "std/vector<std/string>")

   # Type of activation function used. See wbc/controllers/ActivationFunction.hpp for details.
   property("activation_function", "wbc/ActivationFunction")

   # Current activation values
   output_port("activation", "base/VectorXd")

   # Current time between two consecutive calls of updateHook()
   output_port("actual_cycle_time", "double")

   # The controllers should be called periodically to achieve best performance.
   periodic 0.001
end

#
# Implementation of a proportional position controller in joint space.
#
task_context "JointPositionController", subclasses: "ControllerTask" do
   needs_configuration

   # Proportional gain. Size has to be the same as size of field_names.
   property("p_gain", "base/VectorXd")

   # Feed Forward gain. Size has to be the same as size of field_names.
   property("d_gain", "base/VectorXd")
   
   # Feed Forward gain. Size has to be the same as size of field_names.
   property("ff_gain", "base/VectorXd")

   # Maximum control output (saturation). If one output value exceeds maximum, all
   # other values will be scaled accordingly. Size has to be the same as size of field_names.
   property("max_control_output", "base/VectorXd")

   # Minimum control error (dead zone). If one control error value falls below this minimum, it will be set to zero.
   property("dead_zone", "base/VectorXd")

   # Setpoint of the controller. Joints will be mapped by name internally. Size has to be >= size of field_names. All field names have to be present.
   input_port("setpoint", "base/commands/Joints")

   # Feedback term of the controller. Joints will be mapped by name internally. Size has to be >= size of field_names. All field names have to be present.
   input_port("feedback", "base/samples/Joints")

   # Control output as velocity. Size will be same as size of field_names.
   output_port("control_output", "base/commands/Joints")

   # Debug Ports
   output_port("current_setpoint", "base/commands/Joints")
   output_port("current_feedback", "base/samples/Joints")
   output_port("control_error", "base/VectorXd")

   periodic 0.001
end

#
# Implementation of proportional position controller in Cartesian space. Size of field_names has to be 6. The orientation error will be computed
# using zyx rotation. So the control output will be <linear_velocity(xyz), angular_velocity(zyx)>,  <linear_acc(xyz), angular_acc(zyx)>
#
task_context "CartesianPositionController", subclasses: "ControllerTask" do
   needs_configuration

   # Proportional gain. Size has to be the same as size of field_names.
   property("p_gain", "base/VectorXd")

   # Derivative gain. Size has to be the same as size of field_names.
   property("d_gain", "base/VectorXd")
   
   # Feed Forward gain. Size has to be the same as size of field_names.
   property("ff_gain", "base/VectorXd")

   # Maximum control output (saturation). If one output value exceeds maximum, all
   # other values will be scaled accordingly. Size has to be the same as size of field_names.
   property("max_control_output", "base/VectorXd")

   # Minimum position control error (dead zone). If one control error value falls below this minimum, it will be set to zero.
   property("dead_zone", "base/VectorXd")

   # Setpoint of the controller.
   input_port("setpoint", "base/samples/RigidBodyStateSE3")

   # Feedback term of the controller.
   input_port("feedback", "base/samples/RigidBodyStateSE3")

   # Control output as Cartesian linear/angular velocity as well as Cartesian linear/angular acceleration.
   output_port("control_output", "base/samples/RigidBodyStateSE3")

   # Debug Ports
   output_port("current_setpoint", "base/samples/RigidBodyStateSE3")
   output_port("current_feedback", "base/samples/RigidBodyStateSE3")
   output_port("control_error", "base/VectorXd")

   periodic 0.001
end

#
# Helper task to get Poses from Transformer
#
task_context "ControllerTransformationProxy" do
    needs_configuration

    # Transformation read from transformer
    output_port("transform", "base/samples/RigidBodyState")

    transformer do
        transform "source", "target"
        max_latency 0.5
    end
    port_driven
end

#
# Implementation of a proportional force/torque controller in Cartesian space
#
task_context "CartesianForceController", subclasses: "ControllerTask" do
   needs_configuration

   # Proportional gain. Size has to be the same as size of field_names.
   property("p_gain", "base/VectorXd")

   # Name of the FT Sensor to use when using feedback_wrenches input. If you use single wrench input (feedback), this can be left empty.
   property("ft_sensor_name", "std/string")

   # Maximum control output (saturation). If one output value exceeds maximum, all
   # other values will be scaled accordingly. Size has to be the same as size of field_names.
   property("max_control_output", "base/VectorXd")

   # Minimum control error (dead zone). If one control error value falls below this minimum, it will be set to zero.
   property("dead_zone", "base/VectorXd")

   # Setpoint of the controller
   input_port("setpoint", "base/samples/Wrench")

   # Feedback term of the controller.
   input_port("feedback", "base/samples/Wrench")

   # Feedback term of the controller.
   input_port("feedback_wrenches", "base/samples/Wrenches")

   # Control output as Cartesian velocity/angular velocity.
   output_port("control_output", "base/samples/RigidBodyStateSE3")

   # Debug Ports
   output_port("current_setpoint", "base/samples/Wrench")
   output_port("current_feedback", "base/samples/Wrench")
   output_port("control_error", "base/VectorXd")

   periodic 0.001
end

#
# Implementation of RadialPotentialFields in joint space. Each joint will have one 1-dimensional potential field.
# See wbc/controllers/RadialPotentialField.hpp and wbc/controllers/JointPotentialFieldsController.hpp for details
#
task_context "JointLimitAvoidance", subclasses: "ControllerTask" do
   needs_configuration

   # Default influence distance. Size has to be same as field_names and each entry has to be > 0!
   property("influence_distance", "base/VectorXd")

   # Proportional gain. Size has to be the same as size of field_names.
   property("p_gain", "base/VectorXd")

   # Maximum control output (saturation). If one output value exceeds maximum, all
   # other values will be scaled accordingly. Size has to be the same as size of field_names.
   property("max_control_output", "base/VectorXd")

   # centers of the potential fields. Size has to be either empty or same as size of field_names
   property("joint_limits", "base/JointLimits")

   # Controller feedback: Actual joint position. Size has to be same as size of field_names. Joints will mapped internally by names.
   # All joints from field_names have to be available here!
   input_port("feedback", "base/samples/Joints")

   # Control output as joint velocity. Size will be same as size of field_names.
   output_port("control_output", "base/commands/Joints")

   # Debug ports
   output_port("current_feedback", "base/samples/Joints")
   output_port("field_infos", "std/vector<wbc/PotentialFieldInfo>")
   output_port("current_joint_limits", "base/JointLimits")

   periodic 0.001
end

#
# Implementation of RadialPotentialFields in Cartesian space. Dimension of all fields has to be 3! See wbc/controllers/RadialPotentialField.hpp 
# and wbc/controllers/PotentialFieldsController.hpp for details
#
task_context "CartesianRadialPotentialFields", subclasses: "ControllerTask" do
   needs_configuration

   # Default influence distance. Has to be > 0!
   property("influence_distance", "double")

   # Proportional gain. Size has to be the same as size of field_names.
   property("p_gain", "base/VectorXd")

   # Maximum control output (saturation). If one output value exceeds maximum, all
   # other values will be scaled accordingly. Size has to be the same as size of field_names.
   property("max_control_output", "base/VectorXd")

   # Set new centers of the potential fields here. Number of fields can be anything > 0.
   input_port("pot_field_centers",  "std/vector<base/samples/RigidBodyState>")

   # Controller feedback: Actual position of the control frame on the robot. Orientation is not used in this controller
   input_port("feedback", "base/samples/RigidBodyStateSE3")

   # Control output as cartesian velocity.
   output_port("control_output",   "base/samples/RigidBodyStateSE3")

   # Debug ports:
   output_port("current_feedback", "base/samples/RigidBodyStateSE3")
   output_port("field_infos", "std/vector<wbc/PotentialFieldInfo>")
   output_port("euclidean_distance", "base/VectorXd")

   periodic 0.001
end

#
# Joint Torque PID controller. Dimension of all fields has to be equal to field_names property
#
task_context "JointTorqueController", subclasses: "ControllerTask" do
   needs_configuration

   # PID gains. Size has to be the same as size of field_names.
   property("pid_params", "wbc/PIDCtrlParams")

   # Maximum control output (saturation). If one output value exceeds maximum, all
   # other values will be scaled accordingly. Size has to be the same as size of field_names.
   property("max_control_output", "base/VectorXd")

   # Minimum control error (dead zone). If one control error value falls below this minimum, it will be set to zero.
   property("dead_zone", "base/VectorXd")

   # Setpoint of the controller. Joints will be mapped by name internally. Size has to be >= size of field_names. All field names have to be present.
   input_port("setpoint", "base/commands/Joints")

   # Feedback term of the controller. Joints will be mapped by name internally. Size has to be >= size of field_names. All field names have to be present.
   input_port("feedback", "base/samples/Joints")

   # Control output as velocity. Size will be same as size of field_names.
   output_port("control_output", "base/commands/Joints")

   # Debug Ports
   output_port("current_setpoint", "base/commands/Joints")
   output_port("current_feedback", "base/samples/Joints")
   output_port("control_error", "base/VectorXd")

   periodic 0.001
end

typekit do
   export_types "wbc/ActivationFunction"
   export_types "std/vector<wbc/PotentialFieldInfo>"
   export_types "wbc/PotentialFieldInfo"
end