wbc.orogen

name "wbc"

using_library "base-types"
using_library "wbc-core"

import_types_from "base"
import_types_from "wbc/core/TaskConfig.hpp"
import_types_from "wbc/core/TaskStatus.hpp"
import_types_from "wbc/core/RobotModelConfig.hpp"
import_types_from "wbc/core/QuadraticProgram.hpp"
import_types_from "base/samples/RigidBodyStateSE3.hpp" # Workaround until we have RigidBodyStateSE3 in base/orogen/types
import_types_from "wbcTypes.hpp"

#
# Base WBC Task.
#
#  configureHook:
#
#  1. Load and configure robot models
#  2. Configure wbc scene
#  3. Create dynamic ports
#
#  updateHook:
#
#  1. Read input ports
#  2. Update robot models
#  3. Update Tasks and send them to the solver
#
task_context "WbcTask" do
    needs_configuration

    runtime_states "NO_JOINT_STATE", "NO_FLOATING_BASE_STATE"

    property("robot_model", "wbc/RobotModelConfig").
        doc("Configuration of the robot model. See wbc/core/RobotModelConfig.hpp for details.")

    property("qp_solver", "std/string", "qpoases").
        doc("Type of QP solver used. Can be one of ['hls','qpoases','qpswift','eiquadprog','proxqp']")

    property("wbc_type", "std/string", "velocity_qp").
        doc("Type of WBC scene used. Can be one of ['velocity','velocity_qp','acceleration','acceleration_tsid','acceleration_reduced_tsid']")

    property("wbc_config", "std/vector<wbc/TaskConfig>").
        doc("Configuration of Tasks. The WbcTask will dynamically create the following ports for each task:
             Cartesian tasks:
                 [in] Reference Input (/base/samples/RigidBodyStateSE3):         ref_<task_name>
                 [in] Weights (/base/VectorXd):                      weight_<task_name>
                 [in] Activation function (double):                  activation_<task_name>
                 [out] Task pose/twist/acceleration output (/base/samples/RigidBodyStateSE3): status_<task_name>
                 [out] Task debug output (/wbc/TaskStatus): task_<task_name>
             Joint Space tasks:
                 [in] Reference Input (/base/samples/Joints):            ref_<task_name>
                 [in] Weights (/base/VectorXd):                          weight_<task_name>
                 [in] Activation function (double):                      activation_<task_name>
                 [out] Current joint state output (base/samples/Joints): status_<task_name>
                 [out] Task debug output (/wbc/TaskStatus): task_<task_name>")

    property("initial_joint_weights", "wbc/JointWeights").
        doc("The joint weights control the contribution of each individual joint to the task solution. Values have to be within (0 <= wq <= 1). A zero means here that the joint is not used at all. Size has to be same as number of robot joints or empty, in which case each joint weight is set to 1.")

    property("compute_task_status", "bool", true).
        doc("For debugging purpose: Compute debug the current status for each task, which contains (a) the solver output projected to task space (desired task space motion) and
             (b) the actual task space motion")

    property("integrate", "bool", true).
        doc("Perform numerical integration on the solver output, e.g. if output is acceleration type, integrate twice to get velocity and position")

    # These dynamic ports are created according to the 'wbc_config' (see above)

    dynamic_input_port /.*/, "/base/samples/Joints"
    dynamic_input_port /.*/, "/base/samples/RigidBodyStateSE3"
    dynamic_input_port /.*/, "/base/VectorXd"
    dynamic_input_port /.*/, "/double"
    dynamic_output_port /.*/, "/base/samples/Joints"

    input_port("joint_weights", "wbc/JointWeights").
       doc("Update joint weight values. Size has to be same number of joints.")

    input_port("floating_base_state", "base/samples/RigidBodyStateSE3").
       doc("Pose, Twist and spatial acceleration of the 6 dof floating base.")

    input_port("floating_base_state_deprecated", "base/samples/RigidBodyState").
       doc("Deprecated: In future the port floating_base_state should be used")

    input_port("joint_state", "base/samples/Joints").
       doc("Current joint state of the robot. Right now, only positions are required. All configured joints have to be in this vector.")

    input_port("active_contacts", "wbc/ActiveContacts").
       doc("Set active contact points. The given names have to be configured as contact_points in the robot model.
            Depending on the WBC scene implementation this will change the qp according to the current contact configuration")

    input_port("contact_wrenches", "base/samples/Wrenches").
       doc("Measured contact wrenches. The given names have to be consistent with the configured contact points")

    output_port("solver_output", "base/commands/Joints").
       doc("Computed solver output")

    output_port("com", "base/samples/RigidBodyStateSE3").
       doc("Current center of mass")

    output_port("current_joint_weights", "wbc/JointWeights").
        doc("Debug: Current joint weight vector")

    output_port("timing_stats", "wbc/TimingStats").
       doc("Debug: Statistics on computation time")

    output_port("current_qp", "wbc/HierarchicalQP").
        doc("Debug: Output quadratic prosolver_outputgram for the solver.")

    output_port("full_joint_state", "base/samples/Joints").
        doc("Debug: Complete joint state including virtual joints as used by the solver.")

    output_port("estimated_contact_wrenches", "base/samples/Wrenches").
        doc("Estimated contact wrenches")

    operation("activateTask").
       argument("task_name", "std/string").
       argument("activation", "double")

    operation("activateTasks").
       argument("task_names", "std/vector<std/string>").
       argument("activation", "double")

    operation("deactivateAllTasks")

    periodic 0.001
end

#
# Convert samples of type base/samples/CartesianState to type base/samples/RigidBodyState and vice versa
#
task_context "RbsToCartesianStateTask" do
    needs_configuration

    dynamic_input_port /.*/, "/base/samples/RigidBodyState"
    dynamic_output_port /.*/, "/base/samples/RigidBodyStateSE3"

    property("input_ports", "std/vector<std/string>").
        doc("Vector of names of the input ports of type base/samples/RigidBodyState")

    port_driven
end

#
# Convert samples of type base/samples/RigidBodyState to type base/samples/CartesianState
#
task_context "CartesianStateToRbsTask" do
    needs_configuration

    dynamic_input_port /.*/, "/base/samples/RigidBodyStateSE3"
    dynamic_output_port /.*/, "/base/samples/RigidBodyState"

    property("input_ports", "std/vector<std/string>").
        doc("Vector of names of the input ports of type base/samples/CartesianState")

    port_driven
end

#
# Simple fake joint position interface required for the tutorials
#
task_context "LoopBackDriver" do
    needs_configuration
    property("initial_joint_state", "base/samples/Joints").
        doc("Initial position of the robot. Needs to include all joints")
    property("noise_std_dev", "double", 1e-4).
        doc("White noise std deviation which is added to the position/velocity/acceleration readings")

    input_port("command", "base/commands/Joints").
        doc("Input joint command. Has to contain all joints configureed in initial_joint_state.")

    output_port("joint_state", "base/samples/Joints").
        doc("Current joint state.")

    periodic 0.001
end

typekit do
   export_types "wbc/TaskStatus"
end