Page Menu
Home
c4science
Search
Configure Global Search
Log In
Files
F87592042
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
Sun, Oct 13, 14:21
Size
2 KB
Mime Type
text/x-c
Expires
Tue, Oct 15, 14:21 (1 d, 23 h)
Engine
blob
Format
Raw Data
Handle
21621282
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 "Oncilla.h"
#include "OncillaL1L2.h"
#include "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;
}
Event Timeline
Log In to Comment