Navigation

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)
  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
#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 2 // 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::CCANode {
public:
	SimpleSineMovement(double ampl, double freq) :
			CCANode("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(0),
            hind_knee_amplitude(1),
            hind_knee_offset(0),
            fore_hip_knee_phase_lag(0),
            hind_hip_knee_phase_lag(0),
            frequency(freq), time_s(0), phase(0), antiphase(0) {

		// Create ports
		OUT_LF_HIP = createOutputPort<JointAngles>();
		OUT_RF_HIP = createOutputPort<JointAngles>();
		OUT_LH_HIP = createOutputPort<JointAngles>();
		OUT_RH_HIP = createOutputPort<JointAngles>();
		OUT_LF_KNEE = createOutputPort<JointAngles>();
		OUT_RF_KNEE = createOutputPort<JointAngles>();
		OUT_LH_KNEE = createOutputPort<JointAngles>();
		OUT_RH_KNEE = createOutputPort<JointAngles>();
	}
	~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);

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

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

	unsigned int OUT_LF_HIP, OUT_RF_HIP, OUT_LH_HIP, OUT_RH_HIP;
	unsigned int 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(30, 3);
	CCANodePtr sine = CCANodePtr(s);
	sine->setProcessingStrategy(TimedProcessing::create(1));

	// Configure ports of the sine movement component to connect to the robot
	sine->configureOutputPort(s->OUT_LF_HIP,
			PortConfiguration::LOCAL("/oncilla/cmd/pos/lf/l1"));
	sine->configureOutputPort(s->OUT_RF_HIP,
			PortConfiguration::LOCAL("/oncilla/cmd/pos/rf/l1"));
	sine->configureOutputPort(s->OUT_LH_HIP,
			PortConfiguration::LOCAL("/oncilla/cmd/pos/lh/l1"));
	sine->configureOutputPort(s->OUT_RH_HIP,
			PortConfiguration::LOCAL("/oncilla/cmd/pos/rh/l1"));
	sine->configureOutputPort(s->OUT_LF_KNEE,
			PortConfiguration::LOCAL("/oncilla/cmd/pos/lf/l2"));
	sine->configureOutputPort(s->OUT_RF_KNEE,
			PortConfiguration::LOCAL("/oncilla/cmd/pos/rf/l2"));
	sine->configureOutputPort(s->OUT_LH_KNEE,
			PortConfiguration::LOCAL("/oncilla/cmd/pos/lh/l2"));
	sine->configureOutputPort(s->OUT_RH_KNEE,
			PortConfiguration::LOCAL("/oncilla/cmd/pos/rh/l2"));

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

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

	return EXIT_SUCCESS;
}

This Page

Navigation

© Copyright 2012, Arne Nordmann, Alexandre Tuleu. Created using Sphinx 1.1.3.