00001
00002 #include "drc_plugin.h"
00003
00004
00005 #define PODO_ADDR "10.12.3.30"
00006 #define PODO_PORT 8888
00007
00008 const float D2Rf = 0.0174533;
00009 const float R2Df = 57.2957802;
00010 char ip[20];
00011
00012 namespace gazebo
00013 {
00014
00015 void DRCPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
00016 {
00017 std::cout << "\033[1;32m===================================" << std::endl;
00018 std::cout << " Now Start the DRCPlugin..!!" << std::endl << std::endl;
00019 std::cout << " Developer: Jeongsoo Lim" << std::endl;
00020 std::cout << " E-mail : yjs0497@kaist.ac.kr" << std::endl;
00021 std::cout << "===================================\033[0m" << std::endl;
00022
00023
00024 if (!ros::isInitialized()){
00025 ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
00026 << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
00027 return;
00028 }
00029
00030 filt_alpha = 0.05;
00031 new_ref_cnt = 0;
00032 home_ref_RWH = home_ref_LWH = 0.0;
00033
00034 model = _model;
00035 model->SetGravityMode(false);
00036 JCon = new physics::JointController(model);
00037
00038 std::cout << "Model Name : " << model->GetName() << std::endl;
00039
00040
00041 FILE *fpNet = NULL;
00042 fpNet = fopen("/home/hubo/catkin_ws/src/drc_hubo/ros/settings/network.txt", "r");
00043 if(fpNet == NULL){
00044 std::cout << ">>> Network File Open Error..!!" << std::endl;
00045 sprintf(ip, PODO_ADDR);
00046 }else{
00047 std::cout << ">>> Network File Open Success..!!" << std::endl;
00048 fscanf(fpNet, "%s", ip);
00049 fclose(fpNet);
00050 }
00051
00052 if(CreateSocket(ip, PODO_PORT)){
00053 std::cout << "Created Socket.." << std::endl;
00054 RXDataSize = sizeof(DRC_GAZEBO_JOINT);
00055 TXDataSize = sizeof(DRC_GAZEBO_SENSOR);
00056 RXBuffer = (void*)malloc(RXDataSize);
00057 TXBuffer = (void*)malloc(TXDataSize);
00058 int threadID = pthread_create(&LANTHREAD_t, NULL, &LANThread, this);
00059 if(threadID < 0){
00060 std::cout << "Create Thread Error.." << std::endl;
00061 }
00062 }else{
00063 std::cout << "Create Socket Error.." << std::endl;
00064 }
00065
00066
00067 FILE *fpGain = NULL;
00068 fpGain = fopen("/home/hubo/catkin_ws/src/drc_hubo/ros/settings/gain.txt", "r");
00069 if(fpGain == NULL){
00070 std::cout << ">>> Gain File Open Error..!!" << std::endl;
00071 }else{
00072 std::cout << ">>> Gain File Open Success..!!" << std::endl;
00073 for(int i=0; i<NO_OF_JOINTS; i++){
00074 fscanf(fpGain, "%f, %f, %f\n", &PIDGains[i][0], &PIDGains[i][1], &PIDGains[i][2]);
00075 }
00076 fclose(fpGain);
00077 }
00078
00079
00080 for(int i=0; i<NO_OF_JOINTS; i++){
00081 if(i == RHAND || i == LHAND)
00082 continue;
00083 JPtrs[i] = model->GetJoint(JointNameList[i]);
00084 JCon->AddJoint(JPtrs[i]);
00085 JCon->SetPositionPID(JPtrs[i]->GetScopedName(),common::PID(PIDGains[i][0],PIDGains[i][1],PIDGains[i][2]));
00086 setGainOverride(i, 100, 5);
00087 }
00088 for(int i=0; i<5; i++)
00089 adjustAllGain();
00090
00091
00092 JPtr_LHAND[0] = model->GetJoint("LHAND_a1");
00093 JPtr_LHAND[1] = model->GetJoint("LHAND_a2");
00094 JPtr_LHAND[2] = model->GetJoint("LHAND_a3");
00095 JPtr_LHAND[3] = model->GetJoint("LHAND_b1");
00096 JPtr_LHAND[4] = model->GetJoint("LHAND_b2");
00097 JPtr_LHAND[5] = model->GetJoint("LHAND_b3");
00098 JPtr_LHAND[6] = model->GetJoint("LHAND_c1");
00099 JPtr_LHAND[7] = model->GetJoint("LHAND_c2");
00100 JPtr_LHAND[8] = model->GetJoint("LHAND_c3");
00101
00102 JPtr_RHAND[0] = model->GetJoint("RHAND_a1");
00103 JPtr_RHAND[1] = model->GetJoint("RHAND_a2");
00104 JPtr_RHAND[2] = model->GetJoint("RHAND_a3");
00105 JPtr_RHAND[3] = model->GetJoint("RHAND_b1");
00106 JPtr_RHAND[4] = model->GetJoint("RHAND_b2");
00107 JPtr_RHAND[5] = model->GetJoint("RHAND_b3");
00108 JPtr_RHAND[6] = model->GetJoint("RHAND_c1");
00109 JPtr_RHAND[7] = model->GetJoint("RHAND_c2");
00110 JPtr_RHAND[8] = model->GetJoint("RHAND_c3");
00111 for(int i=0; i<9; i++){
00112 JCon->AddJoint(JPtr_RHAND[i]);
00113 JCon->SetPositionPID(JPtr_RHAND[i]->GetScopedName(),common::PID(20,0,0.01));
00114 JCon->AddJoint(JPtr_LHAND[i]);
00115 JCon->SetPositionPID(JPtr_LHAND[i]->GetScopedName(),common::PID(20,0,0.01));
00116 }
00117
00118
00119 JPtr_RAFT = model->GetJoint("RAFT");
00120 JPtr_LAFT = model->GetJoint("LAFT");
00121 JPtr_RWFT = model->GetJoint("RWFT");
00122 JPtr_LWFT = model->GetJoint("LWFT");
00123 JCon->AddJoint(JPtr_RAFT);
00124 JCon->AddJoint(JPtr_LAFT);
00125 JCon->AddJoint(JPtr_RWFT);
00126 JCon->AddJoint(JPtr_LWFT);
00127 JCon->SetPositionPID(JPtr_RAFT->GetScopedName(),common::PID(PIDGains[RAR][0],PIDGains[RAR][1],PIDGains[RAR][2]));
00128 JCon->SetPositionPID(JPtr_LAFT->GetScopedName(),common::PID(PIDGains[LAR][0],PIDGains[LAR][1],PIDGains[LAR][2]));
00129 JCon->SetPositionPID(JPtr_RWFT->GetScopedName(),common::PID(0,0,0.001));
00130 JCon->SetPositionPID(JPtr_LWFT->GetScopedName(),common::PID(0,0,0.001));
00131
00132
00133
00134 JPtr_RFWH = model->GetJoint("RFWH");
00135 JPtr_LFWH = model->GetJoint("LFWH");
00136 JPtr_RFUNI = model->GetJoint("RFUNI");
00137 JPtr_LFUNI = model->GetJoint("LFUNI");
00138 JPtr_Head = model->GetJoint("head_joint");
00139
00140
00141
00142 for(int i=0; i<NO_OF_JOINTS; i++){
00143 if(i == RHAND || i == LHAND)
00144 continue;
00145 JPtrs[i]->SetProvideFeedback(true);
00146 }
00147
00148 JPtr_RAFT->SetProvideFeedback(true);
00149 JPtr_LAFT->SetProvideFeedback(true);
00150 JPtr_RWFT->SetProvideFeedback(true);
00151 JPtr_LWFT->SetProvideFeedback(true);
00152
00153
00154
00155 for(int i=0;i<=NO_OF_JOINTS;i++){
00156 refs[i] = 0.0;
00157 }
00158 ref_RHAND = ref_LHAND = 1.4708;
00159
00160 refs[LEB] += -20*D2Rf;
00161 refs[REB] += -20*D2Rf;
00162 refs[RSR] += -15*D2Rf;
00163 refs[LSR] += 15*D2Rf;
00164
00165
00166 for(int i=0; i<=NO_OF_JOINTS; i++){
00167 if(i == RHAND || i == LHAND)
00168 continue;
00169 JCon->SetJointPosition(JPtrs[i],refs[i]);
00170 }
00171 JCon->SetJointPosition(JPtr_RAFT,0);
00172 JCon->SetJointPosition(JPtr_LAFT,0);
00173 JCon->SetJointPosition(JPtr_RWFT,0);
00174 JCon->SetJointPosition(JPtr_LWFT,0);
00175
00176 RAFT = boost::dynamic_pointer_cast<sensors::ForceTorqueSensor>(sensors::get_sensor("FT_RAFT"));
00177 LAFT = boost::dynamic_pointer_cast<sensors::ForceTorqueSensor>(sensors::get_sensor("FT_LAFT"));
00178 IMU = boost::dynamic_pointer_cast<sensors::ImuSensor>(sensors::get_sensor("imu"));
00179
00180 if(!LAFT || !RAFT || !IMU)
00181 std::cout << "SENSOR ERROR (NULL)" << std::endl;
00182
00183 IMU->SetActive(true);
00184 LAFT->SetActive(true);
00185 RAFT->SetActive(true);
00186
00187
00188 IMU->SetUpdateRate(1000);
00189 LAFT->SetUpdateRate(1000);
00190 RAFT->SetUpdateRate(1000);
00191
00192
00193 UpdateConnection = event::Events::ConnectWorldUpdateBegin(boost::bind(&DRCPlugin::OnUpdate, this, _1));
00194 LAFTupdateConnection = LAFT->ConnectUpdated(boost::bind(&DRCPlugin::OnLAFTUpdate, this));
00195 RAFTupdateConnection = RAFT->ConnectUpdated(boost::bind(&DRCPlugin::OnRAFTUpdate, this));
00196 IMUupdateConnection = IMU->ConnectUpdated(boost::bind(&DRCPlugin::OnIMUUpdate, this));
00197
00198 }
00199
00200
00201 void DRCPlugin::OnUpdate(const common::UpdateInfo &){
00202 static int cnt = 0;
00203
00204 if(FirstRcvData){
00205 if(FirstRcvCnt == 0){
00206 model->SetGravityMode(false);
00207 model->SetWorldPose(math::Pose(0,0,2, 0,0,0));
00208 for(int i=0; i<NO_OF_JOINTS; i++){
00209 setGainOverride(i, 100, 20);
00210 }
00211 }else if(FirstRcvCnt == 21){
00212 for(int i=0; i<NO_OF_JOINTS; i++){
00213 if(i == RHAND || i == LHAND || i == RWH || i == LWH)
00214 continue;
00215 refs[i] = RXJointData.JointReference[i]*D2Rf;
00216 }
00217 ref_RWH += RXJointData.JointReference[RWH]*D2Rf/5.0;
00218 ref_LWH += RXJointData.JointReference[LWH]*D2Rf/5.0;
00219 ref_RHAND += RXJointData.JointReference[RHAND]*1.4708/40/2000;
00220 ref_LHAND += RXJointData.JointReference[LHAND]*1.4708/40/2000;
00221 if(ref_RHAND>1.4708) ref_RHAND = 1.4708;
00222 if(ref_RHAND<0) ref_RHAND = 0;
00223 if(ref_LHAND>1.4708) ref_LHAND = 1.4708;
00224 if(ref_LHAND<0) ref_LHAND = 0;
00225
00226
00227 refs[LEB] += -20*D2Rf;
00228 refs[REB] += -20*D2Rf;
00229 refs[RSR] += -15*D2Rf;
00230 refs[LSR] += 15*D2Rf;
00231
00232 for(int i=0; i<NO_OF_JOINTS; i++){
00233 setGainOverride(i, 0, 1000);
00234 }
00235 }else if(FirstRcvCnt == 3200){
00236 if(refs[RKN]*R2Df <= 140.0 || refs[LKN]*R2Df <= 140.0){
00237
00238 float z;
00239 try{
00240 listener.waitForTransform("Body_RAFT", "Body_TORSO", ros::Time(0), ros::Duration(0.05));
00241 listener.waitForTransform("Body_LAFT", "Body_TORSO", ros::Time(0), ros::Duration(0.05));
00242 listener.lookupTransform("Body_RAFT", "Body_TORSO", ros::Time(0), right);
00243 listener.lookupTransform("Body_LAFT", "Body_TORSO", ros::Time(0), left);
00244 z = (left.getOrigin().z() + right.getOrigin().z()) / 2.0 + 0.05;
00245 std::cout << "original : " << model->GetInitialRelativePose().pos.z << std::endl;
00246 std::cout << "new one : " << z << std::endl;
00247 }catch(tf::TransformException &ex){
00248 ROS_WARN("%s",ex.what());
00249 z = 2*0.5*cos(refs[RKN]/2.0)+0.2+0.02;
00250 std::cout << "original 1: " << model->GetInitialRelativePose().pos.z << std::endl;
00251 std::cout << "new one 1: " << z << std::endl;
00252 }
00253 model->SetWorldPose(math::Pose(0,0,z, 0,0,0));
00254 }else{
00255
00256 float z;
00257 try{
00258 listener.waitForTransform("Body_TORSO", "Body_RWH", ros::Time(0), ros::Duration(0.05));
00259 listener.waitForTransform("Body_TORSO", "Body_LWH", ros::Time(0), ros::Duration(0.05));
00260 listener.lookupTransform("Body_TORSO", "Body_RWH", ros::Time(0), right);
00261 listener.lookupTransform("Body_TORSO", "Body_LWH", ros::Time(0), left);
00262 z = -(left.getOrigin().z() + right.getOrigin().z()) / 2.0 + 0.06 + 0.05;
00263 }catch(tf::TransformException &ex){
00264 ROS_WARN("%s",ex.what());
00265 z = 0.4;
00266 }
00267 setGainOverride(RKN, 100, 100);
00268 setGainOverride(LKN, 100, 100);
00269 model->SetWorldPose(math::Pose(0,0,z, 0,0,0));
00270 }
00271 model->GetWorld()->GetPhysicsEngine()->SetGravity(math::Vector3(0,0,-2.9));
00272 model->SetGravityMode(true);
00273 }else if(FirstRcvCnt == 6500){
00274 model->GetWorld()->GetPhysicsEngine()->SetGravity(math::Vector3(0,0,-9.8));
00275 FirstRcvData = false;
00276 SimulateOk = true;
00277 }
00278 }
00279
00280 if(SimulateOk){
00281 for(int i=0; i<NO_OF_JOINTS; i++){
00282 if(i == RHAND || i == LHAND || i == RWH || i == LWH)
00283 continue;
00284 refs[i] = RXJointData.JointReference[i]*D2Rf;
00285 }
00286
00287 ref_RWH += RXJointData.JointReference[RWH]*D2Rf/5.0;
00288 ref_LWH += RXJointData.JointReference[LWH]*D2Rf/5.0;
00289 ref_RHAND += RXJointData.JointReference[RHAND]*1.4708/40/2000;
00290 ref_LHAND += RXJointData.JointReference[LHAND]*1.4708/40/2000;
00291 if(ref_RHAND>1.4708) ref_RHAND = 1.4708;
00292 if(ref_RHAND<0) ref_RHAND = 0;
00293 if(ref_LHAND>1.4708) ref_LHAND = 1.4708;
00294 if(ref_LHAND<0) ref_LHAND = 0;
00295
00296
00297 refs[LEB] += -20*D2Rf;
00298 refs[REB] += -20*D2Rf;
00299 refs[RSR] += -15*D2Rf;
00300 refs[LSR] += 15*D2Rf;
00301 }
00302
00303
00304
00305
00306 for(int i=0; i<NO_OF_JOINTS; i++){
00307 if(i == RHAND || i == LHAND || i == RWH || i == LWH)
00308 continue;
00309 JCon->SetPositionTarget(JPtrs[i]->GetScopedName(),refs[i]);
00310 }
00311 JCon->SetPositionTarget(JPtrs[RWH]->GetScopedName(), ref_RWH);
00312 JCon->SetPositionTarget(JPtrs[LWH]->GetScopedName(), ref_LWH);
00313 for(int i=0; i<9; i++){
00314 JCon->SetPositionTarget(JPtr_RHAND[i]->GetScopedName(), ref_RHAND);
00315 JCon->SetPositionTarget(JPtr_LHAND[i]->GetScopedName(), ref_LHAND);
00316 }
00317 JCon->SetPositionTarget(JPtr_RAFT->GetScopedName(),0);
00318 JCon->SetPositionTarget(JPtr_LAFT->GetScopedName(),0);
00319 JCon->SetPositionTarget(JPtr_RWFT->GetScopedName(),0);
00320 JCon->SetPositionTarget(JPtr_LWFT->GetScopedName(),0);
00321
00322 JPtr_Head->SetPosition(0, 0.0);
00323 JCon->Update();
00324
00325
00326 adjustAllGain();
00327
00328
00329
00330 physics::JointWrench wrenchR = JPtr_RWFT->GetForceTorque(0);
00331 math::Vector3 RWF = wrenchR.body1Force;
00332 math::Vector3 RWT = wrenchR.body1Torque;
00333 filt_RWF = filt_alpha*RWF + (1-filt_alpha)*filt_RWF;
00334 filt_RWT = filt_alpha*RWT + (1-filt_alpha)*filt_RWT;
00335 if(connectionStatus){
00336 TXSensorData.FTSensor[2].force[0] = filt_RWF.x;
00337 TXSensorData.FTSensor[2].force[1] = filt_RWF.y;
00338 TXSensorData.FTSensor[2].force[2] = filt_RWF.z;
00339 TXSensorData.FTSensor[2].torque[0] = filt_RWT.x;
00340 TXSensorData.FTSensor[2].torque[1] = filt_RWT.y;
00341 TXSensorData.FTSensor[2].torque[2] = filt_RWT.z;
00342 }
00343 physics::JointWrench wrenchL = JPtr_LWFT->GetForceTorque(0);
00344 math::Vector3 LWF = wrenchL.body1Force;
00345 math::Vector3 LWT = wrenchL.body1Torque;
00346 filt_LWF = filt_alpha*LWF + (1-filt_alpha)*filt_LWF;
00347 filt_LWT = filt_alpha*LWT + (1-filt_alpha)*filt_LWT;
00348 if(connectionStatus){
00349 TXSensorData.FTSensor[3].force[0] = filt_LWF.x;
00350 TXSensorData.FTSensor[3].force[1] = filt_LWF.y;
00351 TXSensorData.FTSensor[3].force[2] = filt_LWF.z;
00352 TXSensorData.FTSensor[3].torque[0] = filt_LWT.x;
00353 TXSensorData.FTSensor[3].torque[1] = filt_LWT.y;
00354 TXSensorData.FTSensor[3].torque[2] = filt_LWT.z;
00355 }
00356
00357
00358
00359 if(cnt%5==0){
00360 if(connectionStatus){
00361
00362 for(int i=0; i<NO_OF_JOINTS; i++){
00363 if(i == RHAND || i == LHAND)
00364 continue;
00365 TXSensorData.JointCurrentPosition[i] = JPtrs[i]->GetAngle(0).Radian()*R2Df;
00366
00367 if(i == RWH){
00368 TXSensorData.JointCurrentPosition[i] -= home_ref_RWH;
00369 }else if(i == LWH){
00370 TXSensorData.JointCurrentPosition[i] -= home_ref_LWH;
00371 }
00372 }
00373 TXSensorData.JointCurrentPosition[RHAND] = (JPtr_RHAND[0]->GetAngle(0).Radian())*R2Df;
00374 TXSensorData.JointCurrentPosition[LHAND] = (JPtr_LHAND[0]->GetAngle(0).Radian())*R2Df;
00375
00376
00377 TXSensorData.JointCurrentPosition[LEB] -= -20.0;
00378 TXSensorData.JointCurrentPosition[REB] -= -20.0;
00379 TXSensorData.JointCurrentPosition[RSR] -= -15.0;
00380 TXSensorData.JointCurrentPosition[LSR] -= 15.0;
00381
00382 common::Time simTime = model->GetWorld()->GetSimTime();
00383 ros::Time rosTime = ros::Time::now();
00384
00385 TXSensorData.Sim_Time.sec = simTime.sec;
00386 TXSensorData.Sim_Time.nsec = simTime.nsec;
00387 TXSensorData.ROS_Time.sec = rosTime.sec;
00388 TXSensorData.ROS_Time.nsec = rosTime.nsec;
00389 write(sock, &TXSensorData, sizeof(TXSensorData));
00390 }
00391 }
00392 cnt++;
00393 FirstRcvCnt++;
00394 }
00395
00396
00397
00398 void DRCPlugin::OnLAFTUpdate(){
00399 math::Vector3 LAF = LAFT->GetForce();
00400 math::Vector3 LAT = LAFT->GetTorque();
00401 filt_LAF = filt_alpha*LAF + (1-filt_alpha)*filt_LAF;
00402 filt_LAT = filt_alpha*LAT + (1-filt_alpha)*filt_LAT;
00403 if(connectionStatus){
00404 TXSensorData.FTSensor[1].force[0] = filt_LAF.x;
00405 TXSensorData.FTSensor[1].force[1] = filt_LAF.y;
00406 TXSensorData.FTSensor[1].force[2] = filt_LAF.z;
00407 TXSensorData.FTSensor[1].torque[0] = filt_LAT.x;
00408 TXSensorData.FTSensor[1].torque[1] = filt_LAT.y;
00409 TXSensorData.FTSensor[1].torque[2] = filt_LAT.z;
00410 }
00411 }
00412
00413 void DRCPlugin::OnRAFTUpdate(){
00414 math::Vector3 RAF = RAFT->GetForce();
00415 math::Vector3 RAT = RAFT->GetTorque();
00416 filt_RAF = filt_alpha*RAF + (1-filt_alpha)*filt_RAF;
00417 filt_RAT = filt_alpha*RAT + (1-filt_alpha)*filt_RAT;
00418 if(connectionStatus){
00419 TXSensorData.FTSensor[0].force[0] = filt_RAF.x;
00420 TXSensorData.FTSensor[0].force[1] = filt_RAF.y;
00421 TXSensorData.FTSensor[0].force[2] = filt_RAF.z;
00422 TXSensorData.FTSensor[0].torque[0] = filt_RAT.x;
00423 TXSensorData.FTSensor[0].torque[1] = filt_RAT.y;
00424 TXSensorData.FTSensor[0].torque[2] = filt_RAT.z;
00425 }
00426 }
00427
00428 void DRCPlugin::OnIMUUpdate(){
00429 math::Vector3 AV = IMU->GetAngularVelocity();
00430 math::Vector3 LA = IMU->GetLinearAcceleration();
00431 math::Quaternion q = IMU->GetOrientation();
00432 if(connectionStatus){
00433 float l = q.w*q.w + q.x*q.x + q.y*q.y + q.z*q.z;
00434 float w, x, y, z;
00435 if(l > 0.00001){
00436 l = sqrt(l);
00437 w = q.w/l; x = q.x/l; y = q.y/l; z = q.z/l;
00438 }else{
00439 w = 1; x = 0; y = 0; z = 0;
00440 }
00441
00442 TXSensorData.IMUSensor[0] = atan2(2*(w*x + y*z), 1-2*(x*x + y*y))*R2Df;
00443 TXSensorData.IMUSensor[1] = asin(2*(w*y - z*x))*R2Df;
00444 TXSensorData.IMUSensor[2] = atan2(2*(w*z + x*y), 1-2*(y*y + z*z))*R2Df;
00445
00446 TXSensorData.IMUSensor[3] = AV.x*R2Df;
00447 TXSensorData.IMUSensor[4] = AV.y*R2Df;
00448 TXSensorData.IMUSensor[5] = AV.z*R2Df;
00449
00450 TXSensorData.IMUSensor[6] = LA.x;
00451 TXSensorData.IMUSensor[7] = LA.y;
00452 TXSensorData.IMUSensor[8] = LA.z;
00453 }
00454 }
00455
00456
00457 int DRCPlugin::CreateSocket(const char *addr, int port){
00458 sock = socket(AF_INET, SOCK_STREAM, 0);
00459 if(sock == -1){
00460 return false;
00461 }
00462 client.sin_addr.s_addr = inet_addr(addr);
00463 client.sin_family = AF_INET;
00464 client.sin_port = htons(port);
00465
00466 int optval = 1;
00467 setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof(optval));
00468 setsockopt(sock, SOL_SOCKET, SO_REUSEPORT, &optval, sizeof(optval));
00469
00470 return true;
00471 }
00472
00473
00474 int DRCPlugin::Connect2Server(){
00475 if(connect(sock, (struct sockaddr*)&client, sizeof(client)) < 0){
00476 return false;
00477 }
00478 std::cout << "Client connect to server!! (DRC_PLUGIN)" << std::endl;
00479 return true;
00480 }
00481
00482 void DRCPlugin::NewRXData(){
00483 if(FirstRcvData == false && SimulateOk == false){
00484 FirstRcvCnt = 0;
00485 FirstRcvData = true;
00486 }
00487
00488
00489 for(int i=0; i<NO_OF_JOINTS; i++){
00490 prev_refs[i] = refs[i];
00491 }
00492
00493
00494 memcpy(&(RXJointData), RXBuffer, RXDataSize);
00495
00496
00497 for(int i=0; i<NO_OF_JOINTS; i++){
00498 inc_refs[i] = (RXJointData.JointReference[i]*D2Rf-prev_refs[i])/5.0;
00499 }
00500
00501 new_ref_cnt = 0;
00502 }
00503
00504
00505 void *DRCPlugin::LANThread(void *_arg){
00506 DRCPlugin *dp = (DRCPlugin*)_arg;
00507
00508 dp->threadWorking = true;
00509 dp->FirstRcvData = false;
00510 dp->SimulateOk = false;
00511
00512 unsigned int tcp_status = 0x00;
00513 int tcp_size = 0;
00514 int connectCnt = 0;
00515
00516 int readDone = true;
00517 int dataType = 0;
00518
00519 while(dp->threadWorking){
00520 usleep(100);
00521 if(tcp_status == 0x00){
00522
00523 if(dp->sock == 0){
00524 dp->CreateSocket(ip, PODO_PORT);
00525 }
00526 if(dp->Connect2Server()){
00527 tcp_status = 0x01;
00528 dp->connectionStatus = true;
00529 connectCnt = 0;
00530 }else{
00531 if(connectCnt%10 == 0)
00532 std::cout << "Connect to Server Failed.." << std::endl;
00533 connectCnt++;
00534 }
00535 usleep(1000*1000);
00536 }
00537 if(tcp_status == 0x01){
00538
00539 if(readDone == true){
00540 tcp_size = read(dp->sock, &dataType, sizeof(int));
00541 if(tcp_size == sizeof(int)){
00542 readDone = false;
00543 }
00544 }
00545 else if(readDone == false){
00546 switch(dataType){
00547 case GAZEBO_TYPE_JOINT:
00548 tcp_size = read(dp->sock, dp->RXBuffer, sizeof(DRC_GAZEBO_JOINT));
00549 if(tcp_size == sizeof(DRC_GAZEBO_JOINT)){
00550 dp->NewRXData();
00551 readDone = true;
00552 dataType = GAZEBO_TYPE_NO;
00553 }
00554 break;
00555 case GAZEBO_TYPE_GAINOVERRIDE:
00556 tcp_size = read(dp->sock, &dp->GainOverrideCMD, sizeof(DRC_GAZEBO_GO_CMD));
00557 if(tcp_size == sizeof(DRC_GAZEBO_GO_CMD)){
00558 dp->setGainOverride(dp->GainOverrideCMD.joint, dp->GainOverrideCMD.gain, dp->GainOverrideCMD.timeMs);
00559 readDone = true;
00560 dataType = GAZEBO_TYPE_NO;
00561 }
00562 break;
00563 case GAZEBO_TYPE_HOME:
00564 int joint;
00565 tcp_size = read(dp->sock, &dp->HomeJoint, sizeof(int));
00566 if(tcp_size == sizeof(int)){
00567 dp->findHome(dp->HomeJoint);
00568 readDone = true;
00569 dataType = GAZEBO_TYPE_NO;
00570 }
00571 break;
00572 default:
00573 std::cout << "DATA TYPE :" << dataType << std::endl;
00574 break;
00575 }
00576 }
00577
00578
00579 if(tcp_size == 0){
00580 tcp_status = 0x00;
00581 dp->connectionStatus = false;
00582 dp->FirstRcvData = false;
00583 dp->SimulateOk = false;
00584 close(dp->sock);
00585 dp->sock = 0;
00586 std::cout << "Socket Disconnected.." << std::endl;
00587 }
00588 }
00589 }
00590 return NULL;
00591 }
00592
00593
00594
00595 void DRCPlugin::setGainOverride(int joint, float gain, long msTime){
00596 GainOverride[joint].flag = false;
00597 GainOverride[joint].togo = gain;
00598 GainOverride[joint].init = GainOverride[joint].current;
00599 GainOverride[joint].delta = GainOverride[joint].togo - GainOverride[joint].current;
00600 GainOverride[joint].curCnt = 0;
00601 GainOverride[joint].goalCnt = msTime;
00602 GainOverride[joint].flag = true;
00603 }
00604
00605 void DRCPlugin::moveGainOverride(int joint){
00606 if(GainOverride[joint].flag == true){
00607 GainOverride[joint].curCnt++;
00608 if(GainOverride[joint].goalCnt <= GainOverride[joint].curCnt){
00609 GainOverride[joint].goalCnt = GainOverride[joint].curCnt = 0;
00610 GainOverride[joint].current = GainOverride[joint].togo;
00611 GainOverride[joint].flag = false;
00612 }else{
00613 GainOverride[joint].current = GainOverride[joint].init + GainOverride[joint].delta*GainOverride[joint].curCnt/GainOverride[joint].goalCnt;
00614 }
00615 }
00616 }
00617
00618 void DRCPlugin::adjustAllGain(){
00619 for(int i=0; i<NO_OF_JOINTS; i++){
00620 if(i == RHAND || i == LHAND)
00621 continue;
00622 if(GainOverride[i].flag == true){
00623 moveGainOverride(i);
00624 double gain = pow(0.93, GainOverride[i].current);
00625 JCon->SetPositionPID(JPtrs[i]->GetScopedName(),common::PID(gain*PIDGains[i][0],gain*PIDGains[i][1],gain*PIDGains[i][2]));
00626 }
00627 }
00628 }
00629
00630 void DRCPlugin::findHome(int jNum){
00631
00632 if(jNum == RWH){
00633 home_ref_RWH += ref_RWH;
00634 ref_RWH = 0.0;
00635 setGainOverride(RWH, 0, 10);
00636 }else if(jNum == LWH){
00637 home_ref_LWH += ref_LWH;
00638 ref_LWH = 0.0;
00639 setGainOverride(LWH, 0, 10);
00640 }
00641 }
00642
00643
00644 GZ_REGISTER_MODEL_PLUGIN(DRCPlugin)
00645 }