diff --git a/examples/SimpleSineMovement.cpp b/examples/SimpleSineMovement.cpp index 3b00e4d..013eadd 100644 --- a/examples/SimpleSineMovement.cpp +++ b/examples/SimpleSineMovement.cpp @@ -1,80 +1,80 @@ #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), + double hip_amplitude(10), 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), + fore_knee_amplitude(0.1), + fore_knee_offset(0.2), + hind_knee_amplitude(0.1), + hind_knee_offset(0.2), + fore_hip_knee_phase_lag(-0.2), hind_hip_knee_phase_lag(0), - frequency(1); + frequency(1.6); 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); + 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; }