88 printf(
"Starting new connection to the Motion Server\r\n");
90 ATTEMPT_MOTION_CONNECTION:
101 if (connectionIndex == MAX_MOTION_CONNECTIONS)
105 puts(
"Motion server already connected... not accepting last attempt.");
111 puts(
"Motion server already connected... closing old connection.");
113 goto ATTEMPT_MOTION_CONNECTION;
119 mpSetsockopt(sd, SOL_SOCKET, SO_KEEPALIVE, (
char*)&sockOpt,
sizeof(sockOpt));
124 puts(
"Creating new task: IncMoveTask");
126 controller->
tidIncMoveThread = mpCreateTask(MP_PRI_IP_CLK_TAKE, MP_STACK_SIZE,
128 (
int)controller, 0, 0, 0, 0, 0, 0, 0, 0, 0);
129 if (controller->tidIncMoveThread ==
ERROR)
131 puts(
"Failed to create task for incremental-motion. Check robot parameters.");
135 mpSetAlarm(8004,
"MOTOROS FAILED TO CREATE TASK", 4);
142 for(groupNo = 0; groupNo < controller->
numGroup; groupNo++)
146 printf(
"Creating new task: tidAddToIncQueue (groupNo = %d)\n", groupNo);
150 (
int)controller, groupNo, 0, 0, 0, 0, 0, 0, 0, 0);
151 if (controller->ctrlGroups[groupNo]->tidAddToIncQueue ==
ERROR)
153 puts(
"Failed to create task for parsing motion increments. Check robot parameters.");
157 mpSetAlarm(8004,
"MOTOROS FAILED TO CREATE TASK", 5);
166 printf(
"Creating new task: tidMotionConnections (connectionIndex = %d)\n", connectionIndex);
170 controller->
tidMotionConnections[connectionIndex] = mpCreateTask(MP_PRI_TIME_NORMAL, MP_STACK_SIZE,
172 (
int)controller, connectionIndex, 0, 0, 0, 0, 0, 0, 0, 0);
174 if (controller->tidMotionConnections[connectionIndex] !=
ERROR)
180 puts(
"Could not create new task in the motion server. Check robot parameters.");
182 controller->sdMotionConnections[connectionIndex] =
INVALID_SOCKET;
183 controller->tidMotionConnections[connectionIndex] =
INVALID_TASK;
185 mpSetAlarm(8004,
"MOTOROS FAILED TO CREATE TASK", 6);
199 BOOL bDeleteIncMovTask;
201 printf(
"Closing Motion Server Connection\r\n");
209 bDeleteIncMovTask =
TRUE;
214 bDeleteIncMovTask =
FALSE;
220 if(bDeleteIncMovTask)
226 for(i=0; i < controller->
numGroup; i++)
243 printf(
"Motion Server Connection Closed\r\n");
256 expectedSize = minSize;
275 if (recvByteSize >= (
int)(minSize +
sizeof(int)))
277 expectedSize = minSize + (
sizeof(int) * 2);
299 expectedSize = minSize;
317 int byteSize = 0, byteSizeResponse = 0;
321 int partialMsgByteCount = 0;
322 BOOL bSkipNetworkRecv =
FALSE;
328 if (!bSkipNetworkRecv)
331 memset((&receiveMsg) + partialMsgByteCount, 0x00,
sizeof(
SimpleMsg) - partialMsgByteCount);
332 byteSize = mpRecv(controller->
sdMotionConnections[connectionIndex], (
char*)((&receiveMsg) + partialMsgByteCount),
sizeof(
SimpleMsg) - partialMsgByteCount, 0);
336 byteSize += partialMsgByteCount;
337 partialMsgByteCount = 0;
341 byteSize = partialMsgByteCount;
342 partialMsgByteCount = 0;
343 bSkipNetworkRecv =
FALSE;
348 if(byteSize >= minSize)
352 if (expectedSize == -1)
354 printf(
"Unknown Message Received (%d)\r\n", receiveMsg.
header.
msgType);
357 else if (byteSize >= expectedSize)
365 else if (byteSize > expectedSize)
372 partialMsgByteCount = 0;
377 Db_Print(
"MessageReceived(%d bytes): expectedSize=%d, processing rest of bytes (%d, %d, %d)\r\n", byteSize, expectedSize,
sizeof(receiveMsg), receiveMsg.
body.
jointTrajData.
sequence, ((
int*)((
char*)&receiveMsg + expectedSize))[5]);
378 partialMsgByteCount = byteSize - expectedSize;
379 memmove(&receiveMsg, (
char*)&receiveMsg + expectedSize, partialMsgByteCount);
382 if (partialMsgByteCount >= minSize)
385 bSkipNetworkRecv = (partialMsgByteCount >= expectedSize);
390 partialMsgByteCount = 0;
394 Db_Print(
"MessageReceived(%d bytes): expectedSize=%d\r\n", byteSize, expectedSize);
400 Db_Print(
"Unknown Data Received (%d bytes)\r\n", byteSize);
408 if (byteSizeResponse <= 0)
427 int invalidSubcode = 0;
432 memset(replyMsg, 0x00,
sizeof(
SimpleMsg));
489 printf(
"Invalid message type: %d\n", receiveMsg->
header.
msgType);
495 if(invalidSubcode != 0)
523 printf(
"ERROR: Controller is not ready (code: %d). Can't process ROS_MSG_MOTO_JOINT_TRAJ_PT_FULL_EX.\r\n", subcode);
575 printf(
"ERROR: Ros_MotionServer_InitTrajPointFullEx returned %d\n", ret);
590 printf(
"ERROR: Ros_MotionServer_AddTrajPointFullEx returned %d\n", ret);
596 printf(
"ERROR: Ros_MotionServer_AddTrajPointFullEx returned %d\n", ret);
741 for(groupNo=0; groupNo<controller->
numGroup; groupNo++)
755 return(bStopped && bRet);
764 MP_SERVO_POWER_SEND_DATA sServoData;
765 MP_STD_RSP_DATA stdRespData;
769 #ifdef DUMMY_SERVO_MODE 773 if (servoOnOff == OFF)
776 if (servoOnOff == ON)
786 printf(
"Setting servo power: %d\n", servoOnOff);
787 memset(&sServoData, 0x00,
sizeof(sServoData));
788 memset(&stdRespData, 0x00,
sizeof(stdRespData));
789 sServoData.sServoPower = servoOnOff;
791 ret = mpSetServoPower(&sServoData, &stdRespData);
792 if( (ret == 0) && (stdRespData.err_no == 0) )
821 MP_ALARM_STATUS_RSP_DATA alarmstatus;
822 MP_STD_RSP_DATA responseData;
824 returnBoolean =
TRUE;
826 ret = mpGetAlarmStatus(&alarmstatus);
829 printf(
"Could not get alarm status\n");
835 MP_ALARM_CODE_RSP_DATA alarmcode;
836 ret = mpGetAlarmCode(&alarmcode);
839 printf(
"Could not get alarm code\n");
844 for (i = 0; i < alarmcode.usAlarmNum; i += 1)
845 printf(
"Has alarm: %d[%d], resetting...\n", alarmcode.AlarmData.usAlarmNo[i], alarmcode.AlarmData.usAlarmData[i]);
848 ret = mpResetAlarm(&responseData);
851 printf(
"Could not reset the alarm, failure code: %d\n", responseData.err_no);
852 returnBoolean =
FALSE;
858 MP_ALARM_CODE_RSP_DATA alarmcode;
859 ret = mpGetAlarmCode(&alarmcode);
862 printf(
"Could not get error code\n");
867 printf(
"Has error: %d[%d], resetting...\n", alarmcode.usErrorNo, alarmcode.usErrorData);
870 ret = mpCancelError(&responseData);
873 printf(
"Could not cancel the error, failure code: %d\n", responseData.err_no);
874 returnBoolean =
FALSE;
879 return returnBoolean;
888 MP_STD_RSP_DATA rData;
889 MP_START_JOB_SEND_DATA sStartData;
894 printf(
"In StartTrajMode\r\n");
911 #ifndef DUMMY_SERVO_MODE 923 memset(&rData, 0x00,
sizeof(rData));
924 ret = mpCancelError(&rData);
933 memset(&rData, 0x00,
sizeof(rData));
934 ret = mpResetAlarm(&rData);
957 #ifndef DUMMY_SERVO_MODE 961 MP_SERVO_POWER_SEND_DATA sServoData;
962 memset(&sServoData, 0x00,
sizeof(sServoData));
970 sServoData.sServoPower = 1;
971 memset(&rData, 0x00,
sizeof(rData));
972 ret = mpSetServoPower(&sServoData, &rData);
973 if( (ret == 0) && (rData.err_no ==0) )
999 for(grpNo = 0; grpNo < MP_GRP_NUM; ++grpNo)
1008 memset(&rData, 0x00,
sizeof(rData));
1009 memset(&sStartData, 0x00,
sizeof(sStartData));
1010 sStartData.sTaskNo = 0;
1012 ret = mpStartJob(&sStartData, &rData);
1013 if( (ret != 0) || (rData.err_no !=0) )
1083 printf(
"ERROR: Controller is not ready (code: %d). Can't process ROS_MSG_JOINT_TRAJ_PT_FULL.\r\n", subcode);
1104 if( (trajData->
validFields & validationFlags) != validationFlags)
1106 printf(
"ERROR: Validfields = %d\r\n", trajData->
validFields);
1164 jointTrajData.
time = jointTrajDataEx->
time;
1166 memcpy(jointTrajData.
vel, jointTrajDataEx->
vel,
sizeof(
float)*ROS_MAX_JOINT);
1167 memcpy(jointTrajData.
acc, jointTrajDataEx->
acc,
sizeof(
float)*ROS_MAX_JOINT);
1177 long pulsePos[MAX_PULSE_AXES];
1178 long curPos[MAX_PULSE_AXES];
1205 for(i=0; i<MAX_PULSE_AXES; i++)
1210 printf(
"ERROR: Trajectory start position doesn't match current position (MOTO joint order).\r\n");
1211 printf(
" - Requested start: %ld, %ld, %ld, %ld, %ld, %ld, %ld, %ld\r\n",
1212 pulsePos[0], pulsePos[1], pulsePos[2],
1213 pulsePos[3], pulsePos[4], pulsePos[5],
1214 pulsePos[6], pulsePos[7]);
1215 printf(
" - Current pos: %ld, %ld, %ld, %ld, %ld, %ld, %ld, %ld\r\n",
1216 curPos[0], curPos[1], curPos[2],
1217 curPos[3], curPos[4], curPos[5],
1218 curPos[6], curPos[7]);
1219 printf(
" - ctrlGroup->prevPulsePos: %ld, %ld, %ld, %ld, %ld, %ld, %ld, %ld\r\n",
1254 jointTrajData.
time = jointTrajDataEx->
time;
1256 memcpy(jointTrajData.
vel, jointTrajDataEx->
vel,
sizeof(
float)*ROS_MAX_JOINT);
1257 memcpy(jointTrajData.
acc, jointTrajDataEx->
acc,
sizeof(
float)*ROS_MAX_JOINT);
1282 for(i=0; i<ctrlGroup->
numAxes; i++)
1291 printf(
"ERROR: Invalid speed in message TrajPointFull data: \r\n axis: %d, speed: %f, limit: %f\r\n",
1295 Ros_SimpleMsg_DumpTrajPtFull(jointTrajData);
1357 float accCoef1[MP_GRP_AXES_NUM];
1358 float accCoef2[MP_GRP_AXES_NUM];
1360 int calculationTime_ms;
1362 long newPulsePos[MP_GRP_AXES_NUM];
1370 startTrajData = &_startTrajData;
1379 endTrajData->
pos[3] += -endTrajData->
pos[1] + endTrajData->
pos[2];
1380 endTrajData->
vel[3] += -endTrajData->
vel[1] + endTrajData->
vel[2];
1383 memset(newPulsePos, 0x00,
sizeof(newPulsePos));
1384 memset(&incData, 0x00,
sizeof(incData));
1385 incData.
frame = MP_INC_PULSE_DTYPE;
1389 memset(&accCoef1, 0x00,
sizeof(accCoef1));
1390 memset(&accCoef2, 0x00,
sizeof(accCoef2));
1391 interval = (endTrajData->
time - startTrajData->
time) / 1000.0
f;
1394 for (i = 0; i < ctrlGroup->
numAxes; i++)
1397 accCoef1[i] = ( 6 * (endTrajData->
pos[i] - startTrajData->
pos[i]) / (interval * interval) )
1398 - ( 2 * (endTrajData->
vel[i] + 2 * startTrajData->
vel[i]) / interval);
1399 accCoef2[i] = ( -12 * (endTrajData->
pos[i] - startTrajData->
pos[i]) / (interval * interval * interval))
1400 + ( 6 * (endTrajData->
vel[i] + startTrajData->
vel[i]) / (interval * interval) );
1405 printf(
"Warning: Group %d - Time difference between endTrajData (%d) and startTrajData (%d) is 0 or less.\r\n", groupNo, endTrajData->
time, startTrajData->
time);
1409 calculationTime_ms = startTrajData->
time;
1411 timeInc_ms = interpolPeriod;
1419 calculationTime_ms += timeInc_ms;
1420 interpolTime = (calculationTime_ms - startTrajData->
time) / 1000.0
f;
1422 if( calculationTime_ms < endTrajData->
time )
1425 curTrajData->
time = calculationTime_ms;
1428 for (i = 0; i < ctrlGroup->
numAxes; i++)
1431 curTrajData->
pos[i] = startTrajData->
pos[i]
1432 + startTrajData->
vel[i] * interpolTime
1433 + accCoef1[i] * interpolTime * interpolTime / 2
1434 + accCoef2[i] * interpolTime * interpolTime * interpolTime / 6;
1437 curTrajData->
vel[i] = startTrajData->
vel[i]
1438 + accCoef1[i] * interpolTime
1439 + accCoef2[i] * interpolTime * interpolTime / 2;
1443 if(timeInc_ms < interpolPeriod)
1445 timeInc_ms = interpolPeriod;
1455 if(calculationTime_ms > endTrajData->
time)
1466 for (i = 0; i < MP_GRP_AXES_NUM; i++)
1512 q->
data[index] = *dataToEnQ;
1521 printf(
"ERROR: Unable to add point to queue. Queue is locked up!\r\n");
1567 for(groupNo=0; groupNo<controller->
numGroup; groupNo++)
1602 printf(
"ERROR: Unable to access queue count. Queue is locked up!\r\n");
1616 for(groupNo=0; groupNo<controller->
numGroup; groupNo++)
1621 else if (qCnt ==
ERROR)
1636 MP_POS_DATA moveData;
1638 MP_EXPOS_DATA moveData;
1649 printf(
"IncMoveTask Started\r\n");
1651 memset(&moveData, 0x00,
sizeof(moveData));
1653 for(i=0; i<controller->
numGroup; i++)
1655 moveData.ctrl_grp |= (0x01 << i);
1661 mpClkAnnounce(MP_INTERPOLATION_CLK);
1669 for(i=0; i<controller->
numGroup; i++)
1680 moveData.grp_pos_info[i].pos_tag.data[2] = q->
data[q->
idx].
tool;
1681 moveData.grp_pos_info[i].pos_tag.data[3] = q->
data[q->
idx].
frame;
1682 moveData.grp_pos_info[i].pos_tag.data[4] = q->
data[q->
idx].
user;
1684 memcpy(&moveData.grp_pos_info[i].pos, &q->
data[q->
idx].
inc,
sizeof(LONG) * MP_GRP_AXES_NUM);
1693 if( (q_time <= q->
data[q->
idx].time)
1694 && (q->
data[q->
idx].
time - q_time <= controller->interpolPeriod) )
1699 if( (moveData.grp_pos_info[i].pos_tag.data[2] != q->
data[q->
idx].
tool)
1700 || (moveData.grp_pos_info[i].pos_tag.data[3] != q->
data[q->
idx].
frame)
1701 || (moveData.grp_pos_info[i].pos_tag.data[4] != q->
data[q->
idx].
user) )
1708 for(axis=0; axis<MP_GRP_AXES_NUM; axis++)
1709 moveData.grp_pos_info[i].pos[axis] += q->
data[q->
idx].
inc[axis];
1727 moveData.grp_pos_info[i].pos_tag.data[2] = 0;
1728 moveData.grp_pos_info[i].pos_tag.data[3] = MP_INC_PULSE_DTYPE;
1729 moveData.grp_pos_info[i].pos_tag.data[4] = 0;
1730 memset(&moveData.grp_pos_info[i].pos, 0x00,
sizeof(LONG) * MP_GRP_AXES_NUM);
1738 printf(
"ERROR: Can't get data from queue. Queue is locked up.\r\n");
1739 memset(&moveData.grp_pos_info[i].pos, 0x00,
sizeof(LONG) * MP_GRP_AXES_NUM);
1746 if (controller->bIsDx100Sda)
1747 moveData.ctrl_grp = 1 | (1 << 2);
1749 moveData.ctrl_grp = 1;
1750 ret = mpMeiIncrementMove(MP_SL_ID1, &moveData);
1754 printf(
"mpMeiIncrementMove returned: %d (ctrl_grp = %d)\r\n", ret, moveData.ctrl_grp);
1756 printf(
"mpMeiIncrementMove returned: %d\r\n", ret);
1759 moveData.ctrl_grp = 2;
1762 ret = mpMeiIncrementMove(MP_SL_ID2, &moveData);
1766 printf(
"mpMeiIncrementMove returned: %d (ctrl_grp = %d)\r\n", ret, moveData.ctrl_grp);
1768 printf(
"mpMeiIncrementMove returned: %d\r\n", ret);
1772 ret = mpExRcsIncrementMove(&moveData);
1775 if(ret == E_EXRCS_CTRL_GRP)
1776 printf(
"mpExRcsIncrementMove returned: %d (ctrl_grp = %d)\r\n", ret, moveData.ctrl_grp);
1777 #if (YRC1000||YRC1000u) 1778 else if (ret == E_EXRCS_IMOV_UNREADY)
1780 printf(
"mpExRcsIncrementMove returned UNREADY: %d (Could be PFL Active)\r\n", ret);
1785 printf(
"mpExRcsIncrementMove returned: %d\r\n", ret);
1815 jointMotionData->
time = (int)(jointTrajData->
time * 1000);
1817 for(i=0; i<maxAxes; i++)
1819 jointMotionData->
pos[i] = jointTrajData->
pos[i];
1820 jointMotionData->
vel[i] = jointTrajData->
vel[i];
1821 jointMotionData->
acc[i] = jointTrajData->
acc[i];
1830 printf(
"%s %s\r\n", msgPrefix, errMsg);
1835 MP_SERVO_POWER_SEND_DATA sServoData;
1836 MP_STD_RSP_DATA rData;
1839 #ifdef DUMMY_SERVO_MODE 1846 sServoData.sServoPower = 0;
1847 memset(&sServoData, 0x00,
sizeof(sServoData));
1848 memset(&rData, 0x00,
sizeof(rData));
1849 ret = mpSetServoPower(&sServoData, &rData);
1850 if ((ret == 0) && (rData.err_no == 0))
1884 memset(replyMsg, 0x00,
sizeof(
SimpleMsg));
1894 for (i = 0; i < controller->
numGroup; i += 1)
1916 if (groupNo >= 0 && groupNo < controller->numRobot)
void Ros_Controller_SetIOState(ULONG signal, BOOL status)
LONG inc[MP_GRP_AXES_NUM]
int Ros_MotionServer_InitTrajPointFullEx(CtrlGroup *ctrlGroup, SmBodyJointTrajPtExData *jointTrajDataEx, int sequence)
void Ros_Controller_ErrNo_ToString(int errNo, char errMsg[ERROR_MSG_MAX_SIZE], int errMsgSize)
AXIS_TYPE type[MAX_PULSE_AXES]
void Ros_MotionServer_IncMoveLoopStart(Controller *controller)
BOOL Ros_Controller_IsHold(Controller *controller)
BOOL Ros_MotionServer_AddPulseIncPointToQ(Controller *controller, int groupNo, Incremental_data *dataToEnQ)
SmBodyJointTrajPtFull jointTrajData
BOOL Ros_CtrlGroup_GetPulsePosCmd(CtrlGroup *ctrlGroup, long pulsePos[MAX_PULSE_AXES])
int Ros_MotionServer_SetSelectedTool(Controller *controller, SimpleMsg *receiveMsg, SimpleMsg *replyMsg)
#define IO_FEEDBACK_FAILURE
BOOL Ros_Controller_IsOperating(Controller *controller)
DH_PARAMETERS dhParameters[MOT_MAX_GR]
BOOL Ros_Controller_IsServoOn(Controller *controller)
int Ros_MotionServer_AddTrajPointFull(CtrlGroup *ctrlGroup, SmBodyJointTrajPtFull *jointTrajData)
struct _SmHeader SmHeader
struct _SmBodyMotoMotionCtrl SmBodyMotoMotionCtrl
void Ros_MotionServer_JointTrajDataToIncQueue(Controller *controller, int groupNo)
#define MOTION_START_TIMEOUT
int Ros_MotionServer_JointTrajDataProcess(Controller *controller, SimpleMsg *receiveMsg, SimpleMsg *replyMsg)
#define START_MAX_PULSE_DEVIATION
SmBodyMotoMotionCtrl motionCtrl
#define MASK_ISALARM_ACTIVEALARM
int Ros_IoServer_WriteIOBit(SimpleMsg *receiveMsg, SimpleMsg *replyMsg)
CtrlGroup * ctrlGroups[MP_GRP_NUM]
#define ERROR_MSG_MAX_SIZE
struct _SmBodyMotoWriteIOGroup SmBodyMotoWriteIOGroup
float maxSpeed[MP_GRP_AXES_NUM]
void Ros_MotionServer_WaitForSimpleMsg(Controller *controller, int connectionIndex)
#define Q_OFFSET_IDX(a, b, c)
int Ros_MotionServer_GetQueueCnt(Controller *controller, int groupNo)
long prevPulsePos[MAX_PULSE_AXES]
int Ros_MotionServer_GetExpectedByteSizeForMessageType(SimpleMsg *receiveMsg, int recvByteSize)
SmBodyJointTrajPtFullEx jointTrajDataEx
struct _SmBodySelectTool SmBodySelectTool
int Ros_IoServer_WriteIOGroup(SimpleMsg *receiveMsg, SimpleMsg *replyMsg)
void Db_Print(char *msgFormat,...)
float pos[MP_GRP_AXES_NUM]
BOOL Ros_MotionServer_ResetAlarm(Controller *controller)
int Ros_SimpleMsg_MotionReply(SimpleMsg *receiveMsg, int result, int subcode, SimpleMsg *replyMsg, int ctrlGrp)
struct _SmBodyMotoReadIOGroup SmBodyMotoReadIOGroup
int sdMotionConnections[MAX_MOTION_CONNECTIONS]
#define IO_FEEDBACK_MOTIONSERVERCONNECTED
struct _SmBodyJointTrajPtExData SmBodyJointTrajPtExData
void Ros_CtrlGroup_ConvertToMotoPos(CtrlGroup *ctrlGroup, float radPos[MAX_PULSE_AXES], long motopulsePos[MAX_PULSE_AXES])
int tidMotionConnections[MAX_MOTION_CONNECTIONS]
int Ros_MotionServer_MotionCtrlProcess(Controller *controller, SimpleMsg *receiveMsg, SimpleMsg *replyMsg)
void Ros_Sleep(float milliseconds)
STATUS Ros_MotionServer_DisableEcoMode(Controller *controller)
struct _SmBodyMotoMotionReply SmBodyMotoMotionReply
BOOL Ros_MotionServer_ServoPower(Controller *controller, int servoOnOff)
BOOL Ros_Controller_IsEcoMode(Controller *controller)
#define MIN_VALID_TOOL_INDEX
SmBodySelectTool selectTool
struct _SmBodyJointTrajPtFullEx SmBodyJointTrajPtFullEx
float data[ROS_MAX_JOINT]
#define IO_FEEDBACK_MP_INCMOVE_DONE
#define MOTION_INIT_ROS_JOB
struct _SmBodyJointFeedback SmBodyJointFeedback
int Ros_MotionServer_InitTrajPointFull(CtrlGroup *ctrlGroup, SmBodyJointTrajPtFull *jointTrajData)
BOOL Ros_Controller_IsValidGroupNo(Controller *controller, int groupNo)
UCHAR Ros_CtrlGroup_GetAxisConfig(CtrlGroup *ctrlGroup)
BOOL Ros_Controller_IsAlarm(Controller *controller)
double min(double a, double b)
float vel[MP_GRP_AXES_NUM]
BOOL Ros_Controller_IsRemote(Controller *controller)
int Ros_MotionServer_GetDhParameters(Controller *controller, SimpleMsg *replyMsg)
struct _SmBodyJointFeedbackEx SmBodyJointFeedbackEx
FlagsValidFields validFields
float acc[MP_GRP_AXES_NUM]
#define MAX_VALID_TOOL_INDEX
SmBodyMotoGetDhParameters dhParameters
void Ros_MotionServer_AddToIncQueueProcess(Controller *controller, int groupNo)
void Ros_MotionServer_PrintError(USHORT err_no, char *msgPrefix)
int Ros_IoServer_ReadIOGroup(SimpleMsg *receiveMsg, SimpleMsg *replyMsg)
int Ros_MotionServer_JointTrajPtFullExProcess(Controller *controller, SimpleMsg *receiveMsg, SimpleMsg *replyMsg)
BOOL Ros_Controller_StatusUpdate(Controller *controller)
#define MOTION_START_CHECK_PERIOD
struct _SmBodyMotoWriteIOBit SmBodyMotoWriteIOBit
BOOL Ros_MotionServer_HasDataInQueue(Controller *controller)
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
#define MAX_MOTION_CONNECTIONS
#define MOTION_STOP_TIMEOUT
FlagsValidFields validFields
BOOL Ros_Controller_IsMotionReady(Controller *controller)
BOOL Ros_Controller_IsEStop(Controller *controller)
SmBodyJointTrajPtExData jointTrajPtData[MOT_MAX_GR]
BOOL Ros_MotionServer_SimpleMsgProcess(Controller *controller, SimpleMsg *receiveMsg, SimpleMsg *replyMsg)
void Ros_MotionServer_ConvertToJointMotionData(SmBodyJointTrajPtFull *jointTrajData, JointMotionData *jointMotionData)
int Ros_IoServer_ReadIOBit(SimpleMsg *receiveMsg, SimpleMsg *replyMsg)
int Ros_Controller_GetNotReadySubcode(Controller *controller)
JointMotionData jointMotionData
struct _SmBodyMotoReadIOBit SmBodyMotoReadIOBit
BOOL Ros_MotionServer_StartTrajMode(Controller *controller)
struct _SmBodyRobotStatus SmBodyRobotStatus
BOOL Ros_MotionServer_StopTrajMode(Controller *controller)
Incremental_data data[Q_SIZE]
BOOL Ros_MotionServer_StopMotion(Controller *controller)
#define MAX_TRAJECTORY_TIME_LENGTH
BOOL Ros_Controller_IsError(Controller *controller)
struct _SmBodyJointTrajPtFull SmBodyJointTrajPtFull
int Ros_MotionServer_AddTrajPointFullEx(CtrlGroup *ctrlGroup, SmBodyJointTrajPtExData *jointTrajDataEx, int sequence)
JointMotionData jointMotionDataToProcess
void Ros_MotionServer_StopConnection(Controller *controller, int connectionIndex)
void Ros_MotionServer_StartNewConnection(Controller *controller, int sd)
#define MASK_ISALARM_ACTIVEERROR
AXIS_MOTION_TYPE axisType
BOOL Ros_MotionServer_ClearQ(Controller *controller, int groupNo)
BOOL Ros_MotionServer_ClearQ_All(Controller *controller)