Page Menu
Home
c4science
Search
Configure Global Search
Log In
Files
F88005793
WebotsSynchronizer.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, Oct 16, 06:48
Size
5 KB
Mime Type
text/x-c
Expires
Fri, Oct 18, 06:48 (2 d)
Engine
blob
Format
Raw Data
Handle
21699212
Attached To
R6622 liboncilla
WebotsSynchronizer.cpp
View Options
/**
* \file WebotsSynchronizer.cpp
*
* \date Aug 22, 2010
* \author tuleu
* \author anordman
*/
//#define TEST_ACTUATOR_IH_PRESENCE
#include <libamarsi-quad/utils/PreferenceReader.h>
#include "WebotsSynchronizer.h"
#include <cmath>
#include <iostream>
#include <boost/format.hpp>
#define PI 3.1415265
using
namespace
std
;
using
namespace
boost
;
using
namespace
amarsi
;
namespace
cca
{
namespace
rci
{
namespace
driver
{
WebotsSynchronizer
::
WebotsSynchronizer
(
int
&
argc
,
char
**
argv
)
:
Synchronizer
(
"Webots Synchronizer"
),
Controller
(
argc
,
argv
),
_running
(
true
),
d_amplitudeHip
(
0.3
),
d_offsetHip
(
0
),
d_amplitudeKnee
(
0.4
),
d_offsetKnee
(
0.6
),
d_frequency
(
0.5
),
_actuatorNames
(
amarsi
::
NUMBER_OF_ACTUATORS
),
_servoNames
(
amarsi
::
NUMBER_OF_ACTUATORS
)
{
// FIXME: 1000* is nonsense
this
->
_nodes
.
reserve
(
1000
*
(
unsigned
int
)
amarsi
::
NUMBER_OF_ACTUATORS
);
// Number of actuators is number of nodes in general
this
->
prepareNodeWebotsMapping
();
_primpleAmarsi
=
static_cast
<
ControllerPrimpleAmarsi
*>
(
Controller
::
pimpController_
);
// Initialize simulation
Supervisor
::
instance
().
initSimulation
();
cout
<<
"[WebotsSynchronizer] Webots simulator initialized."
<<
endl
;
}
WebotsSynchronizer
::~
WebotsSynchronizer
()
{
Supervisor
::
instance
().
simulationQuit
();
cout
<<
"[WebotsSynchronizer] Ended simulation"
<<
endl
;
}
void
WebotsSynchronizer
::
userInitSimulation
()
{
//add some motion restriction
Supervisor
::
instance
().
configureMotionRestriction
(
"0.0:SAGITAL_FIXED_PITCH|2.0:SAGITAL_FREE_PITCH|4.0:FREE"
);
Supervisor
::
instance
().
setTrackingOn
();
Supervisor
::
instance
().
setDisplayMode
(
amarsi
::
SHOW_GROUND_REACTION_FORCES
);
}
void
WebotsSynchronizer
::
userInitEnvironment
()
{
//put the robot on a slope of angle d_angle in the front direction.
Supervisor
::
instance
().
setSlope
(
true
,
d_angle
);
}
int
WebotsSynchronizer
::
run
()
{
while
(
true
)
{
step
();
}
return
0
;
}
/* CCA-specific */
CCANodePtr
WebotsSynchronizer
::
create
(
int
&
argc
,
char
**
argv
)
{
return
CCANodePtr
(
new
WebotsSynchronizer
(
argc
,
argv
));
}
std
::
string
WebotsSynchronizer
::
print
()
const
{
format
fmter
(
"<Synchronizer
\"
%1%
\"
>"
);
fmter
%
this
->
getName
();
return
fmter
.
str
();
}
void
WebotsSynchronizer
::
send
()
{
double
value
;
if
(
this
->
_running
)
{
// Sending commands to the hip servos
for
(
unsigned
int
index
=
(
unsigned
int
)
amarsi
::
LEFT_FORE_HIP
;
index
<=
(
unsigned
int
)
amarsi
::
RIGHT_HIND_HIP
;
++
index
)
{
value
=
static_pointer_cast
<
OncillaHip
>
(
this
->
_nodes
[(
unsigned
int
)
index
])
->
getLastPositionCommand
()
->
asDouble
(
0
);
actuator
(
static_cast
<
amarsi
::
ActuatorID
>
(
index
)).
setCommand
(
value
);
}
// Sending commands to the knee servos
for
(
unsigned
int
index
=
(
unsigned
int
)
amarsi
::
LEFT_FORE_KNEE
;
index
<=
(
unsigned
int
)
amarsi
::
RIGHT_HIND_KNEE
;
++
index
)
{
value
=
static_pointer_cast
<
OncillaKnee
>
(
this
->
_nodes
[(
unsigned
int
)
index
])
->
getLastPositionCommand
()
->
asDouble
(
0
);
actuator
(
static_cast
<
amarsi
::
ActuatorID
>
(
index
)).
setCommand
(
value
);
}
}
}
void
WebotsSynchronizer
::
receive
()
{
double
value
;
if
(
this
->
_running
)
{
// Reading encoder values from the hips
for
(
unsigned
int
index
=
(
unsigned
int
)
amarsi
::
LEFT_FORE_HIP
;
index
<=
(
unsigned
int
)
amarsi
::
RIGHT_HIND_HIP
;
++
index
)
{
value
=
this
->
_primpleAmarsi
->
getWebotsServo
(
this
->
_servoNames
[(
unsigned
int
)
index
])
->
getPosition
();
static_pointer_cast
<
OncillaHip
>
(
this
->
_nodes
[(
unsigned
int
)
index
])
->
updateJointPosition
(
JointAngles
::
fromValue
(
value
));
}
// Reading encoder values from the knees
for
(
unsigned
int
index
=
(
unsigned
int
)
amarsi
::
LEFT_FORE_KNEE
;
index
<=
(
unsigned
int
)
amarsi
::
RIGHT_HIND_KNEE
;
++
index
)
{
value
=
this
->
_primpleAmarsi
->
getWebotsServo
(
this
->
_servoNames
[(
unsigned
int
)
index
])
->
getPosition
();
static_pointer_cast
<
OncillaKnee
>
(
this
->
_nodes
[(
unsigned
int
)
index
])
->
updateJointPosition
(
JointAngles
::
fromValue
(
value
));
}
}
}
void
WebotsSynchronizer
::
registerQuadNode
(
CCANodePtr
node
,
ActuatorID
index
)
{
// Try to register node
ResourceNodePtr
rn
=
static_pointer_cast
<
ResourceNode
>
(
node
);
std
::
cout
<<
"[WebotsSynchronizer] Register node "
<<
node
->
getName
()
<<
" at position #"
<<
(
unsigned
int
)
index
<<
std
::
endl
;
this
->
registerNode
(
rn
,
(
unsigned
int
)
index
);
}
void
WebotsSynchronizer
::
prepareNodeWebotsMapping
()
{
this
->
_actuatorNames
.
reserve
((
unsigned
int
)
amarsi
::
NUMBER_OF_ACTUATORS
);
this
->
_servoNames
.
reserve
((
unsigned
int
)
amarsi
::
NUMBER_OF_ACTUATORS
);
// Hip Servos
this
->
_servoNames
[(
unsigned
int
)
amarsi
::
LEFT_FORE_HIP
]
=
"LEFT_FORE_hip_servo"
;
this
->
_servoNames
[(
unsigned
int
)
amarsi
::
RIGHT_FORE_HIP
]
=
"RIGHT_FORE_hip_servo"
;
this
->
_servoNames
[(
unsigned
int
)
amarsi
::
LEFT_HIND_HIP
]
=
"LEFT_HIND_hip_servo"
;
this
->
_servoNames
[(
unsigned
int
)
amarsi
::
RIGHT_HIND_HIP
]
=
"RIGHT_HIND_hip_servo"
;
// Knee Servos
this
->
_servoNames
[(
unsigned
int
)
amarsi
::
LEFT_FORE_KNEE
]
=
"LEFT_FORE_FRONT_KNEE_SERVO"
;
this
->
_servoNames
[(
unsigned
int
)
amarsi
::
RIGHT_FORE_KNEE
]
=
"RIGHT_FORE_FRONT_KNEE_SERVO"
;
this
->
_servoNames
[(
unsigned
int
)
amarsi
::
LEFT_HIND_KNEE
]
=
"LEFT_HIND_FRONT_KNEE_SERVO"
;
this
->
_servoNames
[(
unsigned
int
)
amarsi
::
RIGHT_HIND_KNEE
]
=
"RIGHT_HIND_FRONT_KNEE_SERVO"
;
}
}
}
}
Event Timeline
Log In to Comment