drc_plugin.cpp
Go to the documentation of this file.
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     // Make sure the ROS node for Gazebo has already been initialized
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     // Create Socket ---------------------
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     // Load Gains for Joints --------------
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     // provide feedback for getting wrench---
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     // Listen to the update event. This event is broadcast every simulation iteration.
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             // default offset
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                 // Walking mode
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                 // Wheel mode
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         // default offset
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     // move joints
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     // adjust gain override
00326     adjustAllGain();
00327 
00328 
00329     // RWFT, LWFT ------
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             // send sensor data
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             // default offset
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         // roll, pitch, yaw
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         // rvel, pvel, yvel
00446         TXSensorData.IMUSensor[3] = AV.x*R2Df;
00447         TXSensorData.IMUSensor[4] = AV.y*R2Df;
00448         TXSensorData.IMUSensor[5] = AV.z*R2Df;
00449         // accx, accy, accz
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     // save previous refs
00489     for(int i=0; i<NO_OF_JOINTS; i++){
00490         prev_refs[i] = refs[i];
00491     }
00492 
00493     // refresh new refs
00494     memcpy(&(RXJointData), RXBuffer, RXDataSize);
00495 
00496     // calculate incremental refs
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             // If client was not connected
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             // If client was connected
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             // disconnected..
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     // Only for RWH & LWH
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 }


drc_plugin
Author(s): JeongsooLim , SeungwooHong
autogenerated on Sat Jun 8 2019 20:42:28