diff --git a/examples/SimpleSineMovement.cpp b/examples/SimpleSineMovement.cpp
index 3b00e4d..04a46d5 100644
--- a/examples/SimpleSineMovement.cpp
+++ b/examples/SimpleSineMovement.cpp
@@ -1,80 +1,148 @@
#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 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;
+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() {
+int main(){
- // Instantiate the actual robot object
- Oncilla oncilla;
+ // 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;
+ //sets some parameter for the sinewave
+ double hip_amplitude(10) , fore_hip_offset(0) , hind_hip_offset(0) ,
+ fore_knee_amplitude(.1) , fore_knee_offset(.2) ,
+ hind_knee_amplitude(.1) , hind_knee_offset(.2) ,
+ fore_hip_knee_phase_lag(0) , hind_hip_knee_phase_lag(0) , frequency(
+ 1.6);
+ double time_s , phase , antiphase;
+
+ printf("*** Note:\n");
+ printf("*** This is a dummy walking example to show the basic usage of the\n");
+ printf("*** simulator with API Level 0! Adapt the example to your own needs.\n");
+
+ time_s = 0;
+ while (time_s < 10) {
+ // 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();
+ }
+
+ printf("*** RESET!\n");
+ oncilla.getSupervisor()->getWorld()->reset();
+
+ 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;
}