PowerCubeCtrl_OROCOS.cpp

Go to the documentation of this file.

/*
 * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0

 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */


#include "PowerCubeCtrl_OROCOS.h"

using namespace RTT;

PowerCubeCtrl_OROCOS::PowerCubeCtrl_OROCOS(std::string name, std::string xmlFile, int dof)
: TaskContext(name),
        m_xmlFile(xmlFile),
        m_dof(dof),
        // initialize dataports:
        m_in_Angles("in_Angles"),
        m_in_Velocities("in_Velocities"),
        m_in_Currents("in_Currents"),
        m_out_Angles("out_Angles"),  // note: initial value
        m_out_Velocities("out_Velocities"),  // note: initial value
        // initialize method
        m_stop_method("stop_movement",&PowerCubeCtrl_OROCOS::stopArm, this),
        //m_moveJointSpace_method("moveJointSpace", &PowerCubeCtrl_OROCOS::moveJointSpace, this),
        m_CanDev_prop("CanDev_n", "Can Device No.", 0),
        m_CanBaud_prop("BaudRate", "Can Baud Rate", 1000),
        m_dof_prop("DOF", "Degrees of Freedom", 7),
        m_MaxVel_prop("Max_Vel", "Maximum Velocity", 0.5),
        m_MaxAcc_prop("Max_Acc", "Maximum Acceleration", 0.8),
        m_powercubectrl()
{
    this->ports()->addPort( &m_in_Angles, "Port for desired Angles" );
    m_in_Angles_connected = false;
    this->ports()->addPort( &m_in_Velocities, "Port for desired velocities" );
    m_in_Velocities_connected = false;
    this->ports()->addPort( &m_in_Currents, "Port for desired currents" );
    m_in_Currents_connected = false;

    this->ports()->addPort( &m_out_Angles, "Port for current positions" );
    this->ports()->addPort( &m_out_Velocities, "Port for current velocities" );

    this->methods()->addMethod(&m_stop_method, "Stop the Arm");
    //this->methods()->addMethod(&m_moveJointSpace_method, "execute synchronized ramp commands");

        this->properties()->addProperty(&m_CanDev_prop);
        this->properties()->addProperty(&m_CanBaud_prop);
        this->properties()->addProperty(&m_dof_prop);
        this->properties()->addProperty(&m_MaxVel_prop);
        this->properties()->addProperty(&m_MaxAcc_prop);

        //m_mod_params.clear();

        m_modId_props.clear();
        m_ul_props.clear();
        m_ll_props.clear();
        m_offset_props.clear();

        for (int i=0; i<m_dof; i++)
        {
                ostringstream os;

                os << "modId" << i+1;
                m_modId_props.push_back( Property<int>( os.str(), os.str(), 0 ) );

                os.str("");     os.clear();
                os << "upperLimit" << i+1;
                m_ul_props.push_back( Property<double>( os.str(), os.str(), 0.0 ) );

                os.str("");     os.clear();
                os << "lowerLimit" << i+1;
                m_ll_props.push_back( Property<double>( os.str(), os.str(), 0.0 ) );

                os.str("");     os.clear();
                os << "offset" << i+1;
                m_offset_props.push_back( Property<double>( os.str(), os.str(), 0.0 ) );

        }

        m_mod_params.clear();
        for (int i=0; i<m_dof; i++)
        {
                ostringstream os;
                os << "modParams" << i+1;

                PropertyBag bag( os.str() );
                bag.add( &m_modId_props[i] );
                bag.add( &m_ul_props[i] );
                bag.add( &m_ll_props[i] );
                bag.add( &m_offset_props[i] );

                Property<PropertyBag>  prop_of_bag( os.str(), "", bag);

                m_mod_params.push_back( prop_of_bag );
        }

        for (int i=0; i<m_dof; i++)
        {
                this->properties()->addProperty( &(m_mod_params[i]) );
        }
    //m_stopped = false;
}

PowerCubeCtrl_OROCOS::~PowerCubeCtrl_OROCOS()
{
        stop();
}

bool PowerCubeCtrl_OROCOS::configureHook()
{
        this->marshalling()->readProperties(m_xmlFile);

        // COPY PROPERTIES INTO STRUCT POWERCUBEPARAMETERS

        PowerCubeParameters pcparams;

        pcparams.modIds.resize(m_dof);
        pcparams.upperlimits.resize(m_dof);
        pcparams.lowerlimits.resize(m_dof);
        pcparams.offsets.resize(m_dof);

        pcparams.dof=m_dof_prop.get();
        pcparams.CanDevice=m_CanDev_prop.get();
        pcparams.BaudRate=m_CanBaud_prop.get();
        pcparams.maxVel=m_MaxVel_prop.get();
        pcparams.maxAcc=m_MaxAcc_prop.get();

        int i;
        if(m_dof != pcparams.dof)
        {
                log(Info) << "Degrees of freedom" << m_dof << "doesn't match with xml input DOF." << pcparams.dof << endlog();
            return false;
        }

        for (i=0 ; i< (m_dof) ; i++)
        {
                pcparams.modIds[i] = m_modId_props[i].get();
                pcparams.upperlimits[i] = m_ul_props[i].get();
                pcparams.lowerlimits[i] = m_ll_props[i].get();
                pcparams.offsets[i] = m_offset_props[i].get();
        }

//      int id = m_modId_props[3].get();

        if ( m_powercubectrl.Init(pcparams) )
        {
                log(Info) << "PowerCubeCtrl initialized successfully." << endlog();
                return true;
        }
        else
        {
                log(Info) << "Error while initializing PowerCubeCtrl:" << endlog();
                log(Info) << m_powercubectrl.getErrorMessage() <<  endlog();
                return false;
        }
        return true;
}

void PowerCubeCtrl_OROCOS::stopHook()
{
        //m_stopped = true;
        stopArm();
}

bool PowerCubeCtrl_OROCOS::startHook()
{
    if ( m_in_Angles.connected() )
    {
        // only one of the input ports should be used simultaniously
        if ( m_in_Velocities.connected() || m_in_Currents.connected() ) {
                log(Info) << "Error in PowerCubeSim.startHook(): more than one input port is connected!" << endlog();
                return false;
        }
        m_in_Angles_connected = true;
        log(Info) << "PowerCubeSim succesfully detected connection at in_Angles" << endlog();
        //m_stopped = false;
            return true;
    }
    if ( m_in_Velocities.connected() )
    {
        // only one of the input ports should be used simultaniously
        if ( m_in_Angles.connected() || m_in_Currents.connected() ) {
                log(Info) << "Error in PowerCubeSim.startHook(): more than one input port is connected!" << endlog();
                return false;
        }
        m_in_Velocities_connected = true;
        log(Info) << "PowerCubeSim succesfully detected connection at in_Velocities" << endlog();
        //m_stopped = false;
        return true;
    }
    if ( m_in_Currents.connected() )
    {
        log(Info) << "Error, port \"setCur_R\" is connected to PowerCubeSim_OROCOS.\"";
        log(Info) << "Current movement is not yet supported by PowerCubeSim." << endlog();
        return false;
        /*
        // only one of the input ports should be used simultaniously
        if ( m_in_Angles.connected() || m_in_Velocities.connected() ) {
                log(Info) << "Error in PowerCubeSim.startHook(): more than one input port is connected!" << endlog();
                return false;
        }
        m_in_Current_connected = true;
        //m_stopped = false;
            return true;
            */
    }
    else
    {
        log(Info) << "No Input port is connected, PowerCubeSim will only return the current state." << endlog();
    }
    return true;
}

void PowerCubeCtrl_OROCOS::updateHook()
{
        // log(Info) << "updateHook is being executed." << endlog();

        // Execute desired movements:
        if ( m_in_Angles_connected )
        {
                std::vector<double> angles_desired(7);
                angles_desired = m_in_Angles.Get();
            m_powercubectrl.MovePos( angles_desired );
        }

        if ( m_in_Velocities_connected )
        {
                std::vector<double> vel_desired(7);
                vel_desired = m_in_Velocities.Get();
            m_powercubectrl.MoveVel( vel_desired );
        }
        if ( m_in_Currents_connected )
        {
                std::vector<double> currents_desired(7);
                currents_desired = m_in_Currents.Get();
            m_powercubectrl.MoveCur( currents_desired );
        }

    // get current angles & velocities
    std::vector<double> curConfig(7);
        m_powercubectrl.getConfig(curConfig);
    m_out_Angles.Set(curConfig);

    std::vector<double> curVelocities(7);
    m_powercubectrl.getJointVelocities(curVelocities);
    m_out_Velocities.Set(curVelocities);
}

bool PowerCubeCtrl_OROCOS::stopArm()
{
    //stop
    m_powercubectrl.Stop();
    return true;
}

bool PowerCubeCtrl_OROCOS::moveJointSpace(std::vector<double> target)
{
        return m_powercubectrl.MoveJointSpaceSync(target);
}

bool PowerCubeCtrl_OROCOS::isArmStopped()
{
        return !m_powercubectrl.statusMoving();
}