Simple Sine Movement Component

In the first example, a simple sine movement is implemented as CCA component (C++) that is locally connected to the Oncilla CCA Interface. The example can be executed by opening the world file Example2.wbt installed by the Oncilla Project Wizard.

In order to move the robot, the component has eight ports of JointAngles type, that are connected to the appropriate ports of the Oncilla CCA Interface to move the four L1 and L2 (hip and knee) joints of oncilla.

Single steps:

1. A component is created by deriving frome the CCA base node (line 27)
2. The component creates eight ports of type JointAngles to send the commands to the eight L1 and L2 joints (lines 37-44).
3. After calculating the desired commands, JointAngle objects with the commands are created and published over the ports (lines 56-64)

#include <iostream>
#include <fstream>

#include <stdlib.h>
#include <math.h>

#include <rsc/misc/langutils.h>

#include <cca/timing/PeriodicBeat.h>
#include <cca/processing/all.h>

#include <rci/dto/JointAngles.h>

#include "cca-oncilla/CCAOncilla.h"

#define TIME_STEP 6 // Webots control step in ms
using namespace std;
using namespace boost;

using namespace rci;
using namespace rci::oncilla;

using namespace cca;
using namespace cca::oncilla;

/**
 * Sine Movement Component
 *
 * The sine movement component generates a sine movement in its processing
 * method that is called by the CCA framework. Generated joint angles for the
 * eight L1 and L2 joint of Oncilla are published in processing step over the
 * eight according output ports of the component
 */
class SimpleSineMovement: public cca::Node {
public:
    SimpleSineMovement(double ampl, double freq) :
            Node("SimpleSineMovement"), OUT_LF_HIP(), OUT_RF_HIP(),
                    OUT_LH_HIP(), OUT_RH_HIP(), OUT_LF_KNEE(), OUT_RF_KNEE(),
                    OUT_LH_KNEE(), OUT_RH_KNEE(),

                    hip_amplitude(ampl), fore_hip_offset(0), hind_hip_offset(0),
                    fore_knee_amplitude(.1), fore_knee_offset(.2),
                    hind_knee_amplitude(.1), hind_knee_offset(.2),
                    fore_hip_knee_phase_lag(-0.2), hind_hip_knee_phase_lag(0),
                    frequency(freq), time_s(0), phase(0), antiphase(0) {

        // Create ports
        OUT_LF_HIP = OutputPort<JointAngles>::create();
        registerPort("out_lf_hip", OUT_LF_HIP);

        OUT_RF_HIP = OutputPort<JointAngles>::create();
        registerPort("out_rf_hip", OUT_RF_HIP);

        OUT_LH_HIP = OutputPort<JointAngles>::create();
        registerPort("out_lh_hip", OUT_LH_HIP);

        OUT_RH_HIP = OutputPort<JointAngles>::create();
        registerPort("out_rh_hip", OUT_RH_HIP);

        OUT_LF_KNEE = OutputPort<JointAngles>::create();
        registerPort("out_lf_knee", OUT_LF_KNEE);

        OUT_RF_KNEE = OutputPort<JointAngles>::create();
        registerPort("out_rf_knee", OUT_RF_KNEE);

        OUT_LH_KNEE = OutputPort<JointAngles>::create();
        registerPort("out_lh_knee", OUT_LH_KNEE);

        OUT_RH_KNEE = OutputPort<JointAngles>::create();
        registerPort("out_rh_knee", OUT_RH_KNEE);

    }
    ~SimpleSineMovement() {
    }
    void onProcess() {

        // The time is our phase - TODO: Use virtual / simulated time
        time_s = rsc::misc::currentTimeMillis() / 1000.0;

        phase = std::fmod(time_s * frequency * 2 * M_PI, 2 * M_PI);
        antiphase = std::fmod(phase + M_PI, 2 * M_PI);

        OUT_LF_HIP->publish(
                JointAngles::fromDeg(
                        hip_position(phase, hip_amplitude, fore_hip_offset)));
        OUT_RF_HIP->publish(
                JointAngles::fromDeg(
                        hip_position(antiphase, hip_amplitude,
                                fore_hip_offset)));
        OUT_LH_HIP->publish(
                JointAngles::fromDeg(
                        hip_position(antiphase, hip_amplitude,
                                hind_hip_offset)));
        OUT_RH_HIP->publish(
                JointAngles::fromDeg(
                        hip_position(phase, hip_amplitude, hind_hip_offset)));

        OUT_LF_KNEE->publish(
                JointAngles::fromRad(
                        knee_position(phase, fore_hip_knee_phase_lag,
                                fore_knee_amplitude, fore_knee_offset)));
        OUT_RF_KNEE->publish(
                JointAngles::fromRad(
                        knee_position(antiphase, fore_hip_knee_phase_lag,
                                fore_knee_amplitude, fore_knee_offset)));
        OUT_LH_KNEE->publish(
                JointAngles::fromRad(
                        knee_position(antiphase, hind_hip_knee_phase_lag,
                                hind_knee_amplitude, hind_knee_offset)));
        OUT_RH_KNEE->publish(
                JointAngles::fromRad(
                        knee_position(phase, hind_hip_knee_phase_lag,
                                hind_knee_amplitude, hind_knee_offset)));
    }

    OutputPort<JointAngles>::Ptr OUT_LF_HIP, OUT_RF_HIP, OUT_LH_HIP, OUT_RH_HIP;
    OutputPort<JointAngles>::Ptr OUT_LF_KNEE, OUT_RF_KNEE, OUT_LH_KNEE,
            OUT_RH_KNEE;

private:
    double hip_position(double phase, double amplitude, double offset) {
        return amplitude * std::cos(phase) + offset;
    }

    double knee_position(double phase, double phase_lag, double amplitude,
            double offset) {
        return amplitude * ((phase + phase_lag) > M_PI ? 1.0 : 0.0) + offset;
    }

    double hip_amplitude, fore_hip_offset, hind_hip_offset, fore_knee_amplitude,
            fore_knee_offset, hind_knee_amplitude, hind_knee_offset,
            fore_hip_knee_phase_lag, hind_hip_knee_phase_lag, frequency;
    double time_s, phase, antiphase;
};

/**
 * The main routine of this example instantiates the Oncilla robot interface
 * and the sine movement component. It connects the component to the interface
 * by configuring the output ports of the sine component to match the input
 * (command) ports of the robot.
 */
int main() {
    // Create a general beat for our system
    BeatPtr heartbeat = PeriodicBeat::create(TIME_STEP); // TIME_STEP ms

    // Instantiate the Oncilla representation in CCA
    CCAOncilla oncilla = CCAOncilla(heartbeat, PortConfiguration::CCALocal);

    // Create the component that send commands
    SimpleSineMovement *s = new SimpleSineMovement(10, 1.6);
    NodePtr sine = NodePtr(s);
    sine->setProcessingStrategy(Timed::samplerate(1));

    // Configure ports of the sine movement component to connect to the robot
    sine->configureOutputPort("out_lf_hip",
            PortConfiguration::LOCAL("/oncilla/cmd/pos/lf/l1"));
    sine->configureOutputPort("out_rf_hip",
            PortConfiguration::LOCAL("/oncilla/cmd/pos/rf/l1"));
    sine->configureOutputPort("out_lh_hip",
            PortConfiguration::LOCAL("/oncilla/cmd/pos/lh/l1"));
    sine->configureOutputPort("out_rh_hip",
            PortConfiguration::LOCAL("/oncilla/cmd/pos/rh/l1"));
    sine->configureOutputPort("out_lf_knee",
            PortConfiguration::LOCAL("/oncilla/cmd/pos/lf/l2"));
    sine->configureOutputPort("out_rf_knee",
            PortConfiguration::LOCAL("/oncilla/cmd/pos/rf/l2"));
    sine->configureOutputPort("out_lh_knee",
            PortConfiguration::LOCAL("/oncilla/cmd/pos/lh/l2"));
    sine->configureOutputPort("out_rh_knee",
            PortConfiguration::LOCAL("/oncilla/cmd/pos/rh/l2"));

    // Register the sine movement component to the beat
    heartbeat->registerReceiver(sine);

    printf("*** Note:\n");
    printf("*** This is a dummy walking example to show the basic usage of the\n");
    printf("*** simulator with API Level 1! Adapt the example to your own needs.\n");

    // Run the beat
    heartbeat->run();

    return EXIT_SUCCESS;
}