24 #include <interfaces/HumanoidMotionInterface.h>
26 #include <core/exceptions/software.h>
46 HumanoidMotionInterface::HumanoidMotionInterface() : Interface()
48 data_size =
sizeof(HumanoidMotionInterface_data_t);
50 data = (HumanoidMotionInterface_data_t *)
data_ptr;
53 enum_map_LegEnum[(int)LEG_LEFT] =
"LEG_LEFT";
54 enum_map_LegEnum[(int)LEG_RIGHT] =
"LEG_RIGHT";
55 enum_map_StandupEnum[(int)STANDUP_DETECT] =
"STANDUP_DETECT";
56 enum_map_StandupEnum[(int)STANDUP_BACK] =
"STANDUP_BACK";
57 enum_map_StandupEnum[(int)STANDUP_FRONT] =
"STANDUP_FRONT";
72 unsigned char tmp_hash[] = {0x58, 0x4e, 0xd5, 0x1c, 0xbb, 0xf7, 0x6d, 0x85, 0x15, 0x52, 0x3b, 0x5a, 0x2b, 0x99, 0x5d, 0xc6};
77 HumanoidMotionInterface::~HumanoidMotionInterface()
89 case LEG_LEFT:
return "LEG_LEFT";
90 case LEG_RIGHT:
return "LEG_RIGHT";
91 default:
return "UNKNOWN";
102 case STANDUP_DETECT:
return "STANDUP_DETECT";
103 case STANDUP_BACK:
return "STANDUP_BACK";
104 case STANDUP_FRONT:
return "STANDUP_FRONT";
105 default:
return "UNKNOWN";
148 return data->arms_enabled;
213 if ( strncmp(
"StopMessage",
type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
215 }
else if ( strncmp(
"WalkStraightMessage",
type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
217 }
else if ( strncmp(
"WalkSidewaysMessage",
type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
219 }
else if ( strncmp(
"WalkArcMessage",
type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
221 }
else if ( strncmp(
"WalkVelocityMessage",
type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
223 }
else if ( strncmp(
"TurnMessage",
type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
225 }
else if ( strncmp(
"KickMessage",
type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
227 }
else if ( strncmp(
"ParkMessage",
type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
229 }
else if ( strncmp(
"GetUpMessage",
type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
231 }
else if ( strncmp(
"StandupMessage",
type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
233 }
else if ( strncmp(
"MoveHeadMessage",
type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
237 "message type for this interface type.",
type);
253 memcpy(data, oi->data,
sizeof(HumanoidMotionInterface_data_t));
259 if (strcmp(enumtype,
"LegEnum") == 0) {
260 return tostring_LegEnum((
LegEnum)val);
262 if (strcmp(enumtype,
"StandupEnum") == 0) {
282 data = (StopMessage_data_t *)
data_ptr;
284 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
285 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
287 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
305 data = (StopMessage_data_t *)
data_ptr;
332 data_size =
sizeof(WalkStraightMessage_data_t);
335 data = (WalkStraightMessage_data_t *)
data_ptr;
337 data->distance = ini_distance;
338 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
339 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
341 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
348 data_size =
sizeof(WalkStraightMessage_data_t);
351 data = (WalkStraightMessage_data_t *)
data_ptr;
353 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
354 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
356 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
375 data = (WalkStraightMessage_data_t *)
data_ptr;
387 return data->distance;
432 data_size =
sizeof(WalkSidewaysMessage_data_t);
435 data = (WalkSidewaysMessage_data_t *)
data_ptr;
437 data->distance = ini_distance;
438 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
439 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
441 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
448 data_size =
sizeof(WalkSidewaysMessage_data_t);
451 data = (WalkSidewaysMessage_data_t *)
data_ptr;
453 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
454 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
456 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
475 data = (WalkSidewaysMessage_data_t *)
data_ptr;
487 return data->distance;
533 data_size =
sizeof(WalkArcMessage_data_t);
536 data = (WalkArcMessage_data_t *)
data_ptr;
538 data->angle = ini_angle;
539 data->radius = ini_radius;
540 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
541 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
543 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
551 data_size =
sizeof(WalkArcMessage_data_t);
554 data = (WalkArcMessage_data_t *)
data_ptr;
556 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
557 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
559 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
579 data = (WalkArcMessage_data_t *)
data_ptr;
669 data_size =
sizeof(WalkVelocityMessage_data_t);
672 data = (WalkVelocityMessage_data_t *)
data_ptr;
676 data->theta = ini_theta;
677 data->speed = ini_speed;
678 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
679 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
681 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
691 data_size =
sizeof(WalkVelocityMessage_data_t);
694 data = (WalkVelocityMessage_data_t *)
data_ptr;
696 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
697 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
699 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
721 data = (WalkVelocityMessage_data_t *)
data_ptr;
887 data = (TurnMessage_data_t *)
data_ptr;
889 data->angle = ini_angle;
890 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
891 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
893 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
903 data = (TurnMessage_data_t *)
data_ptr;
905 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
906 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
908 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
927 data = (TurnMessage_data_t *)
data_ptr;
988 data = (KickMessage_data_t *)
data_ptr;
991 data->strength = ini_strength;
992 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
993 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
995 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
1006 data = (KickMessage_data_t *)
data_ptr;
1008 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
1009 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
1011 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
1031 data = (KickMessage_data_t *)
data_ptr;
1073 return data->strength;
1119 data = (ParkMessage_data_t *)
data_ptr;
1121 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
1122 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
1124 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
1142 data = (ParkMessage_data_t *)
data_ptr;
1167 data_size =
sizeof(GetUpMessage_data_t);
1170 data = (GetUpMessage_data_t *)
data_ptr;
1172 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
1173 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
1175 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
1193 data = (GetUpMessage_data_t *)
data_ptr;
1220 data_size =
sizeof(StandupMessage_data_t);
1223 data = (StandupMessage_data_t *)
data_ptr;
1225 data->from_pos = ini_from_pos;
1226 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
1227 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
1229 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
1236 data_size =
sizeof(StandupMessage_data_t);
1239 data = (StandupMessage_data_t *)
data_ptr;
1241 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
1242 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
1244 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
1263 data = (StandupMessage_data_t *)
data_ptr;
1322 data_size =
sizeof(MoveHeadMessage_data_t);
1325 data = (MoveHeadMessage_data_t *)
data_ptr;
1327 data->yaw = ini_yaw;
1328 data->pitch = ini_pitch;
1329 data->speed = ini_speed;
1330 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
1331 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
1333 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
1342 data_size =
sizeof(MoveHeadMessage_data_t);
1345 data = (MoveHeadMessage_data_t *)
data_ptr;
1347 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
1348 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
1350 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
1371 data = (MoveHeadMessage_data_t *)
data_ptr;
1524 if ( m10 != NULL ) {