Homec4science
Diffusion liboncilla (issue521) Browse

liboncilla/examples/SimpleSineMovement.cpp
0d8781e6b7bfissue521

Last Change
Raw File

SimpleSineMovement.cpp
View Options

#include <iostream>
#include <fstream>
#include <stdlib.h>
#include <math.h>
#include <rsc/misc/langutils.h>
#include <rci/dto/JointAngles.h>
#include "liboncilla/Oncilla.h"
#include "liboncilla/OncillaL1L2.h"
#include "liboncilla/OncillaSynchronizer.h"
using namespace std;
using namespace boost;
using namespace rci;
using namespace rci::oncilla;
int main() {
double amplitude_rad = 1.0;
double frequency_Hz = 1.0;
// 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(Oncilla::LEFT_FORE);
OncillaL2Ptr left_fore_knee = oncilla.getL2(Oncilla::LEFT_FORE);
OncillaL1Ptr right_fore_hip = oncilla.getL1(Oncilla::RIGHT_FORE);
OncillaL2Ptr right_fore_knee = oncilla.getL2(Oncilla::RIGHT_FORE);
OncillaL1Ptr left_hind_hip = oncilla.getL1(Oncilla::LEFT_HIND);
OncillaL2Ptr left_hind_knee = oncilla.getL2(Oncilla::LEFT_HIND);
OncillaL1Ptr right_hind_hip = oncilla.getL1(Oncilla::RIGHT_HIND);
OncillaL2Ptr right_hind_knee = oncilla.getL2(Oncilla::RIGHT_HIND);
// Now move the joints
double time_s, phase1, phase2;
JointAnglesPtr hip1, hip2, knee1, knee2;
while (true) {
// The time is our phase - TODO: Use virtual / simulated time
time_s = rsc::misc::currentTimeMicros() / 1000;
phase1 = time_s * frequency_Hz;
phase2 = (time_s + 0.5) * frequency_Hz;
hip1 = JointAngles::fromRad(amplitude_rad * sin(phase1));
left_fore_hip->setJointPosition(hip1);
right_hind_hip->setJointPosition(hip1);
hip2 = JointAngles::fromRad(amplitude_rad * sin(phase2));
left_hind_hip->setJointPosition(hip2);
right_fore_hip->setJointPosition(hip2);
knee1 = JointAngles::fromRad(amplitude_rad * sin(phase1));
left_fore_knee->setJointPosition(knee1);
right_hind_knee->setJointPosition(knee1);
knee2 = JointAngles::fromRad(amplitude_rad * sin(phase2));
left_hind_knee->setJointPosition(knee2);
right_fore_knee->setJointPosition(knee2);
// Now that we set the new angles, we do one processing step
synchronizer->process();
}
return EXIT_SUCCESS;
}
c4science · Help