24 #include <interfaces/NaoJointStiffnessInterface.h>
26 #include <core/exceptions/software.h>
46 NaoJointStiffnessInterface::NaoJointStiffnessInterface() : Interface()
48 data_size =
sizeof(NaoJointStiffnessInterface_data_t);
50 data = (NaoJointStiffnessInterface_data_t *)
data_ptr;
83 unsigned char tmp_hash[] = {0x29, 0x35, 0x74, 0x2f, 0x4e, 0x93, 0x53, 0xc4, 0x28, 0x56, 0xc8, 0x4a, 0x66, 0x81, 0xd6, 0x6d};
88 NaoJointStiffnessInterface::~NaoJointStiffnessInterface()
100 return data->head_yaw;
130 return data->head_pitch;
160 return data->l_shoulder_pitch;
190 return data->l_shoulder_roll;
220 return data->l_elbow_yaw;
250 return data->l_elbow_roll;
280 return data->l_wrist_yaw;
340 return data->l_hip_yaw_pitch;
370 return data->l_hip_roll;
400 return data->l_hip_pitch;
430 return data->l_knee_pitch;
460 return data->l_ankle_pitch;
490 return data->l_ankle_roll;
520 return data->r_shoulder_pitch;
550 return data->r_shoulder_roll;
580 return data->r_elbow_yaw;
610 return data->r_elbow_roll;
640 return data->r_wrist_yaw;
700 return data->r_hip_yaw_pitch;
730 return data->r_hip_roll;
760 return data->r_hip_pitch;
790 return data->r_knee_pitch;
820 return data->r_ankle_pitch;
850 return data->r_ankle_roll;
883 return data->minimum;
913 if ( strncmp(
"SetStiffnessMessage",
type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
915 }
else if ( strncmp(
"SetBodyStiffnessMessage",
type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
917 }
else if ( strncmp(
"SetStiffnessesMessage",
type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
921 "message type for this interface type.",
type);
937 memcpy(data, oi->data,
sizeof(NaoJointStiffnessInterface_data_t));
961 data_size =
sizeof(SetStiffnessMessage_data_t);
964 data = (SetStiffnessMessage_data_t *)
data_ptr;
966 data->servo = ini_servo;
967 data->value = ini_value;
968 data->time_sec = ini_time_sec;
976 data_size =
sizeof(SetStiffnessMessage_data_t);
979 data = (SetStiffnessMessage_data_t *)
data_ptr;
1000 data = (SetStiffnessMessage_data_t *)
data_ptr;
1082 return data->time_sec;
1128 data_size =
sizeof(SetBodyStiffnessMessage_data_t);
1131 data = (SetBodyStiffnessMessage_data_t *)
data_ptr;
1133 data->value = ini_value;
1134 data->time_sec = ini_time_sec;
1141 data_size =
sizeof(SetBodyStiffnessMessage_data_t);
1144 data = (SetBodyStiffnessMessage_data_t *)
data_ptr;
1164 data = (SetBodyStiffnessMessage_data_t *)
data_ptr;
1206 return data->time_sec;
1275 NaoJointStiffnessInterface::SetStiffnessesMessage::SetStiffnessesMessage(
const float ini_time_sec,
const float ini_head_yaw,
const float ini_head_pitch,
const float ini_l_shoulder_pitch,
const float ini_l_shoulder_roll,
const float ini_l_elbow_yaw,
const float ini_l_elbow_roll,
const float ini_l_wrist_yaw,
const float ini_l_hand,
const float ini_l_hip_yaw_pitch,
const float ini_l_hip_roll,
const float ini_l_hip_pitch,
const float ini_l_knee_pitch,
const float ini_l_ankle_pitch,
const float ini_l_ankle_roll,
const float ini_r_shoulder_pitch,
const float ini_r_shoulder_roll,
const float ini_r_elbow_yaw,
const float ini_r_wrist_yaw,
const float ini_r_hand,
const float ini_r_hip_yaw_pitch,
const float ini_r_hip_roll,
const float ini_r_hip_pitch,
const float ini_r_knee_pitch,
const float ini_r_ankle_pitch,
const float ini_r_ankle_roll,
const float ini_r_elbow_roll) :
Message(
"SetStiffnessesMessage")
1277 data_size =
sizeof(SetStiffnessesMessage_data_t);
1280 data = (SetStiffnessesMessage_data_t *)
data_ptr;
1282 data->time_sec = ini_time_sec;
1283 data->head_yaw = ini_head_yaw;
1284 data->head_pitch = ini_head_pitch;
1285 data->l_shoulder_pitch = ini_l_shoulder_pitch;
1286 data->l_shoulder_roll = ini_l_shoulder_roll;
1287 data->l_elbow_yaw = ini_l_elbow_yaw;
1288 data->l_elbow_roll = ini_l_elbow_roll;
1289 data->l_wrist_yaw = ini_l_wrist_yaw;
1290 data->l_hand = ini_l_hand;
1291 data->l_hip_yaw_pitch = ini_l_hip_yaw_pitch;
1292 data->l_hip_roll = ini_l_hip_roll;
1293 data->l_hip_pitch = ini_l_hip_pitch;
1294 data->l_knee_pitch = ini_l_knee_pitch;
1295 data->l_ankle_pitch = ini_l_ankle_pitch;
1296 data->l_ankle_roll = ini_l_ankle_roll;
1297 data->r_shoulder_pitch = ini_r_shoulder_pitch;
1298 data->r_shoulder_roll = ini_r_shoulder_roll;
1299 data->r_elbow_yaw = ini_r_elbow_yaw;
1300 data->r_wrist_yaw = ini_r_wrist_yaw;
1301 data->r_hand = ini_r_hand;
1302 data->r_hip_yaw_pitch = ini_r_hip_yaw_pitch;
1303 data->r_hip_roll = ini_r_hip_roll;
1304 data->r_hip_pitch = ini_r_hip_pitch;
1305 data->r_knee_pitch = ini_r_knee_pitch;
1306 data->r_ankle_pitch = ini_r_ankle_pitch;
1307 data->r_ankle_roll = ini_r_ankle_roll;
1308 data->r_elbow_roll = ini_r_elbow_roll;
1340 data_size =
sizeof(SetStiffnessesMessage_data_t);
1343 data = (SetStiffnessesMessage_data_t *)
data_ptr;
1388 data = (SetStiffnessesMessage_data_t *)
data_ptr;
1400 return data->time_sec;
1430 return data->head_yaw;
1460 return data->head_pitch;
1490 return data->l_shoulder_pitch;
1510 change_field(data->l_shoulder_pitch, new_l_shoulder_pitch);
1520 return data->l_shoulder_roll;
1540 change_field(data->l_shoulder_roll, new_l_shoulder_roll);
1550 return data->l_elbow_yaw;
1580 return data->l_elbow_roll;
1610 return data->l_wrist_yaw;
1640 return data->l_hand;
1670 return data->l_hip_yaw_pitch;
1690 change_field(data->l_hip_yaw_pitch, new_l_hip_yaw_pitch);
1700 return data->l_hip_roll;
1730 return data->l_hip_pitch;
1760 return data->l_knee_pitch;
1790 return data->l_ankle_pitch;
1820 return data->l_ankle_roll;
1850 return data->r_shoulder_pitch;
1870 change_field(data->r_shoulder_pitch, new_r_shoulder_pitch);
1880 return data->r_shoulder_roll;
1900 change_field(data->r_shoulder_roll, new_r_shoulder_roll);
1910 return data->r_elbow_yaw;
1940 return data->r_wrist_yaw;
1970 return data->r_hand;
2000 return data->r_hip_yaw_pitch;
2020 change_field(data->r_hip_yaw_pitch, new_r_hip_yaw_pitch);
2030 return data->r_hip_roll;
2060 return data->r_hip_pitch;
2090 return data->r_knee_pitch;
2120 return data->r_ankle_pitch;
2150 return data->r_ankle_roll;
2180 return data->r_elbow_roll;