diff --git a/motoman_driver/MotoPlus/Controller.c b/motoman_driver/MotoPlus/Controller.c
index a01d96ec..49560975 100644
--- a/motoman_driver/MotoPlus/Controller.c
+++ b/motoman_driver/MotoPlus/Controller.c
@@ -227,7 +227,9 @@ BOOL Ros_Controller_Init(Controller* controller)
 		if(grpNo < controller->numGroup)
 		{
 			// Determine if specific group exists and allocate memory for it
-			controller->ctrlGroups[grpNo] = Ros_CtrlGroup_Create(grpNo, controller->interpolPeriod);
+			controller->ctrlGroups[grpNo] = Ros_CtrlGroup_Create(grpNo,								//Zero based index of the group number(0 - 3)
+																(grpNo==(controller->numGroup-1)),	//TRUE if this is the final group that is being initialized. FALSE if you plan to call this function again.
+																controller->interpolPeriod);		//Value of the interpolation period (ms) for the robot controller.
 			if(controller->ctrlGroups[grpNo] != NULL)
 			{
 				Ros_CtrlGroup_GetPulsePosCmd(controller->ctrlGroups[grpNo], controller->ctrlGroups[grpNo]->prevPulsePos); // set the current commanded pulse
diff --git a/motoman_driver/MotoPlus/CtrlGroup.c b/motoman_driver/MotoPlus/CtrlGroup.c
index 3783d932..fd1237f3 100644
--- a/motoman_driver/MotoPlus/CtrlGroup.c
+++ b/motoman_driver/MotoPlus/CtrlGroup.c
@@ -31,20 +31,6 @@
 
 #include "MotoROS.h"
 
-//-----------------------
-// Function Declarations
-//-----------------------
-MP_GRP_ID_TYPE Ros_CtrlGroup_FindGrpId(int groupNo);
-CtrlGroup* Ros_CtrlGroup_Create(int groupNo, float interpolPeriod);
-BOOL Ros_CtrlGroup_GetPulsePosCmd(CtrlGroup* ctrlGroup, long pulsePos[MAX_PULSE_AXES]);
-BOOL Ros_CtrlGroup_GetFBPulsePos(CtrlGroup* ctrlGroup, long pulsePos[MAX_PULSE_AXES]);
-void Ros_CtrlGroup_ConvertToRosPos(CtrlGroup* ctrlGroup, long pulsePos[MAX_PULSE_AXES], 
-									float rosPos[MAX_PULSE_AXES]);
-void Ros_CtrlGroup_ConvertToMotoPos(CtrlGroup* ctrlGroup, float rosPos[MAX_PULSE_AXES],
-									long pulsePos[MAX_PULSE_AXES]);
-UCHAR Ros_CtrlGroup_GetAxisConfig(CtrlGroup* ctrlGroup);
-BOOL Ros_CtrlGroup_IsRobot(CtrlGroup* ctrlGroup);
-
 //-----------------------
 // Function implementation
 //-----------------------
@@ -70,7 +56,7 @@ MP_GRP_ID_TYPE Ros_CtrlGroup_FindGrpId(int groupNo)
 // Create a CtrlGroup data structure for existing group otherwise 
 // return NULL
 //-------------------------------------------------------------------
-CtrlGroup* Ros_CtrlGroup_Create(int groupNo, float interpolPeriod)
+CtrlGroup* Ros_CtrlGroup_Create(int groupNo, BOOL bIsLastGrpToInit, float interpolPeriod)
 {
 	CtrlGroup* ctrlGroup;
 	int numAxes;
@@ -124,6 +110,15 @@ CtrlGroup* Ros_CtrlGroup_Create(int groupNo, float interpolPeriod)
 		if (status != OK)
 			bInitOk = FALSE;
 
+		status = GP_getFeedbackSpeedMRegisterAddresses(groupNo, //zero based index of the control group
+														TRUE, //If the register-speed-feedback is not enabled, automatically modify the SC.PRM file to enable this feature.
+														bIsLastGrpToInit, //If activating the reg-speed-feedback feature, delay the alarm until all the groups have been processed.
+														&ctrlGroup->speedFeedbackRegisterAddress); //[OUT] Index of the M registers containing the feedback speed values.
+		if (status != OK)
+		{
+			ctrlGroup->speedFeedbackRegisterAddress.bFeedbackSpeedEnabled = FALSE;
+		}
+
 		ctrlGroup->bIsBaxisSlave = (numAxes == 5) && slaveAxis;
 
 		//adjust the axisType field to account for robots with non-contiguous axes (such as delta or palletizing which use SLU--T axes)
@@ -211,12 +206,14 @@ CtrlGroup* Ros_CtrlGroup_Create(int groupNo, float interpolPeriod)
 
 //-------------------------------------------------------------------
 // Get the commanded pulse position in pulse (in motoman joint order)
+// Used for MOTION SERVER connection for positional planning calculations.
 //-------------------------------------------------------------------
 BOOL Ros_CtrlGroup_GetPulsePosCmd(CtrlGroup* ctrlGroup, long pulsePos[MAX_PULSE_AXES])
 {
 	LONG status = 0;
 	MP_CTRL_GRP_SEND_DATA sData;
 	MP_PULSE_POS_RSP_DATA pulse_data;
+	float rosAnglePos[MP_GRP_AXES_NUM]; //temporary storage for B axis compensation
 	int i;
 
 	memset(pulsePos, 0, MAX_PULSE_AXES*sizeof(long));  // clear result, in case of error
@@ -252,12 +249,23 @@ BOOL Ros_CtrlGroup_GetPulsePosCmd(CtrlGroup* ctrlGroup, long pulsePos[MAX_PULSE_
 	for (i=0; i<MAX_PULSE_AXES; ++i)
 		pulsePos[i] = pulse_data.lPos[i];
 
+	// For MPL80/100 robot type (SLUBT): Controller automatically moves the B-axis
+	// to maintain orientation as other axes are moved.
+	if (ctrlGroup->bIsBaxisSlave)
+	{
+		//B axis compensation works on the ROS ANGLE positions, not on MOTO PULSE positions
+		Ros_CtrlGroup_ConvertToRosPos(ctrlGroup, pulsePos, rosAnglePos);
+		rosAnglePos[3] += -rosAnglePos[1] + rosAnglePos[2];
+		Ros_CtrlGroup_ConvertToMotoPos(ctrlGroup, rosAnglePos, pulsePos);
+	}
+
 	return TRUE;
 }
 
 
 //-------------------------------------------------------------------
-// Get the corrected feedback pulse position in pulse
+// Get the corrected feedback pulse position in pulse.
+// Used exclusively for STATE SERVER connection to report position.
 //-------------------------------------------------------------------
 BOOL Ros_CtrlGroup_GetFBPulsePos(CtrlGroup* ctrlGroup, long pulsePos[MAX_PULSE_AXES])
 {
@@ -321,10 +329,99 @@ BOOL Ros_CtrlGroup_GetFBPulsePos(CtrlGroup* ctrlGroup, long pulsePos[MAX_PULSE_A
 	// assign return value
 	for (i=0; i<MAX_PULSE_AXES; ++i)
 		pulsePos[i] = pulse_data.lPos[i];
+
+	//--------------------------------------------------------------------
+	//NOTE: Do NOT apply any B axis compensation here. 
+	//		This is actual feedback which is reported to the state server.
+	//--------------------------------------------------------------------
 	
 	return TRUE;
 }
 
+//-------------------------------------------------------------------
+// Get the corrected feedback pulse speed in pulse for each axis.
+//-------------------------------------------------------------------
+BOOL Ros_CtrlGroup_GetFBServoSpeed(CtrlGroup* ctrlGroup, long pulseSpeed[MAX_PULSE_AXES])
+{
+	int i;
+
+#ifndef DUMMY_SERVO_MODE
+	LONG status;
+	MP_IO_INFO registerInfo[MAX_PULSE_AXES * 2]; //values are 4 bytes, which consumes 2 registers
+	USHORT registerValues[MAX_PULSE_AXES * 2];
+	UINT32 registerValuesLong[MAX_PULSE_AXES * 2];
+	double dblRegister;
+
+	if (!ctrlGroup->speedFeedbackRegisterAddress.bFeedbackSpeedEnabled)
+		return FALSE;
+
+	for (i = 0; i < MAX_PULSE_AXES; i += 1)
+	{
+		registerInfo[i * 2].ulAddr = ctrlGroup->speedFeedbackRegisterAddress.cioAddressForAxis[i][0];
+		registerInfo[(i * 2) + 1].ulAddr = ctrlGroup->speedFeedbackRegisterAddress.cioAddressForAxis[i][1];
+	}
+
+	// get raw (uncorrected/unscaled) joint speeds
+	status = mpReadIO(registerInfo, registerValues, MAX_PULSE_AXES * 2);
+	if (status != OK)
+	{
+		printf("Failed to get pulse feedback speed: %u\n", status);
+		return FALSE;
+	}
+
+	for (i = 0; i < MAX_PULSE_AXES; i += 1)
+	{
+		//move to 32 bit storage
+		registerValuesLong[i * 2] = registerValues[i * 2];
+		registerValuesLong[(i * 2) + 1] = registerValues[(i * 2) + 1];
+
+		//combine both registers into single 4 byte value (0.0001 deg/sec or 1 um/sec)
+		dblRegister = (registerValuesLong[(i * 2) + 1] << 16) | registerValuesLong[i * 2];
+
+		//convert to pulse/sec
+		if (ctrlGroup->axisType.type[i] == AXIS_ROTATION)
+		{
+			dblRegister /= 1.0E4; //deg/sec
+			dblRegister *= RAD_PER_DEGREE; //rad/sec
+			dblRegister *= ctrlGroup->pulseToRad.PtoR[i]; //pulse/sec
+		}
+		else if (ctrlGroup->axisType.type[i] == AXIS_LINEAR)
+		{
+			dblRegister /= 1.0E6; //m/sec
+			dblRegister *= ctrlGroup->pulseToMeter.PtoM[i]; //pulse/sec
+		}
+
+		pulseSpeed[i] = (long)dblRegister;
+	}
+
+	// Apply correction to account for cross-axis coupling.
+	// Note: This is only required for feedback.
+	// Controller handles this correction internally when 
+	// dealing with command positon.
+	for (i = 0; i<MAX_PULSE_AXES; ++i)
+	{
+		FB_AXIS_CORRECTION *corr = &ctrlGroup->correctionData.correction[i];
+		if (corr->bValid)
+		{
+			int src_axis = corr->ulSourceAxis;
+			int dest_axis = corr->ulCorrectionAxis;
+			pulseSpeed[dest_axis] -= (int)(pulseSpeed[src_axis] * corr->fCorrectionRatio);
+		}
+	}
+#else
+	MP_CTRL_GRP_SEND_DATA sData;
+	MP_SERVO_SPEED_RSP_DATA pulse_data;
+
+	mpGetServoSpeed(&sData, &pulse_data);
+
+	// assign return value
+	for (i = 0; i<MAX_PULSE_AXES; ++i)
+		pulseSpeed[i] = pulse_data.lSpeed[i];
+#endif
+
+	return TRUE;
+}
+
 //-------------------------------------------------------------------
 // Retrieves the absolute value (Nm) of the maximum current servo torque.
 //-------------------------------------------------------------------
diff --git a/motoman_driver/MotoPlus/CtrlGroup.h b/motoman_driver/MotoPlus/CtrlGroup.h
index 3475438c..f7dd184e 100644
--- a/motoman_driver/MotoPlus/CtrlGroup.h
+++ b/motoman_driver/MotoPlus/CtrlGroup.h
@@ -43,7 +43,9 @@
 
 #define	Q_OFFSET_IDX( a, b, c )	(((a)+(b)) >= (c) ) ? ((a)+(b)-(c)) \
 				: ( (((a)+(b)) < 0 ) ? ((a)+(b)+(c)) : ((a)+(b)) )
-				
+		
+#define RAD_PER_DEGREE (0.0174533)
+
 typedef struct
 {
 	LONG time;
@@ -61,8 +63,6 @@ typedef struct
 	Incremental_data data[Q_SIZE];
 } Incremental_q;
 
-
-
 // jointMotionData values are in radian and joint order in sequential order 
 typedef struct
 {
@@ -100,6 +100,8 @@ typedef struct
 	AXIS_MOTION_TYPE axisType;					// Indicates whether axis is rotary or linear
 
 	BOOL bIsBaxisSlave;							// Indicates the B axis will automatically move to maintain orientation as other axes are moved
+
+	JOINT_FEEDBACK_SPEED_ADDRESSES speedFeedbackRegisterAddress; //CIO address for the registers containing feedback speed
 } CtrlGroup;
 
 
@@ -107,15 +109,20 @@ typedef struct
 // External Functions Declaration
 //---------------------------------
 
-extern CtrlGroup* Ros_CtrlGroup_Create(int groupNo, float interpolPeriod);
+//Initialize specific control group. This should be called for each group connected to the robot
+//controller in numerical order.
+//	int groupNo: Zero based index of the group number (0-3)
+//	BOOL bIsLastGrpToInit: TRUE if this is the final group that is being initialized. FALSE if you plan to call this function again.
+//	float interpolPeriod: Value of the interpolation period (ms) for the robot controller.
+extern CtrlGroup* Ros_CtrlGroup_Create(int groupNo, BOOL bIsLastGrpToInit, float interpolPeriod);
 
 extern BOOL Ros_CtrlGroup_GetPulsePosCmd(CtrlGroup* ctrlGroup, long pulsePos[MAX_PULSE_AXES]);
-
 extern BOOL Ros_CtrlGroup_GetFBPulsePos(CtrlGroup* ctrlGroup, long pulsePos[MAX_PULSE_AXES]);
+extern BOOL Ros_CtrlGroup_GetFBServoSpeed(CtrlGroup* ctrlGroup, long pulseSpeed[MAX_PULSE_AXES]);
 
 extern BOOL Ros_CtrlGroup_GetTorque(CtrlGroup* ctrlGroup, double torqueValues[MAX_PULSE_AXES]);
-extern void Ros_CtrlGroup_ConvertToRosPos(CtrlGroup* ctrlGroup, long pulsePos[MAX_PULSE_AXES], float rosPos[MAX_PULSE_AXES]);
 
+extern void Ros_CtrlGroup_ConvertToRosPos(CtrlGroup* ctrlGroup, long pulsePos[MAX_PULSE_AXES], float rosPos[MAX_PULSE_AXES]);
 extern void Ros_CtrlGroup_ConvertToMotoPos(CtrlGroup* ctrlGroup, float radPos[MAX_PULSE_AXES], long pulsePos[MAX_PULSE_AXES]);
 
 extern UCHAR Ros_CtrlGroup_GetAxisConfig(CtrlGroup* ctrlGroup);
diff --git a/motoman_driver/MotoPlus/MotionServer.c b/motoman_driver/MotoPlus/MotionServer.c
index 8f887d95..d03e214a 100644
--- a/motoman_driver/MotoPlus/MotionServer.c
+++ b/motoman_driver/MotoPlus/MotionServer.c
@@ -1,13 +1,5 @@
 // MotionServer.c
 //
-// History:
-// 05/22/2013: Original release v.1.0.0
-// 06/05/2013: Fix for multi-arm control to prevent return -3 (Invalid group) 
-//			   when calling function mpExRcsIncrementMove.
-// 06/12/2013: Release v.1.0.1
-// June 2014:	Release v1.2.0
-//				Add support for multiple control groups.
-//				Add support for DX200 controller.
 /*
 * Software License Agreement (BSD License) 
 *
@@ -486,6 +478,7 @@ int Ros_MotionServer_JointTrajPtFullExProcess(Controller* controller, SimpleMsg*
 	SmBodyJointTrajPtFullEx* msgBody;	
 	CtrlGroup* ctrlGroup;
 	int ret, i;
+	FlagsValidFields validationFlags;
 
 	msgBody = &receiveMsg->body.jointTrajDataEx;
 
@@ -521,7 +514,8 @@ int Ros_MotionServer_JointTrajPtFullExProcess(Controller* controller, SimpleMsg*
 		}
 			
 		// Check that minimum information (time, position, velocity) is valid
-		if( (msgBody->jointTrajPtData[i].validFields & 0x07) != 0x07 )
+		validationFlags = Valid_Time | Valid_Position | Valid_Velocity;
+		if( (msgBody->jointTrajPtData[i].validFields & validationFlags) != validationFlags)
 		{
 			printf("ERROR: Validfields = %d\r\n", msgBody->jointTrajPtData[i].validFields);
 			Ros_SimpleMsg_MotionReply(receiveMsg, ROS_RESULT_INVALID, ROS_RESULT_INVALID_DATA_INSUFFICIENT, replyMsg, msgBody->jointTrajPtData[i].groupNo);
@@ -1042,6 +1036,7 @@ int Ros_MotionServer_JointTrajDataProcess(Controller* controller, SimpleMsg* rec
 	SmBodyJointTrajPtFull* trajData;
 	CtrlGroup* ctrlGroup;
 	int ret;
+	FlagsValidFields validationFlags;
 
 	// Check if controller is able to receive incremental move and if the incremental move thread is running
 	if(!Ros_Controller_IsMotionReady(controller))
@@ -1067,7 +1062,8 @@ int Ros_MotionServer_JointTrajDataProcess(Controller* controller, SimpleMsg* rec
 	}
 	
 	// Check that minimum information (time, position, velocity) is valid
-	if( (trajData->validFields & 0x07) != 0x07 )
+	validationFlags = Valid_Time | Valid_Position | Valid_Velocity;
+	if( (trajData->validFields & validationFlags) != validationFlags)
 	{
 		printf("ERROR: Validfields = %d\r\n", trajData->validFields);
 		Ros_SimpleMsg_MotionReply(receiveMsg, ROS_RESULT_INVALID, ROS_RESULT_INVALID_DATA_INSUFFICIENT, replyMsg, receiveMsg->body.jointTrajData.groupNo);
@@ -1141,6 +1137,15 @@ int Ros_MotionServer_InitTrajPointFull(CtrlGroup* ctrlGroup, SmBodyJointTrajPtFu
 		Ros_MotionServer_ConvertToJointMotionData(jointTrajData, &ctrlGroup->jointMotionData);
 		ctrlGroup->timeLeftover_ms = 0;
 		ctrlGroup->q_time = ctrlGroup->jointMotionData.time;
+		
+		// For MPL80/100 robot type (SLUBT): Controller automatically moves the B-axis
+		// to maintain orientation as other axes are moved.
+		if (ctrlGroup->bIsBaxisSlave)
+		{
+			//ROS joint order
+			ctrlGroup->jointMotionData.pos[3] += -ctrlGroup->jointMotionData.pos[1] + ctrlGroup->jointMotionData.pos[2];
+			ctrlGroup->jointMotionData.vel[3] += -ctrlGroup->jointMotionData.vel[1] + ctrlGroup->jointMotionData.vel[2];
+		}
 
 		// Convert start position to pulse format
 		Ros_CtrlGroup_ConvertToMotoPos(ctrlGroup, ctrlGroup->jointMotionData.pos, pulsePos);
@@ -1155,15 +1160,19 @@ int Ros_MotionServer_InitTrajPointFull(CtrlGroup* ctrlGroup, SmBodyJointTrajPtFu
 			// Check if position matches current command position
 			if(abs(pulsePos[i] - curPos[i]) > START_MAX_PULSE_DEVIATION)
 			{
-				printf("ERROR: Trajectory start position doesn't match current position.\r\n");
-				printf("    %ld, %ld, %ld, %ld, %ld, %ld, %ld, %ld\r\n",
+				printf("ERROR: Trajectory start position doesn't match current position (MOTO joint order).\r\n");
+				printf(" - Requested start: %ld, %ld, %ld, %ld, %ld, %ld, %ld, %ld\r\n",
 					pulsePos[0], pulsePos[1], pulsePos[2],
 					pulsePos[3], pulsePos[4], pulsePos[5],
 					pulsePos[6], pulsePos[7]);
-				printf("    %ld, %ld, %ld, %ld, %ld, %ld, %ld, %ld\r\n",
+				printf(" - Current pos: %ld, %ld, %ld, %ld, %ld, %ld, %ld, %ld\r\n",
 					curPos[0], curPos[1], curPos[2],
 					curPos[3], curPos[4], curPos[5],
 					curPos[6], curPos[7]);
+				printf(" - ctrlGroup->prevPulsePos: %ld, %ld, %ld, %ld, %ld, %ld, %ld, %ld\r\n",
+					ctrlGroup->prevPulsePos[0], ctrlGroup->prevPulsePos[1], ctrlGroup->prevPulsePos[2],
+					ctrlGroup->prevPulsePos[3], ctrlGroup->prevPulsePos[4], ctrlGroup->prevPulsePos[5],
+					ctrlGroup->prevPulsePos[6], ctrlGroup->prevPulsePos[7]);
 
 				return ROS_RESULT_INVALID_DATA_START_POS;
 			}
@@ -1174,15 +1183,7 @@ int Ros_MotionServer_InitTrajPointFull(CtrlGroup* ctrlGroup, SmBodyJointTrajPtFu
 				// excessive speed
 				return ROS_RESULT_INVALID_DATA_SPEED;
 			}
-		}
-		
-		// For MPL80/100 robot type (SLUBT): Controller automatically moves the B-axis
-		// to maintain orientation as other axes are moved.
-		if (ctrlGroup->bIsBaxisSlave)
-		{
-			ctrlGroup->jointMotionData.pos[3] += -ctrlGroup->jointMotionData.pos[1] + ctrlGroup->jointMotionData.pos[2];
-			ctrlGroup->jointMotionData.vel[3] += -ctrlGroup->jointMotionData.vel[1] + ctrlGroup->jointMotionData.vel[2];
-		}
+		}		
 
 		//printf("Trajectory Start Initialized\r\n");
 		// Return success
@@ -1327,6 +1328,7 @@ void Ros_MotionServer_JointTrajDataToIncQueue(Controller* controller, int groupN
 	// to maintain orientation as other axes are moved.
 	if (ctrlGroup->bIsBaxisSlave)
 	{
+		//ROS joint order
 		endTrajData->pos[3] += -endTrajData->pos[1] + endTrajData->pos[2];
 		endTrajData->vel[3] += -endTrajData->vel[1] + endTrajData->vel[2];
 	}
diff --git a/motoman_driver/MotoPlus/MotoROS.h b/motoman_driver/MotoPlus/MotoROS.h
index 6a330c6f..55bb2ac8 100644
--- a/motoman_driver/MotoPlus/MotoROS.h
+++ b/motoman_driver/MotoPlus/MotoROS.h
@@ -32,7 +32,7 @@
 #ifndef MOTOROS_H
 #define MOTOROS_H
 
-#define APPLICATION_VERSION					"1.6.0"
+#define APPLICATION_VERSION					"1.7.0"
 
 #include "MotoPlus.h"
 #include "ParameterExtraction.h"
diff --git a/motoman_driver/MotoPlus/MpRosAllControllers.VC.db b/motoman_driver/MotoPlus/MpRosAllControllers.VC.db
index 27683f1e..df25080a 100644
Binary files a/motoman_driver/MotoPlus/MpRosAllControllers.VC.db and b/motoman_driver/MotoPlus/MpRosAllControllers.VC.db differ
diff --git a/motoman_driver/MotoPlus/ParameterExtraction.dnLib b/motoman_driver/MotoPlus/ParameterExtraction.dnLib
index d19d9e17..b3fe98b4 100644
Binary files a/motoman_driver/MotoPlus/ParameterExtraction.dnLib and b/motoman_driver/MotoPlus/ParameterExtraction.dnLib differ
diff --git a/motoman_driver/MotoPlus/ParameterExtraction.fsLib b/motoman_driver/MotoPlus/ParameterExtraction.fsLib
index c3511611..32d3623c 100644
Binary files a/motoman_driver/MotoPlus/ParameterExtraction.fsLib and b/motoman_driver/MotoPlus/ParameterExtraction.fsLib differ
diff --git a/motoman_driver/MotoPlus/ParameterExtraction.h b/motoman_driver/MotoPlus/ParameterExtraction.h
index 7bf33571..1c21e738 100644
--- a/motoman_driver/MotoPlus/ParameterExtraction.h
+++ b/motoman_driver/MotoPlus/ParameterExtraction.h
@@ -178,6 +178,24 @@ extern STATUS 	GP_getPulseToMeter(int ctrlGrp, PULSE_TO_METER* PulseToMeter);
 /******************************************************************************/
 extern STATUS 	GP_isBaxisSlave(int ctrlGrp, BOOL* bBaxisIsSlave);
 
+/******************************************************************************/
+/* << 21 >>                                                              	  */
+/* Function name : STATUS GP_getFeedbackSpeedMRegisterAddresses()			  */
+/* Functionality : Obtains the MRegister CIO addresses that contain the 	  */
+/*				   feedback speed for each axis. Optionally enables this      */
+/*				   feature if not already enabled.						      */
+/* Parameter	 : int ctrlGrp - Robot control group (zero based index) [IN]  */
+/*				   BOOL bActivateIfNotEnabled - TRUE to enable feature [IN]   */
+/*				   BOOL bForceRebootAfterActivation - TRUE to force the user  */
+/*				   to reboot if this feature gets activated. Set to FALSE if  */
+/*				   you plan to enable for additional control groups. [IN]     */
+/*				   JOINT_FEEDBACK_SPEED_ADDRESSES* registerAddresses -		  */
+/*				   Obtains the CIO register address for the feedback data [OUT]*/
+/* Return value	 : Success = OK 											  */
+/*				 : Failure = NG												  */
+/******************************************************************************/
+extern STATUS	GP_getFeedbackSpeedMRegisterAddresses(int ctrlGrp, BOOL bActivateIfNotEnabled, BOOL bForceRebootAfterActivation, JOINT_FEEDBACK_SPEED_ADDRESSES* registerAddresses);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/motoman_driver/MotoPlus/ParameterExtraction.mpLib b/motoman_driver/MotoPlus/ParameterExtraction.mpLib
index c6ff1240..61e0ee47 100644
Binary files a/motoman_driver/MotoPlus/ParameterExtraction.mpLib and b/motoman_driver/MotoPlus/ParameterExtraction.mpLib differ
diff --git a/motoman_driver/MotoPlus/ParameterExtraction.yrcLib b/motoman_driver/MotoPlus/ParameterExtraction.yrcLib
index 88744fd7..8c981c5a 100644
Binary files a/motoman_driver/MotoPlus/ParameterExtraction.yrcLib and b/motoman_driver/MotoPlus/ParameterExtraction.yrcLib differ
diff --git a/motoman_driver/MotoPlus/ParameterTypes.h b/motoman_driver/MotoPlus/ParameterTypes.h
index 41316d48..433adf35 100644
--- a/motoman_driver/MotoPlus/ParameterTypes.h
+++ b/motoman_driver/MotoPlus/ParameterTypes.h
@@ -106,6 +106,12 @@ typedef struct
 	INT32 maxLimit[MAX_PULSE_AXES];
 } JOINT_ANGULAR_VELOCITY_LIMITS;
 
+typedef struct
+{
+	BOOL bFeedbackSpeedEnabled;
+	ULONG cioAddressForAxis[MAX_PULSE_AXES][2];
+} JOINT_FEEDBACK_SPEED_ADDRESSES;
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/motoman_driver/MotoPlus/RosSetupValidation.dnLib b/motoman_driver/MotoPlus/RosSetupValidation.dnLib
index fd0ff8bf..a9e79c45 100644
Binary files a/motoman_driver/MotoPlus/RosSetupValidation.dnLib and b/motoman_driver/MotoPlus/RosSetupValidation.dnLib differ
diff --git a/motoman_driver/MotoPlus/RosSetupValidation.fsLib b/motoman_driver/MotoPlus/RosSetupValidation.fsLib
index bed0c913..82e9066e 100644
Binary files a/motoman_driver/MotoPlus/RosSetupValidation.fsLib and b/motoman_driver/MotoPlus/RosSetupValidation.fsLib differ
diff --git a/motoman_driver/MotoPlus/RosSetupValidation.mpLib b/motoman_driver/MotoPlus/RosSetupValidation.mpLib
index e9507312..14b2ed1a 100644
Binary files a/motoman_driver/MotoPlus/RosSetupValidation.mpLib and b/motoman_driver/MotoPlus/RosSetupValidation.mpLib differ
diff --git a/motoman_driver/MotoPlus/RosSetupValidation.yrcLib b/motoman_driver/MotoPlus/RosSetupValidation.yrcLib
index 48daeb3e..01e09e78 100644
Binary files a/motoman_driver/MotoPlus/RosSetupValidation.yrcLib and b/motoman_driver/MotoPlus/RosSetupValidation.yrcLib differ
diff --git a/motoman_driver/MotoPlus/SimpleMessage.c b/motoman_driver/MotoPlus/SimpleMessage.c
index e654dd6e..8ca11d88 100644
--- a/motoman_driver/MotoPlus/SimpleMessage.c
+++ b/motoman_driver/MotoPlus/SimpleMessage.c
@@ -52,6 +52,7 @@ int Ros_SimpleMsg_JointFeedback(CtrlGroup* ctrlGroup, SimpleMsg* sendMsg)
 {
 	int bRet;
 	long pulsePos[MAX_PULSE_AXES];
+	long pulseSpeed[MAX_PULSE_AXES];
 	
 	//initialize memory
 	memset(sendMsg, 0x00, sizeof(SimpleMsg));
@@ -66,21 +67,21 @@ int Ros_SimpleMsg_JointFeedback(CtrlGroup* ctrlGroup, SimpleMsg* sendMsg)
 	
 	// set body
 	sendMsg->body.jointFeedback.groupNo = ctrlGroup->groupNo;
-	sendMsg->body.jointFeedback.validFields = 2;
+	sendMsg->body.jointFeedback.validFields = Valid_Position;
 	
+	//feedback position
 	bRet = Ros_CtrlGroup_GetFBPulsePos(ctrlGroup, pulsePos);
 	if(bRet!=TRUE)
-		return 0;
-				
+		return 0;				
 	Ros_CtrlGroup_ConvertToRosPos(ctrlGroup, pulsePos, sendMsg->body.jointFeedback.pos);
 
-	// For testing
-	//bRet = Ros_CtrlGroup_GetPulsePosCmd(ctrlGroup, pulsePos);
-	//if(bRet!=TRUE)
-	//	return 0;
-	//	
-	//Ros_CtrlGroup_ConvertToRosPos(ctrlGroup, pulsePos, sendMsg->body.jointFeedback.vel);
-	// End testing
+	//servo speed
+	bRet = Ros_CtrlGroup_GetFBServoSpeed(ctrlGroup, pulseSpeed);
+	if (bRet == TRUE)
+	{
+		Ros_CtrlGroup_ConvertToRosPos(ctrlGroup, pulseSpeed, sendMsg->body.jointFeedback.vel);
+		sendMsg->body.jointFeedback.validFields |= Valid_Velocity;
+	}
 	
 	return(sendMsg->prefix.length + sizeof(SmPrefix));
 }
diff --git a/motoman_driver/MotoPlus/SimpleMessage.h b/motoman_driver/MotoPlus/SimpleMessage.h
index d7480ab1..79e2bfd5 100644
--- a/motoman_driver/MotoPlus/SimpleMessage.h
+++ b/motoman_driver/MotoPlus/SimpleMessage.h
@@ -161,6 +161,14 @@ struct _SmHeader
 } __attribute__((__packed__));
 typedef struct _SmHeader SmHeader;
 
+typedef enum
+{
+	Valid_Time = 1,
+	Valid_Position = 2,
+	Valid_Velocity = 4,
+	Valid_Acceleration = 8
+} FlagsValidFields;
+
 //--------------
 // Body Section
 //--------------
@@ -181,7 +189,7 @@ struct _SmBodyJointTrajPtFull	// ROS_MSG_JOINT_TRAJ_PT_FULL = 14
 {
 	int groupNo;  				// Robot/group ID;  0 = 1st robot 
 	int sequence;				// Index of point in trajectory; 0 = Initial trajectory point, which should match the robot current position.
-	int validFields;			// Bit-mask indicating which “optional” fields are filled with data. 1=time, 2=position, 4=velocity, 8=acceleration
+	FlagsValidFields validFields;	// Bit-mask indicating which “optional” fields are filled with data. 1=time, 2=position, 4=velocity, 8=acceleration
 	float time;					// Timestamp associated with this trajectory point; Units: in seconds 
 	float pos[ROS_MAX_JOINT];	// Desired joint positions in radian.  Base to Tool joint order  
 	float vel[ROS_MAX_JOINT];	// Desired joint velocities in radian/sec.  
@@ -192,11 +200,11 @@ typedef struct _SmBodyJointTrajPtFull SmBodyJointTrajPtFull;
 struct _SmBodyJointFeedback		// ROS_MSG_JOINT_FEEDBACK = 15
 {
 	int groupNo;  				// Robot/group ID;  0 = 1st robot 
-	int validFields;			// Bit-mask indicating which “optional” fields are filled with data. 1=time, 2=position, 4=velocity, 8=acceleration
+	FlagsValidFields validFields;	// Bit-mask indicating which “optional” fields are filled with data. 1=time, 2=position, 4=velocity, 8=acceleration
 	float time;					// Timestamp associated with this trajectory point; Units: in seconds 
-	float pos[ROS_MAX_JOINT];	// Desired joint positions in radian.  Base to Tool joint order  
-	float vel[ROS_MAX_JOINT];	// Desired joint velocities in radian/sec.  
-	float acc[ROS_MAX_JOINT];	// Desired joint accelerations in radian/sec^2.
+	float pos[ROS_MAX_JOINT];	// Feedback joint positions in radian.  Base to Tool joint order  
+	float vel[ROS_MAX_JOINT];	// Feedback joint velocities in radian/sec.  
+	float acc[ROS_MAX_JOINT];	// Feedback joint accelerations in radian/sec^2.
 } __attribute__((__packed__));
 typedef struct _SmBodyJointFeedback SmBodyJointFeedback;
 
@@ -225,7 +233,7 @@ typedef struct _SmBodyMotoMotionReply SmBodyMotoMotionReply;
 struct _SmBodyJointTrajPtExData
 {
 	int groupNo;  				// Robot/group ID;  0 = 1st robot 
-	int validFields;			// Bit-mask indicating which “optional” fields are filled with data. 1=time, 2=position, 4=velocity, 8=acceleration
+	FlagsValidFields validFields;	// Bit-mask indicating which “optional” fields are filled with data. 1=time, 2=position, 4=velocity, 8=acceleration
 	float time;					// Timestamp associated with this trajectory point; Units: in seconds 
 	float pos[ROS_MAX_JOINT];	// Desired joint positions in radian.  Base to Tool joint order  
 	float vel[ROS_MAX_JOINT];	// Desired joint velocities in radian/sec.  
diff --git a/motoman_driver/MotoPlus/_buildLog/MotoROSDX100.log b/motoman_driver/MotoPlus/_buildLog/MotoROSDX100.log
index af20dc4a..7f7cb2da 100644
--- a/motoman_driver/MotoPlus/_buildLog/MotoROSDX100.log
+++ b/motoman_driver/MotoPlus/_buildLog/MotoROSDX100.log
@@ -1,10 +1,10 @@
   ------ Now building project: MotoRosDX100------
   Compiling Controller.c
 Controller.c : warning : In function `Db_Print'
-Controller.c(958): warning : unused parameter `msgFormat'
+Controller.c(960): warning : unused parameter `msgFormat'
   Compiling CtrlGroup.c
 CtrlGroup.c : warning : In function `Ros_CtrlGroup_IsRobot'
-CtrlGroup.c(516): warning : comparison of unsigned expression >= 0 is always true
+CtrlGroup.c(613): warning : comparison of unsigned expression >= 0 is always true
   Compiling IoServer.c
   Compiling MotionServer.c
   Compiling mpMain.c
diff --git a/motoman_driver/MotoPlus/output/DX100/MotoRosDX100/MotoRosDX100.out b/motoman_driver/MotoPlus/output/DX100/MotoRosDX100/MotoRosDX100.out
index 5a71abee..d9f77921 100644
Binary files a/motoman_driver/MotoPlus/output/DX100/MotoRosDX100/MotoRosDX100.out and b/motoman_driver/MotoPlus/output/DX100/MotoRosDX100/MotoRosDX100.out differ
diff --git a/motoman_driver/MotoPlus/output/DX200/MotoRosDX200/MotoRosDX200.out b/motoman_driver/MotoPlus/output/DX200/MotoRosDX200/MotoRosDX200.out
index 597bf9f6..dd0d1876 100644
Binary files a/motoman_driver/MotoPlus/output/DX200/MotoRosDX200/MotoRosDX200.out and b/motoman_driver/MotoPlus/output/DX200/MotoRosDX200/MotoRosDX200.out differ
diff --git a/motoman_driver/MotoPlus/output/FS100/MotoRosFS100/MotoRosFS100.out b/motoman_driver/MotoPlus/output/FS100/MotoRosFS100/MotoRosFS100.out
index 930bbb39..27c94afb 100644
Binary files a/motoman_driver/MotoPlus/output/FS100/MotoRosFS100/MotoRosFS100.out and b/motoman_driver/MotoPlus/output/FS100/MotoRosFS100/MotoRosFS100.out differ
diff --git a/motoman_driver/MotoPlus/output/YRC1000/MotoRosYRC1000/MotoRosYRC1000.out b/motoman_driver/MotoPlus/output/YRC1000/MotoRosYRC1000/MotoRosYRC1000.out
index 21406c26..5507592d 100644
Binary files a/motoman_driver/MotoPlus/output/YRC1000/MotoRosYRC1000/MotoRosYRC1000.out and b/motoman_driver/MotoPlus/output/YRC1000/MotoRosYRC1000/MotoRosYRC1000.out differ