This examples builds on the Oncilla API Level 0, which is the lowest Oncilla interface level. The examples was written for the Oncilla Simulation, but should work exactly the same later on the real hardware platform.
To show basic movement using the Oncilla interface, we provide a simple example performing a sine-movement on the Oncilla joint in position control, to generate a simple walking movement.
The source code below shows the example implementation. To perform the simple movement the following basic steps are done:
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 | #include <rsc/misc/langutils.h>
#include <rci/dto/JointAngles.h>
#include "liboncilla/Oncilla.h"
using namespace std;
using namespace boost;
using namespace rci;
using namespace rci::oncilla;
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;
}
int main() {
// Instantiate the actual robot object
Oncilla oncilla;
// Get the synchronizer, so that we can control the actual simulation
// stepping from within our code
OncillaSynchronizerPtr synchronizer = oncilla.getSynchronizer();
// Get all the joints we are interested in - L1 and L2
OncillaL1Ptr left_fore_hip = oncilla.getL1(LEFT_FORE);
OncillaL2Ptr left_fore_knee = oncilla.getL2(LEFT_FORE);
OncillaL1Ptr right_fore_hip = oncilla.getL1(RIGHT_FORE);
OncillaL2Ptr right_fore_knee = oncilla.getL2(RIGHT_FORE);
OncillaL1Ptr left_hind_hip = oncilla.getL1(LEFT_HIND);
OncillaL2Ptr left_hind_knee = oncilla.getL2(LEFT_HIND);
OncillaL1Ptr right_hind_hip = oncilla.getL1(RIGHT_HIND);
OncillaL2Ptr right_hind_knee = oncilla.getL2(RIGHT_HIND);
//sets some parameter for the sinewave
double hip_amplitude(10),
fore_hip_offset(0),
hind_hip_offset(0),
fore_knee_amplitude(0.5),
fore_knee_offset(0),
hind_knee_amplitude(0),
hind_knee_offset(0),
fore_hip_knee_phase_lag(0),
hind_hip_knee_phase_lag(0),
frequency(0.5);
// Now move the joints
double time_s, phase, antiphase;
time_s = 0;
while (true) {
// The time is our phase - TODO: Use virtual / simulated time
time_s += synchronizer->lastProcessTimeStep();
phase = std::fmod(time_s * frequency * 2 * M_PI, 2 * M_PI);
antiphase = std::fmod(phase + M_PI , 2 * M_PI);
left_fore_hip->setJointPosition(JointAngles::fromDeg(hip_position(phase,hip_amplitude,fore_hip_offset)));
left_fore_knee->setJointPosition(JointAngles::fromRad(knee_position(phase,fore_hip_knee_phase_lag,fore_knee_amplitude,fore_knee_offset)));
right_fore_hip->setJointPosition(JointAngles::fromDeg(hip_position(antiphase,hip_amplitude,fore_hip_offset)));
right_fore_knee->setJointPosition(JointAngles::fromRad(knee_position(antiphase,fore_hip_knee_phase_lag,fore_knee_amplitude,fore_knee_offset)));
left_fore_hip->setJointPosition(JointAngles::fromDeg(hip_position(antiphase,hip_amplitude,hind_hip_offset)));
left_fore_knee->setJointPosition(JointAngles::fromRad(knee_position(antiphase,hind_hip_knee_phase_lag,hind_knee_amplitude,hind_knee_offset)));
right_fore_hip->setJointPosition(JointAngles::fromDeg(hip_position(phase,hip_amplitude,hind_hip_offset)));
right_fore_knee->setJointPosition(JointAngles::fromRad(knee_position(phase,hind_hip_knee_phase_lag,hind_knee_amplitude,hind_knee_offset)));
// Now that we set the new angles, we do one processing step
synchronizer->process();
}
return EXIT_SUCCESS;
}
|
...