00001
00002
00003
00004
00005 #include <robotican_hardware_interface/RiCMotor.h>
00006 #include <robotican_hardware_interface/RiCBoardPotentiometerConfig.h>
00007 #include <robotican_hardware_interface/RiCBoardConfig.h>
00008
00009 namespace robotican_hardware {
00010
00011 void RiCMotor::deviceAck(const DeviceAck *ack) {
00012 Device::deviceAck(ack);
00013 if(isReady()) {
00014 ros_utils::rosInfo("Motor is ready");
00015 }
00016 else {
00017 ros_utils::rosError("RiCBoard can't build motor object for some reason, this program will shut down now");
00018 ros::shutdown();
00019 }
00020 }
00021
00022 RiCMotor::RiCMotor(byte id, TransportLayer *transportLayer, byte motorAddress, byte eSwitchPin, byte eSwitchType)
00023 : Device(id, transportLayer) {
00024 _motorAddress = motorAddress;
00025 _eSwitchPin = eSwitchPin;
00026 _eSwitchType = eSwitchType;
00027 }
00028
00029
00030
00031 CloseLoopMotor::CloseLoopMotor(byte id, TransportLayer *transportLayer, byte motorAddress, byte eSwitchPin, byte eSwitchType,
00032 CloseMotorType::CloseMotorType motorType, CloseMotorMode::CloseMotorMode mode)
00033 : RiCMotor(id, transportLayer, motorAddress, eSwitchPin, eSwitchType) {
00034 _motorType = motorType;
00035 _mode = mode;
00036 _lastCmd = 0.0;
00037
00038 }
00039
00040 JointInfo_t *CloseLoopMotor::getJointInfo() {
00041 return &_jointInfo;
00042 }
00043
00044 void CloseLoopMotor::update(const DeviceMessage *deviceMessage) {
00045 if(isReady()) {
00046
00047 MotorFeedback *feedback = (MotorFeedback *) deviceMessage;
00048 _jointInfo.position = feedback->rad;
00049 _jointInfo.velocity = feedback->rad_s;
00050 }
00051 }
00052
00053 void CloseLoopMotor::write() {
00054 if(isReady()) {
00055 if(checkIfLastCmdChange()) {
00056 MotorSetPoint point;
00057 point.length = sizeof(point);
00058 point.checkSum = 0;
00059 point.id = getId();
00060 point.point = (float) _jointInfo.cmd;
00061
00062 uint8_t *rawData = (uint8_t *) &point;
00063 point.checkSum = _transportLayer->calcChecksum(rawData, point.length);
00064 _transportLayer->write(rawData, point.length);
00065 _lastCmd = (float) _jointInfo.cmd;
00066 }
00067 }
00068 }
00069
00070 bool CloseLoopMotor::checkIfLastCmdChange() {
00071 float delta = fabsf((float) (_jointInfo.cmd - _lastCmd));
00072 return delta >= MOTOR_EPSILON;
00073 }
00074
00075 CloseMotorType::CloseMotorType CloseLoopMotor::getCloseMotorType() {
00076 return _motorType;
00077 }
00078
00079 CloseMotorMode::CloseMotorMode CloseLoopMotor::getMode() {
00080 return _mode;
00081 }
00082
00083 byte RiCMotor::getESwitchPin() {
00084 return _eSwitchPin;
00085 }
00086
00087 byte RiCMotor::getESwitchType() {
00088 return _eSwitchType;
00089 }
00090
00091 byte RiCMotor::getAddress() {
00092 return _motorAddress;
00093 }
00094
00095 OpenLoopMotor::OpenLoopMotor(byte id, TransportLayer *transportLayer, byte motorAddress, byte eSwitchPin, byte eSwitchType,
00096 float maxSetPointSpeed, float minSetPointSpeed, byte encoderA, byte encoderB, int8_t motorDirection,
00097 int8_t encoderDirection, uint16_t LPFHzSpeed, uint16_t LPFHzInput, float LPFAlphaInput,
00098 float LPFAlphaSpeed, uint16_t PPR)
00099 : RiCMotor(id, transportLayer, motorAddress, eSwitchPin, eSwitchType) {
00100
00101 _maxSetPointSpeed = maxSetPointSpeed;
00102 _minSetPointSpeed = minSetPointSpeed;
00103 _encoderPinA = encoderA;
00104 _encoderPinB = encoderB;
00105 _motorDirection = motorDirection;
00106 _encoderDirection = encoderDirection;
00107 _LPFHzSpeed = LPFHzSpeed;
00108 _LPFHzInput = LPFHzInput;
00109 _LPFAlphaInput = LPFAlphaInput;
00110 _PPR = PPR;
00111 _LPFAlphaSpeed = LPFAlphaSpeed;
00112 }
00113
00114 void OpenLoopMotor::update(const DeviceMessage *deviceMessage) {
00115
00116 MotorFeedback *feedback = (MotorFeedback *) deviceMessage;
00117 _jointInfo.position = feedback->rad;
00118 _jointInfo.velocity = feedback->rad_s;
00119
00120 }
00121
00122 void OpenLoopMotor::write() {
00123 if(isReady()) {
00124 MotorSetPoint motorSetPoint;
00125 motorSetPoint.length = sizeof(motorSetPoint);
00126 motorSetPoint.checkSum = 0;
00127 motorSetPoint.id = getId();
00128 motorSetPoint.point = (float) _jointInfo.cmd;
00129
00130
00131 uint8_t *rawData = (uint8_t *) &motorSetPoint;
00132 motorSetPoint.checkSum = _transportLayer->calcChecksum(rawData, motorSetPoint.length);
00133 _transportLayer->write(rawData, motorSetPoint.length);
00134 }
00135 }
00136
00137
00138 void OpenLoopMotor::buildDevice() {
00139 BuildMotorOpenLoop buildMotorOpenLoop;
00140 buildMotorOpenLoop.length = sizeof(buildMotorOpenLoop);
00141 buildMotorOpenLoop.checkSum = 0;
00142 buildMotorOpenLoop.id = getId();
00143 buildMotorOpenLoop.motorAddress = getAddress();
00144 buildMotorOpenLoop.motorDirection = _motorDirection;
00145 buildMotorOpenLoop.eSwitchPin = getESwitchPin();
00146 buildMotorOpenLoop.eSwitchType = getESwitchType();
00147 buildMotorOpenLoop.maxSetPointSpeed = _maxSetPointSpeed;
00148 buildMotorOpenLoop.minSetPointSpeed = _minSetPointSpeed;
00149 buildMotorOpenLoop.encoderPinA = _encoderPinA;
00150 buildMotorOpenLoop.encoderPinB = _encoderPinB;
00151 buildMotorOpenLoop.encoderDirection = _encoderDirection;
00152 buildMotorOpenLoop.PPR = _PPR;
00153 buildMotorOpenLoop.LPFAlphaSpeed = _LPFAlphaSpeed;
00154 buildMotorOpenLoop.LPFHzSpeed = _LPFHzSpeed;
00155 buildMotorOpenLoop.LPFHzInput = _LPFHzInput;
00156 buildMotorOpenLoop.LPFAlphaInput = _LPFAlphaInput;
00157
00158
00159 uint8_t* rawData = (uint8_t*) &buildMotorOpenLoop;
00160 buildMotorOpenLoop.checkSum = _transportLayer->calcChecksum(rawData, buildMotorOpenLoop.length);
00161 _transportLayer->write(rawData, buildMotorOpenLoop.length);
00162 }
00163
00164 JointInfo_t *OpenLoopMotor::getJointInfo() {
00165 return &_jointInfo;
00166 }
00167
00168 CloseLoopMotorWithEncoder::CloseLoopMotorWithEncoder(byte id, TransportLayer *transportLayer, byte motorAddress, byte eSwitchPin,
00169 byte eSwitchType, CloseMotorType::CloseMotorType motoryType,
00170 CloseMotorMode::CloseMotorMode mode, CloseMotorWithEncoderParam param,
00171 std::string jointName)
00172 : CloseLoopMotor(id, transportLayer, motorAddress, eSwitchPin, eSwitchType, motoryType, mode),
00173 _mutex() ,_nodeHandle((std::string("~/") + jointName)), _server(_mutex , _nodeHandle) , _spinner(1){
00174 _callbackType = boost::bind(&CloseLoopMotorWithEncoder::dynamicCallback, this, _1, _2);
00175
00176 _server.setCallback(_callbackType);
00177 _spinner.start();
00178 _timer = _nodeHandle.createTimer(ros::Duration(0.02), &CloseLoopMotorWithEncoder::timerCallback, this);
00179 _timer.start();
00180 _statusPub = _nodeHandle.advertise<std_msgs::Float64>("motor_current_set_point", 10);
00181 robotican_hardware_interface::RiCBoardConfig config;
00182 config.motor_speed_lpf_hz = param.LPFHzSpeed;
00183 config.motor_speed_lpf_alpha = param.LPFAlphaSpeed;
00184 config.motor_input_lpf_hz = param.LPFHzInput;
00185 config.motor_input_lpf_alpha = param.LPFAlphaInput;
00186 config.motor_pid_hz = param.PIDHz;
00187 config.motor_kp = param.KP;
00188 config.motor_ki = param.KI;
00189 config.motor_kd = param.KD;
00190 config.motor_kff = param.KFF;
00191 config.motor_limit = param.limit;
00192
00193 _server.updateConfig(config);
00194
00195 _params = param;
00196 _isSetParam = false;
00197
00198
00199 }
00200
00201
00202 void CloseLoopMotorWithEncoder::buildDevice() {
00203 BuildMotorCloseLoopWithEncoder buildMotorCloseLoop;
00204 buildMotorCloseLoop.length = sizeof(buildMotorCloseLoop);
00205 buildMotorCloseLoop.checkSum = 0;
00206 buildMotorCloseLoop.id = getId();
00207 buildMotorCloseLoop.motorAddress = getAddress();
00208 buildMotorCloseLoop.eSwitchPin = getESwitchPin();
00209 buildMotorCloseLoop.eSwitchType = getESwitchType();
00210 buildMotorCloseLoop.encoderPinA = _params.encoderPinA;
00211 buildMotorCloseLoop.encoderPinB = _params.encoderPinB;
00212 buildMotorCloseLoop.LPFHzSpeed = _params.LPFHzSpeed;
00213 buildMotorCloseLoop.LPFHzInput = _params.LPFHzInput;
00214 buildMotorCloseLoop.PIDHz = _params.PIDHz;
00215 buildMotorCloseLoop.PPR = _params.PPR;
00216 buildMotorCloseLoop.timeout = _params.timeout;
00217 buildMotorCloseLoop.motorDirection = _params.motorDirection;
00218 buildMotorCloseLoop.encoderDirection = _params.encoderDirection;
00219 buildMotorCloseLoop.stopLimitMax = _params.stopLimitMax;
00220 buildMotorCloseLoop.stopLimitMin = _params.stopLimitMin;
00221
00222 buildMotorCloseLoop.LPFAlphaSpeed = _params.LPFAlphaSpeed;
00223 buildMotorCloseLoop.LPFAlphaInput = _params.LPFAlphaInput;
00224 buildMotorCloseLoop.KP = _params.KP;
00225 buildMotorCloseLoop.KI = _params.KI;
00226 buildMotorCloseLoop.KD = _params.KD;
00227 buildMotorCloseLoop.KFF = _params.KFF;
00228 buildMotorCloseLoop.maxSetPointSpeed = _params.maxSetPointSpeed;
00229 buildMotorCloseLoop.minSetPointSpeed = _params.minSetPointSpeed;
00230 buildMotorCloseLoop.maxSetPointPos = _params.maxSetPointPos;
00231 buildMotorCloseLoop.minSetPointPos = _params.minSetPointPos;
00232 buildMotorCloseLoop.limit = _params.limit;
00233 buildMotorCloseLoop.motorType = getCloseMotorType();
00234 buildMotorCloseLoop.motorMode = getMode();
00235 buildMotorCloseLoop.baisMin = _params.baisMin;
00236 buildMotorCloseLoop.baisMax = _params.baisMax;
00237
00238 uint8_t* rawData = (uint8_t*) &buildMotorCloseLoop;
00239 buildMotorCloseLoop.checkSum = _transportLayer->calcChecksum(rawData, buildMotorCloseLoop.length);
00240 _transportLayer->write(rawData, buildMotorCloseLoop.length);
00241 }
00242
00243 void CloseLoopMotorWithEncoder::setParams(uint16_t speedLpfHz, uint16_t inputLpfHz, uint16_t pidHz, float speedLpfAlpha,
00244 float inputLpfAlpha, float KP, float KI, float KD, float KFF, float limit) {
00245 _isSetParam = true;
00246 _params.LPFHzSpeed = speedLpfHz;
00247 _params.PIDHz = pidHz;
00248 _params.LPFAlphaSpeed = speedLpfAlpha;
00249 _params.LPFHzInput = inputLpfHz;
00250 _params.LPFAlphaInput = inputLpfAlpha;
00251 _params.KP = KP;
00252 _params.KI = KI;
00253 _params.KD = KD;
00254 _params.KFF = KFF;
00255 _params.limit = limit;
00256
00257 }
00258
00259 void CloseLoopMotorWithEncoder::write() {
00260 CloseLoopMotor::write();
00261 if(isReady()) {
00262 if (_isSetParam) {
00263 _isSetParam = false;
00264 SetMotorParam param;
00265 param.length = sizeof(param);
00266 param.checkSum = 0;
00267 param.id = getId();
00268 param.speedLpfHz = _params.LPFHzSpeed;
00269 param.pidHz = _params.PIDHz;
00270 param.speedLfpAlpha = _params.LPFAlphaSpeed;
00271 param.KP = _params.KP;
00272 param.KI = _params.KI;
00273 param.KD = _params.KD;
00274 param.KFF = _params.KFF;
00275 param.limit = _params.limit;
00276 param.inputLfpAlpha = 0;
00277 param.inputLpfHz = _params.LPFHzInput;
00278 param.inputLfpAlpha = _params.LPFAlphaInput;
00279
00280 uint8_t *rawData = (uint8_t *) ¶m;
00281
00282 param.checkSum = _transportLayer->calcChecksum(rawData, param.length);
00283 _transportLayer->write(rawData, param.length);
00284 }
00285 }
00286 }
00287
00288 void CloseLoopMotorWithEncoder::dynamicCallback(robotican_hardware_interface::RiCBoardConfig &config, uint32_t level) {
00289 setParams((uint16_t) config.motor_speed_lpf_hz, (uint16_t) config.motor_input_lpf_hz,
00290 (uint16_t) config.motor_pid_hz, (float) config.motor_speed_lpf_alpha,
00291 (float) config.motor_input_lpf_alpha, (float) config.motor_kp, (float) config.motor_ki,
00292 (float) config.motor_kd, (float) config.motor_kff, (float) config.motor_limit);
00293 }
00294
00295 void CloseLoopMotorWithEncoder::timerCallback(const ros::TimerEvent &e) {
00296 std_msgs::Float64 msg;
00297 msg.data = _jointInfo.cmd;
00298
00299 _statusPub.publish(msg);
00300 }
00301
00302 void CloseLoopMotorWithPotentiometer::setParams(uint16_t speedLpfHz, uint16_t inputLpfHz, uint16_t pidHz, float speedLpfAlpha,
00303 float inputLpfAlpha, float KP, float KI, float KD, float KFF, float limit) {
00304
00305 }
00306
00307 void CloseLoopMotorWithPotentiometer::buildDevice() {
00308 BuildCloseLoopWithPotentiometer buildCloseLoopWithPotentiometer;
00309 buildCloseLoopWithPotentiometer.length = sizeof(buildCloseLoopWithPotentiometer);
00310 buildCloseLoopWithPotentiometer.checkSum = 0;
00311 buildCloseLoopWithPotentiometer.id = getId();
00312
00313 buildCloseLoopWithPotentiometer.motorAddress = getAddress();
00314 buildCloseLoopWithPotentiometer.motorMode = getMode();
00315 buildCloseLoopWithPotentiometer.eSwitchPin = getESwitchPin();
00316 buildCloseLoopWithPotentiometer.eSwitchType = getESwitchType();
00317 buildCloseLoopWithPotentiometer.motorType = getCloseMotorType();
00318 buildCloseLoopWithPotentiometer.LPFHzSpeed = _param.LPFHzSpeed;
00319 buildCloseLoopWithPotentiometer.PIDHz = _param.PIDHz;
00320 buildCloseLoopWithPotentiometer.PPR = _param.PPR;
00321 buildCloseLoopWithPotentiometer.timeout = _param.timeout;
00322 buildCloseLoopWithPotentiometer.motorDirection = _param.motorDirection;
00323 buildCloseLoopWithPotentiometer.encoderDirection = _param.encoderDirection;
00324 buildCloseLoopWithPotentiometer.LPFAlphaSpeed = _param.LPFAlphaSpeed;
00325 buildCloseLoopWithPotentiometer.KP = _param.KP;
00326 buildCloseLoopWithPotentiometer.KI = _param.KI;
00327 buildCloseLoopWithPotentiometer.KD = _param.KD;
00328 buildCloseLoopWithPotentiometer.KFF = _param.KFF;
00329 buildCloseLoopWithPotentiometer.maxSetPointSpeed = _param.maxSetPointSpeed;
00330 buildCloseLoopWithPotentiometer.minSetPointSpeed = _param.minSetPointSpeed;
00331 buildCloseLoopWithPotentiometer.maxSetPointPos = _param.maxSetPointPos;
00332 buildCloseLoopWithPotentiometer.minSetPointPos = _param.minSetPointPos;
00333 buildCloseLoopWithPotentiometer.limit = _param.limit;
00334 buildCloseLoopWithPotentiometer.a = _param.a;
00335 buildCloseLoopWithPotentiometer.b = _param.b;
00336 buildCloseLoopWithPotentiometer.pin = _param.pin;
00337 buildCloseLoopWithPotentiometer.tolerance = _param.tolerance;
00338 buildCloseLoopWithPotentiometer.stopLimitMax = _param.stopLimitMax;
00339 buildCloseLoopWithPotentiometer.stopLimitMin = _param.stopLimitMin;
00340 buildCloseLoopWithPotentiometer.LPFHzInput = _param.LPFHzInput;
00341 buildCloseLoopWithPotentiometer.LPFAlphaInput = _param.LPFAlphaInput;
00342 buildCloseLoopWithPotentiometer.k = _param.K;
00343 buildCloseLoopWithPotentiometer.toleranceTime = _param.toleranceTime;
00344
00345 uint8_t* rawData = (uint8_t*)&buildCloseLoopWithPotentiometer;
00346 buildCloseLoopWithPotentiometer.checkSum = _transportLayer->calcChecksum(rawData, buildCloseLoopWithPotentiometer.length);
00347 _transportLayer->write(rawData, buildCloseLoopWithPotentiometer.length);
00348
00349 }
00350
00351 CloseLoopMotorWithPotentiometer::CloseLoopMotorWithPotentiometer(byte id, TransportLayer *transportLayer,
00352 byte motorAddress, byte eSwitchPin,
00353 byte eSwitchType,
00354 CloseMotorType::CloseMotorType motorType,
00355 CloseMotorMode::CloseMotorMode mode,
00356 CloseMotorWithPotentiometerParam motorParam,
00357 std::string jointName)
00358 : CloseLoopMotor(id, transportLayer, motorAddress, eSwitchPin, eSwitchType, motorType, mode)
00359 , _mutex() ,_nodeHandle((std::string("~/") + jointName)), _server(_mutex , _nodeHandle) {
00360 _callbackType = boost::bind(&CloseLoopMotorWithPotentiometer::dynamicCallback, this, _1, _2);
00361
00362 _server.setCallback(_callbackType);
00363 robotican_hardware_interface::RiCBoardPotentiometerConfig config;
00364 config.motor_speed_lpf_hz = motorParam.LPFHzSpeed;
00365 config.motor_input_lpf_hz = motorParam.LPFHzInput;
00366 config.motor_pid_hz = motorParam.PIDHz;
00367 config.motor_speed_lpf_alpha = motorParam.LPFAlphaSpeed;
00368 config.motor_input_lpf_alpha = motorParam.LPFAlphaInput;
00369 config.motor_kp = motorParam.KP;
00370 config.motor_ki = motorParam.KI;
00371 config.motor_kd = motorParam.KD;
00372 config.motor_kff = motorParam.KFF;
00373 config.motor_k = motorParam.K;
00374 config.motor_a = motorParam.a;
00375 config.motor_b = motorParam.b;
00376 config.motor_limit = motorParam.limit;
00377 config.motor_tolerance = motorParam.tolerance;
00378 config.motor_tolerance_time = motorParam.toleranceTime;
00379 _server.updateConfig(config);
00380
00381
00382 _param = motorParam;
00383 _isParamChange = false;
00384 _firstTime = true;
00385 }
00386
00387 void CloseLoopMotorWithPotentiometer::setParams(uint16_t speedLpfHz, uint16_t inputLpfHz, uint16_t pidHz, float speedLpfAlpha, float inputLpfAlpha, float KP,
00388 float KI, float KD, float KFF, float K, float a, float b, float tolerance, uint16_t toleranceTime,
00389 float limit) {
00390 _param.LPFHzSpeed = speedLpfHz;
00391 _param.LPFHzInput = inputLpfHz;
00392 _param.PIDHz = pidHz;
00393 _param.LPFAlphaSpeed = speedLpfAlpha;
00394 _param.LPFAlphaInput = inputLpfAlpha;
00395 _param.KP = KP;
00396 _param.KI = KI;
00397 _param.KD = KD;
00398 _param.KFF = KFF;
00399 _param.K = K;
00400 _param.a = a;
00401 _param.b = b;
00402 _param.tolerance = tolerance;
00403 _param.toleranceTime = toleranceTime;
00404 _param.limit = limit;
00405 _isParamChange = true;
00406
00407 }
00408
00409 void CloseLoopMotorWithPotentiometer::dynamicCallback(robotican_hardware_interface::RiCBoardPotentiometerConfig &config, uint32_t level) {
00410 setParams((uint16_t) config.motor_speed_lpf_hz, (uint16_t) config.motor_input_lpf_hz,
00411 (uint16_t) config.motor_pid_hz, (float) config.motor_speed_lpf_alpha,
00412 (float) config.motor_input_lpf_alpha, (float) config.motor_kp, (float) config.motor_ki,
00413 (float) config.motor_kd, (float) config.motor_kff, (float) config.motor_k, (float) config.motor_a, (float) config.motor_b,
00414 (float) config.motor_tolerance, (uint16_t) config.motor_tolerance_time, (float) config.motor_limit);
00415 }
00416
00417
00418 void CloseLoopMotorWithPotentiometer::write() {
00419 if(!_firstTime) {
00420 CloseLoopMotor::write();
00421 }
00422 if(_isParamChange) {
00423 _isParamChange = false;
00424 SetCloseMotorWithPotentiometer setCloseMotorWithPotentiometer;
00425 setCloseMotorWithPotentiometer.length = sizeof(setCloseMotorWithPotentiometer);
00426 setCloseMotorWithPotentiometer.checkSum = 0;
00427 setCloseMotorWithPotentiometer.id = getId();
00428
00429 setCloseMotorWithPotentiometer.speedLpfHz = _param.LPFHzSpeed;
00430 setCloseMotorWithPotentiometer.inputLpfHz = _param.LPFHzInput;
00431 setCloseMotorWithPotentiometer.pidHz = _param.PIDHz;
00432 setCloseMotorWithPotentiometer.speedLfpAlpha = _param.LPFAlphaSpeed;
00433 setCloseMotorWithPotentiometer.inputLfpAlpha = _param.LPFAlphaInput;
00434 setCloseMotorWithPotentiometer.KP = _param.KP;
00435 setCloseMotorWithPotentiometer.KI = _param.KI;
00436 setCloseMotorWithPotentiometer.KD = _param.KD;
00437 setCloseMotorWithPotentiometer.KFF = _param.KFF;
00438 setCloseMotorWithPotentiometer.K = _param.K;
00439 setCloseMotorWithPotentiometer.a = _param.a;
00440 setCloseMotorWithPotentiometer.b = _param.b;
00441 setCloseMotorWithPotentiometer.limit = _param.limit;
00442 setCloseMotorWithPotentiometer.tolerance = _param.tolerance;
00443 setCloseMotorWithPotentiometer.toleranceTime = _param.toleranceTime;
00444
00445 uint8_t *rawData = (uint8_t*)&setCloseMotorWithPotentiometer;
00446
00447 setCloseMotorWithPotentiometer.checkSum = _transportLayer->calcChecksum(rawData, setCloseMotorWithPotentiometer.length);
00448 _transportLayer->write(rawData, setCloseMotorWithPotentiometer.length);
00449
00450 }
00451
00452 }
00453
00454
00455 void CloseLoopMotorWithPotentiometer::update(const DeviceMessage *deviceMessage) {
00456 CloseLoopMotor::update(deviceMessage);
00457 if(isReady() && _firstTime) {
00458 _jointInfo.cmd = _jointInfo.position;
00459 _firstTime = false;
00460 }
00461 }
00462 }