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


drc_plugin
Author(s): JeongsooLim , SeungwooHong
autogenerated on Sat Jul 22 2017 02:25:33