Homec4science
Diffusion liboncilla (issue521) Browse

liboncilla/examples/SimpleSineMovement.cpp
1383776216c9issue521

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;
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(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);
//sets some parameter for the sinewave
double hip_amplitude(30),
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(1.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->latestProcessLoopDuration();
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;
}
c4science · Help