Page Menu
Home
c4science
Search
Configure Global Search
Log In
Files
F88634084
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
Sat, Oct 19, 21:21
Size
2 KB
Mime Type
text/x-c
Expires
Mon, Oct 21, 21:21 (1 d, 23 h)
Engine
blob
Format
Raw Data
Handle
21801484
Attached To
R6622 liboncilla
SimpleSineMovement.cpp
View Options
#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
(
10
),
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
(
0.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
->
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_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