GazeboTaskboard.cpp

Go to the documentation of this file.

/*
 * Copyright (C) 2013 TopCoder Inc., All Rights Reserved.
 */

#include "../include/GazeboTaskboard.h"
#include <boost/bind.hpp>
#include <stdio.h>
#include <math.h>
#include <common/Events.hh>
#include <std_msgs/String.h>
#include <math/Angle.hh>
#include <math/Pose.hh>
#include <msgs/MessageTypes.hh>

using namespace gazebo;
using namespace gazebo_taskboard;

const double POWER_COVER_JOINT_THRESHOLD = M_PI/4;

const std::string LED_MATERIAL_OFF = "Gazebo/Grey";
const std::string LED_MATERIAL_GREEN = "Gazebo/GreenGlow";
const std::string LED_MATERIAL_BLUE = "Gazebo/BlueGlow";

const double SIMPLE_LED_LENGTH = 0.0004;
const double SIMPLE_LED_RADIUS = 0.003;
const double NUMPAD_LED_LENGTH = 0.0004;
const double NUMPAD_LED_RADIUS = 0.0095;

const double LED_HIDE_OFFSET = 100.0;
const double LED_HIDE_OFFSET2 = 105.0;

const double SAFE_TOGGLE_UPPER_LIMIT_ANGLE = 0.0;
const double SAFE_TOGGLE_LOWER_LIMIT_ANGLE = -0.94;
const double SAFE_TOGGLE_MIDDLE_LIMIT_ANGLE = -0.47;
const double SAFE_TOGGLE_HALF_ANGLE = 0.47;

const math::Vector3 ZERO_VECTOR = math::Vector3(0, 0, 0);

enum LedColor
{
    eLedColor_Green, // Green led
    eLedColor_Blue   // Blue led
};

struct GazeboTaskboardSlot1::Led
{
    bool isOn;

    // We need separate models for different states since ros gazebo plugin does
    // not provide a way to change model's material properties
    physics::ModelPtr onModel;
    physics::ModelPtr offModel;

    // LED model and link names
    std::string onModelName;
    std::string offModelName;
    std::string onLinkName;
    std::string offLinkName;

    // LED model initialization parameters.
    // The LED model is initialized when gazebo world is fully loaded (first update frame)
    int index;
    bool numPadLed;
    LedColor color;
    math::Pose pose;

    Led();
    Led(int index_, bool numPadLed_, LedColor color_, math::Pose pose_);
    void CreateModel(physics::WorldPtr world);
    void SetupModel(physics::WorldPtr world);
};

struct GazeboTaskboardSlot1::TaskboardLeds
{
    Led powerSwitchLed;
    Led rockerSwitchUpLed;
    Led rockerSwitchDownLed;
    Led numPadLeds[NUM_PAD_BUTTONS_COUNT];
    Led toggleA03Led;
    Led toggleA04TopLed;
    Led toggleA04BottomLed;
    Led toggleA05Led;

    TaskboardLeds(const math::Pose& pose);

    void CreateModels(physics::WorldPtr world);

    void SetupModels(physics::WorldPtr world);
};

struct GazeboTaskboardSlot1::TaskboardSlot1State
{
    bool isCoverOpen; 

    TwoWayToggleSwitchState powerSwitchState; 
    
    ThreeWayToggleSwitchState rockerSwitchA01State; 

    bool numPadButtonsSelectedState[NUM_PAD_BUTTONS_COUNT];

    bool isSafeToggleOut[SAFE_TOGGLES_COUNT]; 

    TwoWayToggleSwitchState toggleA03State; 
    
    ThreeWayToggleSwitchState toggleA04State; 
    
    TwoWayToggleSwitchState toggleA05State;

    TaskboardSlot1State()
    {
        for (int i = 0; i < NUM_PAD_BUTTONS_COUNT; i++)
        {
            numPadButtonsSelectedState[i] = false;
        }
        for (int i = 0; i < SAFE_TOGGLES_COUNT; i++)
        {
            isSafeToggleOut[i] = false;
        }
        isCoverOpen = false;
        powerSwitchState = eTwoWayState_Down;
        rockerSwitchA01State = eThreeWayState_Center;
        toggleA03State = eTwoWayState_Down;
        toggleA04State = eThreeWayState_Center;
        toggleA05State = eTwoWayState_Down;
    }
};

struct BaseManipulationState
{
    double startTime;
    double duration;

    BaseManipulationState()
    : startTime(0.0)
    , duration(0.0)
    {}
};
struct PowerCoverManipulationState : BaseManipulationState
{
    math::Vector3 angularVelocity;
};
struct PowerSwitchManipulationState : BaseManipulationState
{
    math::Vector3 angularVelocity;
};
struct RockerSwitchManipulationState : BaseManipulationState
{
    math::Vector3 torque;
};
struct NumPadButtonManipulationState : BaseManipulationState
{
    math::Vector3 force;
};
struct SafeToggleManipulationState : BaseManipulationState
{
    double value;
    
    SafeToggleManipulationState()
    : value(0.0)
    {}
};

struct GazeboTaskboardSlot1::ManipulationState
{
    PowerCoverManipulationState powerCover; 
    PowerSwitchManipulationState powerSwitch; 
    RockerSwitchManipulationState rockerSwitch; 
    NumPadButtonManipulationState numPadButtons[NUM_PAD_BUTTONS_COUNT]; 
    SafeToggleManipulationState safeToggles[SAFE_TOGGLES_COUNT][2]; 
};

inline double deg2rad(double degrees)
{
    return M_PI / 180.0 * degrees;
}

//----------------------------------------------------------------------------------------
GazeboTaskboardSlot1::GazeboTaskboardSlot1()
: firstFrameInitializationDone(false)
, ledsReady(false)
, node("taskboard")
{}

GazeboTaskboardSlot1::~GazeboTaskboardSlot1()
{
    if (updateConnection)
    {
        event::Events::DisconnectWorldUpdateStart(updateConnection);
        updateConnection.reset();
    }
}

void GazeboTaskboardSlot1::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
    model = _parent;
    if (!model)
    {
        ROS_FATAL("No model is provided for GazeboTaskboard model plugin");
        return;
    }
    // Initialize links and joints from taskboard model
    if (!InitLinks() || !InitJoints()) // this also prints ROS_FATAL message in case of error
    {
        return;
    }

    // Listen to the update event.This event is broadcasted every simulation iteration.
    updateConnection = event::Events::ConnectWorldUpdateStart(
        boost::bind(&GazeboTaskboardSlot1::OnUpdate, this));

    // Init services
    srv_manipulatePowerCover = node.advertiseService("manipulate_power_cover", 
                                                     &GazeboTaskboardSlot1::ManipulatePowerCover, this);
    srv_manipulatePowerSwitch = node.advertiseService("manipulate_power_switch", 
                                                     &GazeboTaskboardSlot1::ManipulatePowerSwitch, this);
    srv_manipulateRockerSwitch = node.advertiseService("manipulate_rocker_switch_a01", 
                                                     &GazeboTaskboardSlot1::ManipulateRockerSwitch, this);
    srv_manipulateNumPad = node.advertiseService("manipulate_numpad",
                                                     &GazeboTaskboardSlot1::ManipulateNumPad, this);
    srv_manipulateSafeToggle = node.advertiseService("manipulate_safe_toggle",
                                                     &GazeboTaskboardSlot1::ManipulateSafeToggle, this);

    // Initialize publisher
    const int queueSize = 128;
    publisher = node.advertise<TaskboardPanelA>("TaskboardPanelA", queueSize);

    // Initialize manipulation state
    manipulationState.reset(new ManipulationState);

    //  Initialize LEDs
    leds.reset(new TaskboardLeds(model->GetWorldPose()));

    // Initialize controller according to the model state
    state.reset(new TaskboardSlot1State);
    DeriveStateFromModel();
}

static physics::LinkPtr getLink(physics::ModelPtr model, const std::string& linkName)
{
    physics::LinkPtr link = model->GetLink(linkName);
    if (!link)
    {
        ROS_FATAL("Failed to get %s link from the model", linkName.c_str());
    }
    return link;
}

bool GazeboTaskboardSlot1::InitLinks()
{
    struct
    {
        physics::LinkPtr& linkRef;
        std::string linkName;
    } linksInfo[] = 
    {
        { linkPowerCover        , "taskboard_slot1_switch1_cover"   },
        { linkPowerSwitch       , "taskboard_slot1_switch1"         },
        { linkA01Switch         , "taskboard_slot1_switch"          },
        { linksNumPad[0]        , "taskboard_slot1_A1"              },
        { linksNumPad[1]        , "taskboard_slot1_A2"              },
        { linksNumPad[2]        , "taskboard_slot1_A3"              },
        { linksNumPad[3]        , "taskboard_slot1_B1"              },
        { linksNumPad[4]        , "taskboard_slot1_B2"              },
        { linksNumPad[5]        , "taskboard_slot1_B3"              },
        { linksNumPad[6]        , "taskboard_slot1_C1"              },
        { linksNumPad[7]        , "taskboard_slot1_C2"              },
        { linksNumPad[8]        , "taskboard_slot1_C3"              },
        { linksBaseSafeToggle[0], "taskboard_slot1_switch2_pullout" },
        { linksBaseSafeToggle[1], "taskboard_slot1_switch3_pullout" },
        { linksBaseSafeToggle[2], "taskboard_slot1_switch4_pullout" },
        { linksSafeToggle[0]    , "taskboard_slot1_switch2"         },
        { linksSafeToggle[1]    , "taskboard_slot1_switch3"         },
        { linksSafeToggle[2]    , "taskboard_slot1_switch4"         }
    };
    const int numLinks = sizeof(linksInfo) / sizeof(linksInfo[0]);
    for (int i = 0; i < numLinks; i++)
    {
        linksInfo[i].linkRef = getLink(model, linksInfo[i].linkName);
        if (!linksInfo[i].linkRef)
        {
            return false;
        }
    }
    return true;
}

static physics::JointPtr getJoint(physics::ModelPtr model, const std::string& jointName)
{
    physics::JointPtr joint = model->GetJoint(jointName);
    if (!joint)
    {
        ROS_FATAL("Failed to get %s joint from the model", jointName.c_str());
    }
    return joint;
}

bool GazeboTaskboardSlot1::InitJoints()
{
    struct
    {
        physics::JointPtr& jointRef;
        std::string jointName;
    } jointsInfo[] = 
    {
        { safeTogglesRevoluteJoints[0] , "taskboard_lower/taskboard_slot1_switch2" },
        { safeTogglesRevoluteJoints[1] , "taskboard_lower/taskboard_slot1_switch3" },
        { safeTogglesRevoluteJoints[2] , "taskboard_lower/taskboard_slot1_switch4" }
    };
    const int numJoints = sizeof(jointsInfo) / sizeof(jointsInfo[0]);
    for (int i = 0; i < numJoints; i++)
    {
        jointsInfo[i].jointRef = getJoint(model, jointsInfo[i].jointName);
        if (!jointsInfo[i].jointRef)
        {
            return false;
        }
    }
    return true;
}

void GazeboTaskboardSlot1::DeriveStateFromModel()
{
    // Initialize powerCoverState
    math::Pose pose = linkPowerCover->GetRelativePose();
    double angle = pose.rot.GetRoll();
    state->isCoverOpen = (angle < POWER_COVER_JOINT_THRESHOLD);

    // Initialize powerSwitchState.
    // If the power is on then turn on the leds that should be on in current state.
    pose = linkPowerSwitch->GetRelativePose();
    angle = pose.rot.GetRoll();
    state->powerSwitchState = (angle < M_PI_2) ? eTwoWayState_Up : eTwoWayState_Down;

    // Initialize toggleA03State
    pose = linksSafeToggle[0]->GetRelativePose();
    angle = pose.rot.GetRoll();
    state->toggleA03State = (angle < M_PI_2) ? eTwoWayState_Up : eTwoWayState_Down;

    // Initialize toggleA04State
    pose = linksSafeToggle[1]->GetRelativePose();
    angle = pose.rot.GetRoll();
    if (angle < M_PI_2 - SAFE_TOGGLE_HALF_ANGLE/2)
    {
        state->toggleA04State = eThreeWayState_Up;
    }
    else if (angle < M_PI_2 + SAFE_TOGGLE_HALF_ANGLE/2)
    {
        state->toggleA04State = eThreeWayState_Center;
    }
    else 
    {
        state->toggleA04State = eThreeWayState_Down;
    }

    // Initialize toggleA05State
    pose = linksSafeToggle[2]->GetRelativePose();
    angle = pose.rot.GetRoll();
    state->toggleA05State = (angle < M_PI_2) ? eTwoWayState_Up : eTwoWayState_Down;
}

void GazeboTaskboardSlot1::OnUpdate()
{
    // If condition is true then LED models are available and 
    // we should set them up properly
    if (firstFrameInitializationDone && !ledsReady)
    {
        leds->SetupModels(model->GetWorld());
        TurnOnLeds(); // turn on necessary leds
        ledsReady = true;
        return;
    }
    // Create LED models on the first update frame.
    // This is necessary since physics engine is not ready when Load is called 
    if (!firstFrameInitializationDone)
    {
        leds->CreateModels(model->GetWorld());
        firstFrameInitializationDone = true;
        return;
    }

    // Monitor state changes in taskboard elements and simulate expected behavior
    MonitorPowerCoverStateChanges();
    MonitorPowerSwitchStateChanges();
    MonitorRockerSwitchA01StateChanges();
    MonitorNumpadStateChanges();
    MonitorSafeTogglesStateChanges();

    // Process external manipulation
    HandleManipulation();

    // Spin the wheel! Make sure all topic messages are sent
    ros::spinOnce();
}

void GazeboTaskboardSlot1::MonitorPowerCoverStateChanges()
{
    math::Pose pose = linkPowerCover->GetRelativePose();
    const double angle = pose.rot.GetRoll();
    const double threshold = deg2rad(1.0);

    if (angle > threshold && angle < M_PI_2 - threshold)
    {
        math::Vector3 torque = computeEmpiricalTorque(angle - POWER_COVER_JOINT_THRESHOLD, 1.0, 1.0, 1.5, 15);
        linkPowerCover->SetTorque(torque);
    }
    else if (angle <= threshold)
    {
        if (!state->isCoverOpen)
        {
            state->isCoverOpen = true;
            PublishState();
            // Stabilize link
            linkPowerCover->SetAngularVel(ZERO_VECTOR);
            linkPowerCover->SetTorque(ZERO_VECTOR);
        }
    }
    else if (angle >= M_PI_2 - threshold)
    {
        if (state->isCoverOpen)
        {
            state->isCoverOpen = false;
            PublishState();
            // Stabilize link
            linkPowerCover->SetAngularVel(ZERO_VECTOR);
            linkPowerCover->SetTorque(ZERO_VECTOR);
        }
    }
}

void GazeboTaskboardSlot1::MonitorPowerSwitchStateChanges()
{
    const double halfAngle = 0.419; // According to URDF
    const double upDirLimit = M_PI/2 - halfAngle;
    const double downDirLimit = M_PI/2 + halfAngle;
    const double threshold = deg2rad(1.0);

    math::Pose pose = linkPowerSwitch->GetRelativePose();
    const double angle = pose.rot.GetRoll();

    if (angle > upDirLimit + threshold && angle < downDirLimit - threshold)
    {
        math::Vector3 torque = computeEmpiricalTorque(angle - M_PI_2, 1.0, 1.0, 1.3, 20);
        linkPowerSwitch->SetTorque(torque);
    }
    else if (angle <= upDirLimit + threshold)
    {
        if (state->powerSwitchState == eTwoWayState_Down)
        {
            state->powerSwitchState = eTwoWayState_Up;
            TurnOnLeds();
            PublishState();
            // Stabilize link
            linkPowerSwitch->SetAngularVel(ZERO_VECTOR);
            linkPowerSwitch->SetTorque(ZERO_VECTOR);
        }
    }
    else if (angle >= downDirLimit - threshold)
    {
        if (state->powerSwitchState == eTwoWayState_Up)
        {
            state->powerSwitchState = eTwoWayState_Down;
            TurnOffAllLeds();
            PublishState();
            // Stabilize link
            linkPowerSwitch->SetAngularVel(ZERO_VECTOR);
            linkPowerSwitch->SetTorque(ZERO_VECTOR);
        }
    }
}

void GazeboTaskboardSlot1::MonitorRockerSwitchA01StateChanges()
{
    const double halfAngle = 0.5; // According to URDF
    const double upStateLimit = M_PI_2 - halfAngle;
    const double downStateLimit = M_PI_2 + halfAngle;
    const double threshold = deg2rad(2.0);

    math::Pose pose = linkA01Switch->GetRelativePose();
    double angle = pose.rot.GetRoll();

    if (angle < M_PI_2 - threshold ||  angle > M_PI_2 + threshold)
    {
        // Switch is out of stable position, try to put it back
        const double sign = (angle < M_PI_2) ? 1.0 : -1.0;
        const double deviationAngle = sign * std::max(halfAngle - fabs(angle - M_PI_2), 0.0);
        math::Vector3 torque = computeEmpiricalTorque(deviationAngle, 8.5, 4.0, 2.5, 7);
        linkA01Switch->SetTorque(torque);

        // Check for new state (UP or DOWN)
        if (state->rockerSwitchA01State == eThreeWayState_Center)
        {
            if (angle < upStateLimit + threshold)
            {
                state->rockerSwitchA01State = eThreeWayState_Up;
                SetLedState(leds->rockerSwitchUpLed, true);
                PublishState();
            }
            else if (angle > downStateLimit - threshold)
            {
                state->rockerSwitchA01State = eThreeWayState_Down;
                SetLedState(leds->rockerSwitchDownLed, true);
                PublishState();
            }
        }
    }
    else 
    {
        // Check if switch just returned to center position
        if (state->rockerSwitchA01State != eThreeWayState_Center)
        {
            if (leds->rockerSwitchUpLed.isOn)
            {
                SetLedState(leds->rockerSwitchUpLed, false);
            }
            else if (leds->rockerSwitchDownLed.isOn)
            {
                SetLedState(leds->rockerSwitchDownLed, false);
            }
            linkA01Switch->SetAngularVel(ZERO_VECTOR); // Stabilize switch at center position
            state->rockerSwitchA01State = eThreeWayState_Center;
            PublishState();
        }
    }
}

void GazeboTaskboardSlot1::MonitorNumpadStateChanges()
{
    const double selRelOffset = -0.4811;
    const double epsilon = 0.0003;

    // Simulate springs which influence the buttons, so when we release
    // the button it returns to UNSEL position
    for (int i = 0; i < NUM_PAD_BUTTONS_COUNT; i++)
    {
        linksNumPad[i]->SetForce(ZERO_VECTOR);
        linksNumPad[i]->AddRelativeForce(math::Vector3(0, 0, 5));
    }
    // Check for state changes
    for (int i = 0; i < NUM_PAD_BUTTONS_COUNT; i++)
    {
        const double offset = linksNumPad[i]->GetRelativePose().pos.y;
        
        bool selected = (offset < selRelOffset + epsilon);
        if (selected ^ state->numPadButtonsSelectedState[i])
        {
            state->numPadButtonsSelectedState[i] = selected;
            if (selected)
            {
                SetLedState(leds->numPadLeds[i], !leds->numPadLeds[i].isOn);
            }
            PublishState();
        }
    }
}

bool GazeboTaskboardSlot1::UpdateTransitionFromOutState2Way(
    physics::LinkPtr link, physics::JointPtr joint, TwoWayToggleSwitchState& state) const
{
    const double halfAngle = 0.47; // according to joint specification
    const double threshold = deg2rad(1.0);
    const double upDirLimit = M_PI_2 - halfAngle;
    const double downDirLimit = M_PI_2 + halfAngle;

    math::Pose pose = link->GetRelativePose();
    const double angle = pose.rot.GetRoll();

    if (angle > upDirLimit + threshold && angle < downDirLimit - threshold)
    {
        math::Vector3 torque = computeEmpiricalTorque(angle - M_PI_2, 0.8, 1.0, 1.5, 15);
        link->SetTorque(torque);
        return false;
    }
    if (angle <= upDirLimit + threshold)
    {
        state = eTwoWayState_Up;
        // Latch the joint
        joint->SetHighStop(0, math::Angle(SAFE_TOGGLE_LOWER_LIMIT_ANGLE + threshold));
        joint->SetLowStop(0, math::Angle(SAFE_TOGGLE_LOWER_LIMIT_ANGLE - threshold));
    }
    else
    {
        state = eTwoWayState_Down;
        // Latch the joint
        joint->SetHighStop(0, math::Angle(SAFE_TOGGLE_UPPER_LIMIT_ANGLE + threshold));
        joint->SetLowStop(0, math::Angle(SAFE_TOGGLE_UPPER_LIMIT_ANGLE - threshold));
    }
    return true;
}

bool GazeboTaskboardSlot1::UpdateTransitionFromOutState3Way(
    physics::LinkPtr link, physics::JointPtr joint, ThreeWayToggleSwitchState& state) const
{
    const double halfAngle = 0.47;
    const double upDirLimit = M_PI/2 - halfAngle;
    const double downDirLimit = M_PI/2 + halfAngle;
    const double threshold = deg2rad(1.0);

    math::Pose pose = link->GetRelativePose();
    double angle = pose.rot.GetRoll();
    
    if (angle > upDirLimit + threshold && angle < M_PI_2 - threshold)
    {
        math::Vector3 torque = computeEmpiricalTorque(angle - (M_PI_2 - halfAngle/2), 0.8, 1.0, 1.5, 15);
        link->SetTorque(torque);
        return false;
    }
    else if (angle > M_PI_2 + threshold && angle < downDirLimit - threshold)
    {
        math::Vector3 torque = computeEmpiricalTorque(angle - (M_PI_2 + halfAngle/2), 0.8, 1.0, 1.5, 15);
        link->SetTorque(torque);
        return false;
    }
    if (angle <= upDirLimit + threshold)
    {
        state = eThreeWayState_Up;
        // Latch the joint
        joint->SetHighStop(0, math::Angle(SAFE_TOGGLE_LOWER_LIMIT_ANGLE + threshold));
        joint->SetLowStop(0, math::Angle(SAFE_TOGGLE_LOWER_LIMIT_ANGLE - threshold));
        link->SetTorque(ZERO_VECTOR);
    }
    else if (angle >= downDirLimit - threshold)
    {
        state = eThreeWayState_Down;
        // Latch the joint
        joint->SetHighStop(0, math::Angle(SAFE_TOGGLE_UPPER_LIMIT_ANGLE + threshold));
        joint->SetLowStop(0, math::Angle(SAFE_TOGGLE_UPPER_LIMIT_ANGLE - threshold));
        link->SetTorque(ZERO_VECTOR);
    } 
    else
    {
        state = eThreeWayState_Center;
        // Latch the joint
        joint->SetHighStop(0, math::Angle(SAFE_TOGGLE_MIDDLE_LIMIT_ANGLE + threshold));
        joint->SetLowStop(0, math::Angle(SAFE_TOGGLE_MIDDLE_LIMIT_ANGLE - threshold));
        link->SetTorque(ZERO_VECTOR);
    }
    return true;
}

void GazeboTaskboardSlot1::MonitorSafeTogglesStateChanges()
{
    // The pull out distance as defined in URDF files. 
    const double pullOutDistance = 0.002;
    // The delta below should be smaller then pull out offset.
    const double delta = 0.0004;
    
    // Check which joints is pulled out
    bool isOut[SAFE_TOGGLES_COUNT];
    float offsets[SAFE_TOGGLES_COUNT]; // toggles pulled out offsets
    for (int i = 0; i < SAFE_TOGGLES_COUNT; i++)
    {
        const math::Pose outPose = linksBaseSafeToggle[i]->GetRelativePose();
        const math::Pose pose = linksSafeToggle[i]->GetRelativePose();
        offsets[i] = std::max(pullOutDistance - outPose.CoordPositionSub(pose).z, 0.0);
        isOut[i] = (offsets[i] > pullOutDistance - delta);
    }
     // Set OUT state
    for (int i = 0; i < SAFE_TOGGLES_COUNT; i++)
    {
        if (!state->isSafeToggleOut[i] && isOut[i])
        {
            state->isSafeToggleOut[i] = true;
            
            // Release revolute joint
            safeTogglesRevoluteJoints[i]->SetHighStop(0, math::Angle(SAFE_TOGGLE_UPPER_LIMIT_ANGLE));
            safeTogglesRevoluteJoints[i]->SetLowStop(0, math::Angle(SAFE_TOGGLE_LOWER_LIMIT_ANGLE));
            
            // turn off LEDs
            switch (i)
            {
                case 0: // A03
                    SetLedState(leds->toggleA03Led, false);
                    break;
                case 1: // A04
                    SetLedState(leds->toggleA04TopLed, false);
                    SetLedState(leds->toggleA04BottomLed, false);
                    break;
                case 2: // A05
                    SetLedState(leds->toggleA05Led, false);
                    break;
            }
            PublishState();
        }
    }
    // Update two-way safe toggles that are in transition from OUT state
    if (state->isSafeToggleOut[0] && offsets[0] < pullOutDistance - 2*delta)
    {
        bool transitionDone = UpdateTransitionFromOutState2Way(
            linksSafeToggle[0], safeTogglesRevoluteJoints[0], state->toggleA03State);
        if (transitionDone)
        {
            state->isSafeToggleOut[0] = false;
            SetLedState(leds->toggleA03Led, state->toggleA03State == eTwoWayState_Up);
            PublishState();
        }
    }
    if (state->isSafeToggleOut[2] && offsets[2] < pullOutDistance - 2*delta)
    {
        bool transitionDone = UpdateTransitionFromOutState2Way(
            linksSafeToggle[2], safeTogglesRevoluteJoints[2], state->toggleA05State);
        if (transitionDone)
        {
            state->isSafeToggleOut[2] = false;
            SetLedState(leds->toggleA05Led, state->toggleA05State == eTwoWayState_Up);
            PublishState();
        }
    }
    // Update three-way safe toggles that are in transition from OUT state
    if (state->isSafeToggleOut[1] && offsets[1] < pullOutDistance - 2*delta)
    {
        bool transitionDone = UpdateTransitionFromOutState3Way(
            linksSafeToggle[1], safeTogglesRevoluteJoints[1], state->toggleA04State);
        if (transitionDone)
        {
            state->isSafeToggleOut[1] = false;
            SetLedState(leds->toggleA04TopLed, state->toggleA04State == eThreeWayState_Up);
            SetLedState(leds->toggleA04BottomLed, state->toggleA04State == eThreeWayState_Down);
            PublishState();
        }
    }
    // Simulate springs that pull the toggles in
    for (int i = 0; i < SAFE_TOGGLES_COUNT; i++)
    {
        if (state->isSafeToggleOut[i] || offsets[i] > delta)
        {
            linksSafeToggle[i]->SetForce(ZERO_VECTOR);
            linksSafeToggle[i]->AddRelativeForce(math::Vector3(0, 0, -2));
        }
        else if (offsets[i] <= delta/2)
        {
            linksSafeToggle[i]->SetForce(ZERO_VECTOR);
            linksSafeToggle[i]->SetLinearVel(ZERO_VECTOR);
            linksSafeToggle[i]->SetAngularVel(ZERO_VECTOR);
        }
    }
}

void GazeboTaskboardSlot1::PublishState()
{
    const std::string UP     = "UP";
    const std::string DOWN   = "DOWN";
    const std::string CENTER = "CENTER";
    const std::string ON     = "ON";
    const std::string OFF    = "OFF";
    const std::string SEL    = "SEL";
    const std::string UNSEL  = "UNSEL";
    const std::string OUT    = "OUT";

    // Initialize the message by collecting information from all elements
    TaskboardPanelA msg;

    msg.PANEL_POWER_COVER  = state->isCoverOpen ? UP : DOWN;
    msg.PANEL_POWER_SWITCH = state->powerSwitchState == eTwoWayState_Up ? UP : DOWN;
    msg.PANEL_POWER_LED    = leds->powerSwitchLed.isOn ? ON : OFF;
    
    msg.A01_ROCKER_SWITCH  = state->rockerSwitchA01State == eThreeWayState_Center
                              ? CENTER 
                              : (state->rockerSwitchA01State == eThreeWayState_Up ? UP : DOWN);

    msg.A01_ROCKER_LED_TOP    = leds->rockerSwitchUpLed.isOn ? ON : OFF;
    msg.A01_ROCKER_LED_BOTTOM = leds->rockerSwitchDownLed.isOn ? ON : OFF;

    msg.A02_LED_NUM_PAD_A1 = leds->numPadLeds[0].isOn ? ON : OFF;
    msg.A02_LED_NUM_PAD_A2 = leds->numPadLeds[1].isOn ? ON : OFF;
    msg.A02_LED_NUM_PAD_A3 = leds->numPadLeds[2].isOn ? ON : OFF;
    msg.A02_LED_NUM_PAD_B1 = leds->numPadLeds[3].isOn ? ON : OFF;
    msg.A02_LED_NUM_PAD_B2 = leds->numPadLeds[4].isOn ? ON : OFF;
    msg.A02_LED_NUM_PAD_B3 = leds->numPadLeds[5].isOn ? ON : OFF;
    msg.A02_LED_NUM_PAD_C1 = leds->numPadLeds[6].isOn ? ON : OFF;
    msg.A02_LED_NUM_PAD_C2 = leds->numPadLeds[7].isOn ? ON : OFF;
    msg.A02_LED_NUM_PAD_C3 = leds->numPadLeds[8].isOn ? ON : OFF;

    msg.A02_NUM_PAD_A1 = state->numPadButtonsSelectedState[0] ? SEL : UNSEL;
    msg.A02_NUM_PAD_A2 = state->numPadButtonsSelectedState[1] ? SEL : UNSEL;
    msg.A02_NUM_PAD_A3 = state->numPadButtonsSelectedState[2] ? SEL : UNSEL;
    msg.A02_NUM_PAD_B1 = state->numPadButtonsSelectedState[3] ? SEL : UNSEL;
    msg.A02_NUM_PAD_B2 = state->numPadButtonsSelectedState[4] ? SEL : UNSEL;
    msg.A02_NUM_PAD_B3 = state->numPadButtonsSelectedState[5] ? SEL : UNSEL;
    msg.A02_NUM_PAD_C1 = state->numPadButtonsSelectedState[6] ? SEL : UNSEL;
    msg.A02_NUM_PAD_C2 = state->numPadButtonsSelectedState[7] ? SEL : UNSEL;
    msg.A02_NUM_PAD_C3 = state->numPadButtonsSelectedState[8] ? SEL : UNSEL;
    
    msg.A03_LED        = leds->toggleA03Led.isOn ? ON : OFF;
    msg.A04_LED_TOP    = leds->toggleA04TopLed.isOn ? ON : OFF;
    msg.A04_LED_BOTTOM = leds->toggleA04BottomLed.isOn ? ON : OFF;
    msg.A05_LED        = leds->toggleA05Led.isOn ? ON : OFF;

    msg.A03_TOGGLE = state->isSafeToggleOut[0] 
                      ? OUT 
                      : (state->toggleA03State == eTwoWayState_Up ? UP : DOWN);

    msg.A04_TOGGLE = state->isSafeToggleOut[1] 
                      ? OUT 
                      : (state->toggleA04State == eThreeWayState_Center
                          ? CENTER
                          : (state->toggleA04State == eThreeWayState_Up ? UP : DOWN));

    msg.A05_TOGGLE = state->isSafeToggleOut[2] 
                      ? OUT 
                      : (state->toggleA05State == eTwoWayState_Up ? UP : DOWN);

    // Finally publish the message
    publisher.publish(msg);
}

void GazeboTaskboardSlot1::SetLedState(Led& led, bool on)
{
    // Do nothing if the led is already in the requested state
    if (led.isOn == on)
    {
        return;
    }
    // Do not turn LED on if the power switch is off
    if (state->powerSwitchState != eTwoWayState_Up && on)
    {
        return;
    }

    // Get current pose
    math::Pose pose = led.onModel->GetRelativePose();
    math::Pose pose2 = led.offModel->GetRelativePose();
    
    // Update pose based on new LED state
    if (on)
    {
        pose.pos.z -= LED_HIDE_OFFSET;
        pose2.pos.z += LED_HIDE_OFFSET2;
    }
    else
    {
        pose.pos.z += LED_HIDE_OFFSET;
        pose2.pos.z -= LED_HIDE_OFFSET2;
    }
    led.onModel->SetLinkWorldPose(pose, led.onLinkName);
    led.offModel->SetLinkWorldPose(pose2, led.offLinkName);
    led.isOn = on;
}

void GazeboTaskboardSlot1::TurnOnLeds()
{
    if (state->powerSwitchState == eTwoWayState_Up)
    {
        // Power switch LED
        SetLedState(leds->powerSwitchLed, true);
        
        // Rocker switch LEDs
        if (state->rockerSwitchA01State == eThreeWayState_Up)
        {
            SetLedState(leds->rockerSwitchUpLed, true);
        }
        else if (state->rockerSwitchA01State == eThreeWayState_Down)
        {
            SetLedState(leds->rockerSwitchDownLed, true);
        }
        // A03 LED
        if (state->toggleA03State == eTwoWayState_Up)
        {
            SetLedState(leds->toggleA03Led, true);
        }
        // A05 LED
        if (state->toggleA05State == eTwoWayState_Up)
        {
            SetLedState(leds->toggleA05Led, true);
        }
        // A04 LEDs
        if (state->toggleA04State == eThreeWayState_Up)
        {
            SetLedState(leds->toggleA04TopLed, true);
        }
        else if (state->toggleA04State == eThreeWayState_Down)
        {
            SetLedState(leds->toggleA04BottomLed, true);
        }
    }
}

void GazeboTaskboardSlot1::TurnOffAllLeds()
{
    SetLedState(leds->powerSwitchLed, false);
    SetLedState(leds->rockerSwitchUpLed, false);
    SetLedState(leds->rockerSwitchDownLed, false);
    SetLedState(leds->toggleA03Led, false);
    SetLedState(leds->toggleA05Led, false);
    SetLedState(leds->toggleA04TopLed, false);
    SetLedState(leds->toggleA04BottomLed, false);

    for (int i = 0; i < NUM_PAD_BUTTONS_COUNT; i++)
    {
        SetLedState(leds->numPadLeds[i], false);
    }
}

//----------------------------------------------------------------------------------------
// Taskboard Manipulation
//----------------------------------------------------------------------------------------
bool GazeboTaskboardSlot1::ManipulatePowerCover(
    ManipulatePowerCover::Request& request, ManipulatePowerCover::Response& response)
{
    math::Pose globalPose = model->GetWorldPose();
    const double velocity = request.angle / request.duration;
    
    PowerCoverManipulationState &manipState = manipulationState->powerCover;

    manipState.duration = request.duration;
    manipState.startTime = GetTime();
    manipState.angularVelocity = globalPose.rot * math::Vector3(velocity, 0, 0);

    linkPowerCover->SetAngularVel(manipState.angularVelocity);
    return true;
}

bool GazeboTaskboardSlot1::ManipulatePowerSwitch(
    ManipulatePowerSwitch::Request& request, ManipulatePowerSwitch::Response& response)
{
    math::Pose globalPose = model->GetWorldPose();
    const double velocity = request.angle / request.duration;
    
    PowerSwitchManipulationState &manipState = manipulationState->powerSwitch;

    manipState.duration = request.duration;
    manipState.startTime = GetTime();
    manipState.angularVelocity = globalPose.rot * math::Vector3(velocity, 0, 0);

    linkPowerSwitch->SetAngularVel(manipState.angularVelocity);
    return true;
}

bool GazeboTaskboardSlot1::ManipulateRockerSwitch(
    ManipulateRockerSwitch::Request& request, ManipulateRockerSwitch::Response& response)
{
    math::Pose globalPose = model->GetWorldPose();
    
    RockerSwitchManipulationState &manipState = manipulationState->rockerSwitch;

    manipState.startTime = GetTime();
    manipState.duration = request.duration;
    manipState.torque = globalPose.rot * math::Vector3(request.torque, 0, 0);

    linkA01Switch->SetTorque(manipState.torque);
    return true;
}

bool GazeboTaskboardSlot1::ManipulateNumPad(
    ManipulateNumPad::Request& request, ManipulateNumPad::Response& response)
{
    if (request.index < 0 || request.index >= NUM_PAD_BUTTONS_COUNT)
    {
        return false;
    }
    NumPadButtonManipulationState &manipState = manipulationState->numPadButtons[request.index];
    
    manipState.startTime = GetTime();
    manipState.duration = request.duration;
    manipState.force = math::Vector3(0, 0, -request.force);

    linksNumPad[request.index]->AddRelativeForce(manipState.force);
    return true;
}

bool GazeboTaskboardSlot1::ManipulateSafeToggle(
    ManipulateSafeToggle::Request& request, ManipulateSafeToggle::Response& response)
{
    if (request.index < 0 || request.index >= SAFE_TOGGLES_COUNT)
    {
        return false;
    }
    if (request.mode < 0 || request.mode >= 2)
    {
        return false;
    }
    SafeToggleManipulationState &manipState = manipulationState->safeToggles[request.index][request.mode];

    manipState.startTime = GetTime();
    manipState.duration = request.duration;
    manipState.value = request.value;

    physics::LinkPtr link = linksSafeToggle[request.index];
    if (request.mode == 0)
    {
        link->SetForce(ZERO_VECTOR);
        link->AddRelativeForce(math::Vector3(0, 0, manipState.value));
    }
    else
    {
        const double velocity = manipState.value / manipState.duration;
        math::Pose basePose = model->GetWorldPose();
        math::Vector3 angularVelocity =  basePose.rot * math::Vector3(velocity, 0, 0);
        link->SetAngularVel(angularVelocity);
    }
    return true;
}

void GazeboTaskboardSlot1::HandleManipulation()
{
    const double time = GetTime();
    // Check power cover
    if (manipulationState->powerCover.startTime)
    {
        const double elapsedTime = time - manipulationState->powerCover.startTime;
        if (elapsedTime >= manipulationState->powerCover.duration)
        {
            linkPowerCover->SetAngularVel(ZERO_VECTOR);
            manipulationState->powerCover.startTime = 0.0;
        }
        else 
        {
            linkPowerCover->SetAngularVel(manipulationState->powerCover.angularVelocity);
        }
    }
    // Check power switch
    if (manipulationState->powerSwitch.startTime)
    {
        const double elapsedTime = time - manipulationState->powerSwitch.startTime;
        if (elapsedTime >= manipulationState->powerSwitch.duration)
        {
            linkPowerSwitch->SetAngularVel(ZERO_VECTOR);
            manipulationState->powerSwitch.startTime = 0.0;
        }
        else 
        {
            linkPowerSwitch->SetAngularVel(manipulationState->powerSwitch.angularVelocity);
        }
    }
    // Check rocker switch A01
    if (manipulationState->rockerSwitch.startTime)
    {
        const double elapsedTime = time - manipulationState->rockerSwitch.startTime;
        if (elapsedTime >= manipulationState->rockerSwitch.duration)
        {
            linkA01Switch->SetTorque(ZERO_VECTOR);
            manipulationState->rockerSwitch.startTime = 0.0;
        }
        else
        {
            linkA01Switch->SetTorque(manipulationState->rockerSwitch.torque);
        }
    }
    // Check num pad buttons
    for (int i = 0; i < NUM_PAD_BUTTONS_COUNT; i++)
    {
        NumPadButtonManipulationState &manipState = manipulationState->numPadButtons[i];
        if (manipState.startTime)
        {
            const double elapsedTime = time - manipState.startTime;
            if (elapsedTime >= manipState.duration)
            {
                manipState.startTime = 0.0;
            }
            else
            {
                linksNumPad[i]->AddRelativeForce(manipState.force);
            }
        }
    }
    // Check safe toggles
    for (int i = 0; i < SAFE_TOGGLES_COUNT; i++)
    {
        SafeToggleManipulationState &manipState = manipulationState->safeToggles[i][0];
        if (manipState.startTime)
        {
            physics::LinkPtr link = linksSafeToggle[i];
            const double elapsedTime = time - manipState.startTime;
            if (elapsedTime >= manipState.duration) // reset manipulation influence
            {
                link->SetForce(ZERO_VECTOR);
                manipState.startTime = 0.0;
            }
            else // refresh manipulation influence
            {
                link->SetForce(ZERO_VECTOR);
                link->AddRelativeForce(math::Vector3(0, 0,  manipState.value));
            }
        }
        SafeToggleManipulationState &manipState2 = manipulationState->safeToggles[i][1];
        if (manipState2.startTime)
        {
            physics::LinkPtr link = linksSafeToggle[i];
            const double elapsedTime = time - manipState2.startTime;
            if (elapsedTime >= manipState2.duration) // reset manipulation influence
            {
                link->SetAngularVel(ZERO_VECTOR);
                link->SetLinearVel(ZERO_VECTOR);
                manipState2.startTime = 0.0;
            }
            else // refresh manipulation influence
            {
                const double velocity = manipState2.value / manipState2.duration;
                math::Pose basePose = model->GetWorldPose();
                math::Vector3 angularVelocity =  basePose.rot * math::Vector3(velocity, 0, 0);
                link->SetAngularVel(angularVelocity);
            }
        }
    }
}

math::Vector3 GazeboTaskboardSlot1::computeEmpiricalTorque(
    double deviationAngle, double initialValue,
    double snapCoeff, double snapExp, double torqueCoeff) const
{
    // Horizontal direction along taskboard, it's an axis of rotation for the most switches.
    const math::Vector3 axis = model->GetWorldPose().rot * math::Vector3(1, 0, 0);
    // Rotation direction
    const double sign = (deviationAngle < 0) ? 1.0 : -1.0;
    // Torque amount expression for physically plausible behavior
    const double torqueValue = (initialValue + pow(snapCoeff * fabs(deviationAngle), snapExp)) * sign * torqueCoeff;
    return axis * torqueValue;
}

double GazeboTaskboardSlot1::GetTime() const
{
    return model->GetWorld()->GetSimTime().Double();
}

//----------------------------------------------------------------------------------------
GazeboTaskboardSlot1::Led::Led()
: isOn(false)
, index(-1)
, color(eLedColor_Green)
{}

GazeboTaskboardSlot1::Led::Led(int index_, bool numPadLed_, LedColor color_, math::Pose pose_)
: isOn(false)
, index(index_)
, numPadLed(numPadLed_)
, color(color_)
, pose(pose_)
{}

void GazeboTaskboardSlot1::Led::CreateModel(physics::WorldPtr world)
{
    const std::string sdfTemplate =
        "<gazebo version ='1.0'>\
            <model name ='MODELNAME' static='1'>\
                <origin pose='%1% %2% ZPOS %3% %4% %5%'/>\
                <link name ='LINKNAME' kinematic='1'>\
                    <collision name='COLLISIONNAME'>\
                        <geometry>GEOMETRY</geometry>\
                    </collision>\
                    <visual name='VISUALNAME'>\
                        <geometry>GEOMETRY</geometry>\
                        <material script='MATERIAL'/>\
                    </visual>\
                    <inertial mass='0.5'> \
                        <inertia ixx='1' ixy='0.0' ixz='0.0' iyy='1' iyz='0.0' izz='1'/>\
                    </inertial>\
                </link>\
            </model>\
        </gazebo>";

    // Figure out LED's material based on its color
    const std::string material = (color == eLedColor_Green) ? LED_MATERIAL_GREEN
                                                            : LED_MATERIAL_BLUE;
    // Create model and link names
    const std::string ledNameBase = boost::str(boost::format("taskboardLed%1%") % index);
    const std::string ledLinkNameBase = boost::str(boost::format("taskboardLedLink%1%") % index);
    const std::string ledCollisionNameBase = boost::str(boost::format("taskboardLedCollision%1%") % index);
    const std::string ledVisualNameBase = boost::str(boost::format("taskboardLedVisual%1%") % index);

    const std::string ledName = ledNameBase + "_ON";
    const std::string ledName2 = ledNameBase + "_OFF";
    const std::string ledLinkName = ledLinkNameBase + "_ON";
    const std::string ledLinkName2 = ledLinkNameBase + "_OFF";
    const std::string ledCollisionName = ledCollisionNameBase + "_ON";
    const std::string ledCollisionName2 = ledCollisionNameBase + "_OFF";
    const std::string ledVisualName = ledVisualNameBase + "_ON";
    const std::string ledVisualName2 = ledVisualNameBase + "_OFF";

    // Get LED model description depending on LED type
    std::string ledModel;
    ledModel = boost::str(boost::format("<cylinder length='%1%' radius='%2%'/>") 
                              % (numPadLed ? NUMPAD_LED_LENGTH : SIMPLE_LED_LENGTH)
                              % (numPadLed ? NUMPAD_LED_RADIUS : SIMPLE_LED_RADIUS));

    // Prepare template
    double roll = pose.rot.GetRoll();
    double pitch = pose.rot.GetPitch();
    double yaw = pose.rot.GetYaw();
    std::string xml = boost::str(boost::format(sdfTemplate) 
                                 % pose.pos.x % pose.pos.y 
                                 % roll % pitch % yaw);
    std::string xml2 = xml;

    // SDF xml for turned on LED
    boost::replace_all(xml, "MODELNAME", ledName); 
    boost::replace_all(xml, "ZPOS", boost::str(boost::format("%1%") % (pose.pos.z + LED_HIDE_OFFSET))); 
    boost::replace_all(xml, "LINKNAME", ledLinkName);
    boost::replace_all(xml, "COLLISIONNAME", ledCollisionName);
    boost::replace_all(xml, "VISUALNAME", ledVisualName);
    boost::replace_all(xml, "GEOMETRY", ledModel);
    boost::replace_all(xml, "MATERIAL", material);

    // SDF xml for turned off LED
    boost::replace_all(xml2, "MODELNAME", ledName2);
    boost::replace_all(xml2, "ZPOS", boost::str(boost::format("%1%") % (pose.pos.z + LED_HIDE_OFFSET2))); 
    boost::replace_all(xml2, "LINKNAME", ledLinkName2);
    boost::replace_all(xml2, "COLLISIONNAME", ledCollisionName2);
    boost::replace_all(xml2, "VISUALNAME", ledVisualName2);
    boost::replace_all(xml2, "GEOMETRY", ledModel);
    boost::replace_all(xml2, "MATERIAL", LED_MATERIAL_OFF);

    // Send request to physics engine to create LED models
    sdf::SDF ledSDF;
    ledSDF.SetFromString(xml);
    
    sdf::SDF ledSDF2;
    ledSDF2.SetFromString(xml2);

#if ROS_VERSION_MINIMUM(1, 9, 0)
    // ROS Groovy or above
    world->InsertModelSDF(ledSDF);
    world->InsertModelSDF(ledSDF2);
#else
    // ROS Fuerte
    world->InsertModel(ledSDF);
    world->InsertModel(ledSDF2);
#endif

    // Initialize led structure.
    // NOTE: at this moment we don't initialize led.onModel, led.offModel:
    // they will be available only after physics engine does next step
    onModelName = ledName;
    onLinkName = ledLinkName;
    offModelName = ledName2;
    offLinkName = ledLinkName2;
}

void GazeboTaskboardSlot1::Led::SetupModel(physics::WorldPtr world)
{
    if (!onModel)
    {
        onModel = world->GetModel(onModelName);
        if (!onModel)
        {
            ROS_FATAL("SetupModel: failed to get model %s", onModelName.c_str());
            return;
        }
        onModel->SetCollideMode("none");

        offModel = world->GetModel(offModelName);
        if (!offModel)
        {
            ROS_FATAL("SetupModel: failed to get model %s", offModelName.c_str());
            return;
        }
        offModel->SetCollideMode("none");

        math::Pose pose2 = offModel->GetRelativePose();
        pose2.pos.z -= LED_HIDE_OFFSET2;
        offModel->SetWorldPose(pose2);
    }
}

//----------------------------------------------------------------------------------------
GazeboTaskboardSlot1::TaskboardLeds::TaskboardLeds(const math::Pose& modelPose)
{
    struct LedsInfo {
        Led& led;
        bool numPadLed;
        LedColor color;
        math::Vector3 offset;
    } ledsInfo[] = {
        { powerSwitchLed,       false,  eLedColor_Green,  math::Vector3(0.3250, -0.4720, 1.4110) },
        { rockerSwitchUpLed,    false,  eLedColor_Green,  math::Vector3(0.3245, -0.4720, 1.3340) },
        { rockerSwitchDownLed,  false,  eLedColor_Blue,   math::Vector3(0.3245, -0.4720, 1.2834) },
        { numPadLeds[0],        true,   eLedColor_Blue,   math::Vector3(0.4496, -0.4755, 1.3358) },
        { numPadLeds[1],        true,   eLedColor_Green,  math::Vector3(0.4230, -0.4755, 1.3358) },
        { numPadLeds[2],        true,   eLedColor_Blue,   math::Vector3(0.3964, -0.4755, 1.3358) },
        { numPadLeds[3],        true,   eLedColor_Green,  math::Vector3(0.4496, -0.4755, 1.3095) },
        { numPadLeds[4],        true,   eLedColor_Blue,   math::Vector3(0.4230, -0.4755, 1.3095) },
        { numPadLeds[5],        true,   eLedColor_Green,  math::Vector3(0.3964, -0.4755, 1.3095) },
        { numPadLeds[6],        true,   eLedColor_Blue,   math::Vector3(0.4496, -0.4755, 1.2833) },
        { numPadLeds[7],        true,   eLedColor_Green,  math::Vector3(0.4230, -0.4755, 1.2833) },
        { numPadLeds[8],        true,   eLedColor_Blue,   math::Vector3(0.3964, -0.4755, 1.2833) },
        { toggleA03Led,         false,  eLedColor_Green,  math::Vector3(0.4449, -0.4720, 1.2365) },
        { toggleA04TopLed,      false,  eLedColor_Green,  math::Vector3(0.3812, -0.4720, 1.2365) },
        { toggleA04BottomLed,   false,  eLedColor_Blue,   math::Vector3(0.3812, -0.4720, 1.1855) },
        { toggleA05Led,         false,  eLedColor_Green,  math::Vector3(0.3185, -0.4720, 1.2365) }
    };
    const int ledsCount = sizeof(ledsInfo) / sizeof(ledsInfo[0]);
    const math::Quaternion rot = math::Quaternion(0.707, 0.707, 0, 0);

    for (int i = 0; i < ledsCount; i++)
    {
        const LedsInfo &info = ledsInfo[i];
        const math::Pose pose(modelPose.pos + modelPose.rot * info.offset, modelPose.rot * rot);
        info.led = Led(i, info.numPadLed, info.color, pose);
    }
}

void GazeboTaskboardSlot1::TaskboardLeds::CreateModels(physics::WorldPtr world)
{
    for (int i = 0; i < NUM_PAD_BUTTONS_COUNT; i++)
    {
        numPadLeds[i].CreateModel(world);
    }
    powerSwitchLed.CreateModel(world);
    rockerSwitchUpLed.CreateModel(world);
    rockerSwitchDownLed.CreateModel(world);
    toggleA03Led.CreateModel(world);
    toggleA04TopLed.CreateModel(world);
    toggleA04BottomLed.CreateModel(world);
    toggleA05Led.CreateModel(world);
}

void GazeboTaskboardSlot1::TaskboardLeds::SetupModels(physics::WorldPtr world)
{
    for (int i = 0; i < NUM_PAD_BUTTONS_COUNT; i++)
    {
        numPadLeds[i].SetupModel(world);
    }
    powerSwitchLed.SetupModel(world);
    rockerSwitchUpLed.SetupModel(world);
    rockerSwitchDownLed.SetupModel(world);
    toggleA03Led.SetupModel(world);
    toggleA04TopLed.SetupModel(world);
    toggleA04BottomLed.SetupModel(world);
    toggleA05Led.SetupModel(world);
}