Page MenuHomec4science
Files F67798848

SimpleSineMovement.cpp
No OneTemporary
Actions

File Metadata

Created
Mon, Jun 24, 10:02

SimpleSineMovement.cpp
View Options

#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
::rci::oncilla::Synchronizer::Ptr synchronizer = oncilla.getSynchronizer();
// Get all the joints we are interested in - L1 and L2
L1::Ptr left_fore_hip = oncilla.getL1(LEFT_FORE);
L2::Ptr left_fore_knee = oncilla.getL2(LEFT_FORE);
L1::Ptr right_fore_hip = oncilla.getL1(RIGHT_FORE);
L2::Ptr right_fore_knee = oncilla.getL2(RIGHT_FORE);
L1::Ptr left_hind_hip = oncilla.getL1(LEFT_HIND);
L2::Ptr left_hind_knee = oncilla.getL2(LEFT_HIND);
L1::Ptr right_hind_hip = oncilla.getL1(RIGHT_HIND);
L2::Ptr right_hind_knee = oncilla.getL2(RIGHT_HIND);
//sets some parameter for the sinewave
double hip_amplitude(30),
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(1);
double time_s, phase, antiphase;
time_s = 0;
while (true) {
// The simulated time is our phase
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_hind_hip->setJointPosition(JointAngles::fromDeg(hip_position(antiphase, hip_amplitude,hind_hip_offset)));
left_hind_knee->setJointPosition(JointAngles::fromRad(knee_position(antiphase, hind_hip_knee_phase_lag,hind_knee_amplitude, hind_knee_offset)));
right_hind_hip->setJointPosition(JointAngles::fromDeg(hip_position(phase, hip_amplitude, hind_hip_offset)));
right_hind_knee->setJointPosition(JointAngles::fromRad(knee_position(phase, hind_hip_knee_phase_lag,hind_knee_amplitude, hind_knee_offset)));
// Perform a (simulation) processing step
synchronizer->process();
}
return EXIT_SUCCESS;
}

Event Timeline

c4science · Help