Page Menu
Home
c4science
Search
Configure Global Search
Log In
Files
F82420901
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
Wed, Sep 11, 07:52
Size
2 KB
Mime Type
text/x-c
Expires
Fri, Sep 13, 07:52 (1 d, 23 h)
Engine
blob
Format
Raw Data
Handle
20696551
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