Page Menu
Home
c4science
Search
Configure Global Search
Log In
Files
F62666151
SimpleSineMovement.cpp
No One
Temporary
Actions
Download File
Edit File
Delete File
View Transforms
Subscribe
Mute Notifications
Award Token
Subscribers
None
File Metadata
Details
File Info
Storage
Attached
Created
Tue, May 14, 17:44
Size
3 KB
Mime Type
text/x-c
Expires
Thu, May 16, 17:44 (2 d)
Engine
blob
Format
Raw Data
Handle
17675042
Attached To
R6622 liboncilla
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"
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(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;
}
Event Timeline
Log In to Comment