lwr_controller.cpp
Go to the documentation of this file.
00001 #include <kdl_parser/kdl_parser.hpp>
00002 #include <sys/select.h>
00003 #include <lwr_simulator/lwr_controller.h>
00004 
00005 namespace gazebo
00006 {
00007 
00008 
00010 // Constructor
00011 LWRController::LWRController()
00012 {
00013 }
00014 
00016 // Destructor
00017 LWRController::~LWRController()
00018 {
00019 }
00020 
00022 // Load the controller
00023 void LWRController::Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf )
00024 {
00025   // Get then name of the parent model
00026   std::string modelName = _sdf->GetParent()->GetValueString("name");
00027 
00028   // Get the world name.
00029   this->world = _parent->GetWorld();
00030 
00031   // Get a pointer to the model
00032   this->parent_model_ = _parent;
00033 
00034   // Error message if the model couldn't be found
00035   if (!this->parent_model_)
00036     gzerr << "Unable to get parent model\n";
00037 
00038   // Listen to the update event. This event is broadcast every
00039   // simulation iteration.
00040   this->updateConnection = event::Events::ConnectWorldUpdateStart(
00041       boost::bind(&LWRController::UpdateChild, this));
00042   gzdbg << "plugin model name: " << modelName << "\n";
00043 
00044   // get parameter name
00045   this->robotPrefix = "";
00046   if (_sdf->HasElement("robotPrefix"))
00047     this->robotPrefix = _sdf->GetElement("robotPrefix")->GetValueString();
00048   
00049   remote_port = 7998;
00050   if (_sdf->HasElement("remotePort"))
00051     this->remote_port = _sdf->GetElement("remotePort")->GetValueDouble();
00052 
00053   remote = "127.0.0.1";
00054   if (_sdf->HasElement("remoteIP"))
00055     this->remote = _sdf->GetElement("remoteIP")->GetValueString();
00056 
00057   gzdbg << "remote : " << remote << " : " << remote_port << "\n";
00058   
00059   if (!ros::isInitialized())
00060   {
00061     int argc = 0;
00062     char** argv = NULL;
00063     ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
00064   }
00065   this->rosnode_ = new ros::NodeHandle();
00066     
00067   GetRobotChain();
00068     
00069   for(unsigned int i = 0; i< 7; i++)
00070   {
00071     // fill in gazebo joints pointer
00072     std::string joint_name = this->robotPrefix + "_arm_" + (char)(i + 48) + "_joint";
00073     gazebo::physics::JointPtr joint = this->parent_model_->GetJoint(joint_name);     
00074     if (joint)
00075     {
00076       this->joints_.push_back(joint);
00077     }
00078     else
00079     {
00080       this->joints_.push_back(gazebo::physics::JointPtr());  // FIXME: cannot be null, must be an empty boost shared pointer
00081       ROS_ERROR("A joint named \"%s\" is not part of Mechanism Controlled joints.\n", joint_name.c_str());
00082     }
00083       
00084     stiffness_(i) = 200.0;
00085     damping_(i) = 5.0;
00086     trq_cmd_(i) = 0;
00087     joint_pos_cmd(i) = 0;
00088     
00089     m_msr_data.data.cmdJntPos[i] = 0.0;
00090     m_msr_data.data.cmdJntPosFriOffset[i] = 0.0;
00091   }
00092   
00093   socketFd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
00094   setsockopt(socketFd, SOL_SOCKET, SO_REUSEADDR, 0, 0);
00095   
00096   bzero((char *) &localAddr, sizeof(localAddr));
00097   localAddr.sin_family = AF_INET;
00098   localAddr.sin_addr.s_addr = INADDR_ANY;
00099   localAddr.sin_port = htons(remote_port + 1);
00100 
00101   if (bind(socketFd, (sockaddr*) &localAddr, sizeof(sockaddr_in)) < 0) {
00102     ROS_ERROR("Binding of port failed with ");
00103   }
00104   
00105   bzero((char *) &remoteAddr, sizeof(remoteAddr));
00106         remoteAddr.sin_family = AF_INET;
00107         remoteAddr.sin_addr.s_addr = inet_addr(remote.c_str());
00108         remoteAddr.sin_port = htons(remote_port);
00109   
00110         
00111   m_msr_data.robot.control = FRI_CTRL_JNT_IMP;
00112   m_msr_data.intf.state = FRI_STATE_MON;
00113   m_msr_data.robot.power = 0xFFFF;
00114   m_msr_data.intf.desiredCmdSampleTime = 0.001;
00115   
00116   cnt = 0;
00117 }
00118 
00119 void LWRController::GetRobotChain()
00120 {
00121   std::string chain_start, chain_end;
00122   KDL::Tree my_tree;
00123   std::string robot_desc_string;
00124   rosnode_->param("robot_description", robot_desc_string, std::string());
00125   if (!kdl_parser::treeFromString(robot_desc_string, my_tree)){
00126     ROS_ERROR("Failed to construct kdl tree");
00127   }
00128 
00129   chain_start = std::string("calib_") + this->robotPrefix + "_arm_base_link";
00130   
00131   chain_end = this->robotPrefix + "_arm_7_link";
00132   my_tree.getChain(chain_start, chain_end, chain_);
00133   
00134   dyn = new KDL::ChainDynParam(chain_, KDL::Vector(0.0, 0.0, -9.81));
00135   fk = new KDL::ChainFkSolverPos_recursive(chain_);
00136   jc = new KDL::ChainJntToJacSolver(chain_);
00137 }
00138 
00140 // Update the controller
00141 void LWRController::UpdateChild()
00142 {
00143   struct sockaddr cliAddr;
00144   unsigned int cliAddr_len;
00145   KDL::Frame f;
00146   KDL::Jacobian jac(7);
00147   KDL::JntSpaceInertiaMatrix H(7);
00148   KDL::JntArray pos(7);
00149   KDL::JntArray grav(7);
00150 
00151   for(unsigned int i = 0; i< 7; i++)
00152   {
00153     m_msr_data.data.msrJntPos[i] = pos(i) = joint_pos(i) = joints_[i]->GetAngle(0).GetAsRadian();
00154     joint_vel(i) = joints_[i]->GetVelocity(0);
00155   }
00156 
00157   /*dyn->JntToGravity(pos, grav);*/
00158   fk->JntToCart(pos, f);
00159   
00160   m_msr_data.data.msrCartPos[0] = f.M.data[0];
00161   m_msr_data.data.msrCartPos[1] = f.M.data[1];
00162   m_msr_data.data.msrCartPos[2] = f.M.data[2];
00163   m_msr_data.data.msrCartPos[3] = f.p.data[0];
00164   
00165   m_msr_data.data.msrCartPos[4] = f.M.data[3];
00166   m_msr_data.data.msrCartPos[5] = f.M.data[4];
00167   m_msr_data.data.msrCartPos[6] = f.M.data[5];
00168   m_msr_data.data.msrCartPos[7] = f.p.data[1];
00169   
00170   m_msr_data.data.msrCartPos[8] = f.M.data[6];
00171   m_msr_data.data.msrCartPos[9] = f.M.data[7];
00172   m_msr_data.data.msrCartPos[10] = f.M.data[8];
00173   m_msr_data.data.msrCartPos[11] = f.p.data[2];
00174   
00175   jc->JntToJac(pos, jac);
00176   jac.changeRefFrame(KDL::Frame(f.Inverse().M));
00177   //Kuka uses Tx, Ty, Tz, Rz, Ry, Rx convention, so we need to swap Rz and Rx
00178   jac.data.row(3).swap(jac.data.row(5));
00179   for ( int i = 0; i < FRI_CART_VEC; i++)
00180     for ( int j = 0; j < LBR_MNJ; j++)
00181       m_msr_data.data.jacobian[i*LBR_MNJ+j] = jac.data(i,j);
00182     
00183   dyn->JntToMass(pos, H);
00184   for(unsigned int i=0;i<LBR_MNJ;i++) {
00185     for(unsigned int j=0;j<LBR_MNJ;j++) {
00186       m_msr_data.data.massMatrix[LBR_MNJ*i+j] = H.data(i, j);
00187     }
00188   }
00189   
00190   if(cnt > 10)
00191   {
00192     m_msr_data.intf.state = FRI_STATE_CMD;
00193   } else 
00194   {
00195     m_msr_data.intf.state = FRI_STATE_MON;
00196   }
00197   
00198   //send msr data
00199   if (0 > sendto(socketFd, (void*) &m_msr_data, sizeof(m_msr_data), 0,
00200                         (sockaddr*) &remoteAddr, sizeof(remoteAddr))) {
00201                 ROS_ERROR( "Sending datagram failed.");
00202                 //return -1;
00203         }
00204 
00205   fd_set rd;
00206   struct timeval tv;
00207   tv.tv_sec = 0;
00208   tv.tv_usec = 5000;
00209   
00210   FD_ZERO(&rd);
00211   FD_SET(socketFd, &rd);
00212         
00213   int sret = select(socketFd+1, &rd, NULL, NULL, &tv);
00214   if(sret > 0)
00215   {
00216     int n = recvfrom(socketFd, (void*) &m_cmd_data, sizeof(m_cmd_data), 0,
00217                         (sockaddr*) &cliAddr, &cliAddr_len);
00218     if (sizeof(tFriCmdData) != n) {
00219       ROS_ERROR( "bad packet length");
00220     }
00221 
00222     for(unsigned int i = 0; i < 7; i++){
00223       joint_pos_cmd(i) = m_cmd_data.cmd.jntPos[i];
00224       stiffness_(i) = m_cmd_data.cmd.jntStiffness[i];
00225       damping_(i) = m_cmd_data.cmd.jntDamping[i];
00226       trq_cmd_(i) = m_cmd_data.cmd.addJntTrq[i];
00227     }
00228     trq = stiffness_.asDiagonal() * (joint_pos_cmd -joint_pos) - damping_.asDiagonal() * joint_vel + trq_cmd_;
00229 
00230     for(unsigned int i = 0; i< 7; i++)
00231     {
00232       joints_[i]->SetForce(0, trq(i));
00233     }
00234     if(cnt < 20)
00235     {
00236       ++cnt;
00237     }
00238     
00239   } else {
00240     if(cnt > 0)
00241       --cnt;
00242   }
00243 }
00244 
00245 GZ_REGISTER_MODEL_PLUGIN(LWRController);
00246 
00247 }


lwr_simulation
Author(s): Konrad Banachowicz
autogenerated on Mon Oct 6 2014 02:00:54