FRIComponent.cpp
Go to the documentation of this file.
00001 #include <rtt/TaskContext.hpp>
00002 #include <rtt/Port.hpp>
00003 #include <rtt/Component.hpp>
00004 
00005 // Start of user code includes
00006 #include <kdl/frames.hpp>
00007 #include <kdl/jntarray.hpp>
00008 #include <kdl/jacobian.hpp>
00009 
00010 #include <kuka_lwr_fri/friComm.h>
00011 
00012 #include <lwr_fri/CartesianImpedance.h>
00013 #include <lwr_fri/FriJointImpedance.h>
00014 #include <geometry_msgs/Pose.h>
00015 #include <geometry_msgs/Wrench.h>
00016 #include <geometry_msgs/Twist.h>
00017 #include <std_msgs/Int32.h>
00018 
00019 #include <tf_conversions/tf_kdl.h>
00020 
00021 #include <Eigen/Dense>
00022 
00023 #include <vector>
00024 
00025 #include <sys/socket.h>
00026 #include <arpa/inet.h>
00027 #include <netinet/in.h>
00028 #include <sys/types.h>
00029 #include <errno.h>
00030 #include <fcntl.h>
00031 #include <arpa/inet.h>
00032 
00033 #ifndef HAVE_RTNET
00034 
00035 #define rt_dev_socket socket
00036 #define rt_dev_setsockopt setsockopt
00037 #define rt_dev_bind bind
00038 #define rt_dev_recvfrom recvfrom
00039 #define rt_dev_sendto sendto
00040 #define rt_dev_close close
00041 
00042 #else
00043 #include <rtdm/rtdm.h>
00044 #endif
00045 
00046 typedef Eigen::Matrix<double, 7, 7> Matrix77d;
00047 // End of user code
00048 
00049 class FRIComponent : public RTT::TaskContext {
00050 public:
00051   FRIComponent(const std::string & name) : TaskContext(name) {
00052 
00053     prop_fri_port = 49938;
00054         
00055     this->addProperty("fri_port", prop_fri_port);
00056 
00057     this->ports()->addPort("CartesianImpedanceCommand", port_CartesianImpedanceCommand).doc("");
00058     this->ports()->addPort("CartesianWrenchCommand", port_CartesianWrenchCommand).doc("");
00059     this->ports()->addPort("CartesianPositionCommand", port_CartesianPositionCommand).doc("");
00060     this->ports()->addPort("JointImpedanceCommand", port_JointImpedanceCommand).doc("");
00061     this->ports()->addPort("JointPositionCommand", port_JointPositionCommand).doc("");
00062     this->ports()->addPort("JointTorqueCommand", port_JointTorqueCommand).doc("");
00063     this->ports()->addPort("KRL_CMD", port_KRL_CMD).doc("");
00064 
00065     this->ports()->addPort("CartesianWrench", port_CartesianWrench).doc("");
00066     this->ports()->addPort("RobotState", port_RobotState).doc("");
00067     this->ports()->addPort("FRIState", port_FRIState).doc("");
00068     this->ports()->addPort("JointVelocity", port_JointVelocity).doc("");
00069     this->ports()->addPort("CartesianVelocity", port_CartesianVelocity).doc("");
00070     this->ports()->addPort("CartesianPosition", port_CartesianPosition).doc("");
00071     this->ports()->addPort("MassMatrix", port_MassMatrix).doc("");
00072     this->ports()->addPort("Jacobian", port_Jacobian).doc("");
00073     this->ports()->addPort("JointTorque", port_JointTorque).doc("");
00074     this->ports()->addPort("GravityTorque", port_GravityTorque);
00075     this->ports()->addPort("JointPosition", port_JointPosition).doc("");
00076   }
00077 
00078   ~FRIComponent(){
00079   }
00080 
00081   bool configureHook() {
00082     // Start of user code configureHook
00083     jnt_pos_.resize(LBR_MNJ);
00084     jnt_pos_old_.resize(LBR_MNJ);
00085     jnt_vel_.resize(LBR_MNJ);
00086     jnt_trq_.resize(LBR_MNJ);
00087     grav_trq_.resize(LBR_MNJ);
00088     jnt_pos_cmd_.resize(LBR_MNJ);
00089     jnt_trq_cmd_.resize(LBR_MNJ);
00090     jac_.resize(LBR_MNJ);
00091 
00092     port_JointPosition.setDataSample(jnt_pos_);
00093     port_JointVelocity.setDataSample(jnt_vel_);
00094     port_JointTorque.setDataSample(jnt_trq_);
00095     port_GravityTorque.setDataSample(grav_trq_);
00096     port_Jacobian.setDataSample(jac_);
00097 
00098     if (fri_create_socket() != 0)
00099       return false;
00100     // End of user code
00101     return true;
00102   }
00103 
00104   bool startHook() {
00105     // Start of user code startHook
00106     // End of user code
00107     return true;
00108   }
00109 
00110   void stopHook() {
00111     // Start of user code stopHook
00112     // End of user code
00113   }
00114 
00115   void updateHook() {
00116     if( true) {
00117       doComm();
00118     }
00119   }
00120 
00121 private:
00122 
00123   void doComm() {
00124     // Start of user code Comm
00125 
00126     geometry_msgs::Pose cart_pos, cart_pos_cmd;
00127     geometry_msgs::Wrench cart_wrench, cart_wrench_cmd;
00128     geometry_msgs::Twist cart_twist;
00129     Matrix77d mass;
00130     lwr_fri::FriJointImpedance jnt_imp_cmd;
00131     lwr_fri::CartesianImpedance cart_imp_cmd;
00132 
00133     //Read:
00134     if (fri_recv() == 0) {
00135 
00136       KDL::Frame baseFrame(
00137           KDL::Rotation::RPY(m_msr_data.krl.realData[3] * M_PI / 180.0,
00138               m_msr_data.krl.realData[4] * M_PI / 180.0,
00139               m_msr_data.krl.realData[5] * M_PI / 180.0),
00140           KDL::Vector(m_msr_data.krl.realData[0] / 1000.0,
00141               m_msr_data.krl.realData[1] / 1000.0,
00142               m_msr_data.krl.realData[2] / 1000.0));
00143 
00144       // Fill in fri_joint_state and joint_state
00145       for (unsigned int i = 0; i < LBR_MNJ; i++) {
00146         grav_trq_[i] = m_msr_data.data.gravity[i];
00147         jnt_trq_[i] = m_msr_data.data.estExtJntTrq[i];
00148         jnt_pos_[i] = m_msr_data.data.msrJntPos[i];
00149         jnt_vel_[i] = (jnt_pos_[i] - jnt_pos_old_[i]) / m_msr_data.intf.desiredMsrSampleTime;
00150         jnt_pos_old_[i] = jnt_pos_[i];
00151       }
00152 
00153       geometry_msgs::Quaternion quat;
00154       KDL::Frame cartPos;
00155       cartPos.M = KDL::Rotation(m_msr_data.data.msrCartPos[0],
00156           m_msr_data.data.msrCartPos[1], m_msr_data.data.msrCartPos[2],
00157           m_msr_data.data.msrCartPos[4], m_msr_data.data.msrCartPos[5],
00158           m_msr_data.data.msrCartPos[6], m_msr_data.data.msrCartPos[8],
00159           m_msr_data.data.msrCartPos[9], m_msr_data.data.msrCartPos[10]);
00160       cartPos.p.x(m_msr_data.data.msrCartPos[3]);
00161       cartPos.p.y(m_msr_data.data.msrCartPos[7]);
00162       cartPos.p.z(m_msr_data.data.msrCartPos[11]);
00163       cartPos = baseFrame * cartPos;
00164       tf::PoseKDLToMsg(cartPos, cart_pos);
00165 
00166       KDL::Twist v = KDL::diff(T_old, cartPos, m_msr_data.intf.desiredMsrSampleTime);
00167       v = cartPos.M.Inverse() * v;
00168       T_old = cartPos;
00169       tf::TwistKDLToMsg(v, cart_twist);
00170 
00171       cart_wrench.force.x = m_msr_data.data.estExtTcpFT[0];
00172       cart_wrench.force.y = m_msr_data.data.estExtTcpFT[1];
00173       cart_wrench.force.z = m_msr_data.data.estExtTcpFT[2];
00174       cart_wrench.torque.x = m_msr_data.data.estExtTcpFT[5];
00175       cart_wrench.torque.y = m_msr_data.data.estExtTcpFT[4];
00176       cart_wrench.torque.z = m_msr_data.data.estExtTcpFT[3];
00177 
00178       for (int i = 0; i < FRI_CART_VEC; i++)
00179         for (int j = 0; j < LBR_MNJ; j++)
00180           jac_(i, j) = m_msr_data.data.jacobian[i * LBR_MNJ + j];
00181       //Kuka uses Tx, Ty, Tz, Rz, Ry, Rx convention, so we need to swap Rz and Rx
00182       jac_.data.row(3).swap(jac_.data.row(5));
00183 
00184       for (unsigned int i = 0; i < LBR_MNJ; i++) {
00185         for (unsigned int j = 0; j < LBR_MNJ; j++) {
00186           mass(i, j) = m_msr_data.data.massMatrix[LBR_MNJ * i + j];
00187         }
00188       }
00189 
00190       //Fill in datagram to send:
00191       m_cmd_data.head.datagramId = FRI_DATAGRAM_ID_CMD;
00192       m_cmd_data.head.packetSize = sizeof(tFriCmdData);
00193       m_cmd_data.head.sendSeqCount = ++counter;
00194       m_cmd_data.head.reflSeqCount = m_msr_data.head.sendSeqCount;
00195 
00196       //Process KRL CMD
00197 
00198       if (!(m_msr_data.krl.boolData & (1 << 0))) {
00199         std_msgs::Int32 x;
00200         if (port_KRL_CMD.read(x) == RTT::NewData) {
00201           m_cmd_data.krl.intData[0] = x.data;
00202           m_cmd_data.krl.boolData |= (1 << 0);
00203         }
00204       } else {
00205         m_cmd_data.krl.boolData &= ~(1 << 0);
00206       }
00207 
00208       if (!isPowerOn()) {
00209         // necessary to write cmd if not powered on. See kuka FRI user manual p6 and friremote.cpp:
00210         for (int i = 0; i < LBR_MNJ; i++) {
00211           m_cmd_data.cmd.jntPos[i] = m_msr_data.data.cmdJntPos[i]
00212               + m_msr_data.data.cmdJntPosFriOffset[i];
00213         }
00214       }
00215       if (m_msr_data.intf.state == FRI_STATE_MON || !isPowerOn()) {
00216         // joint position control capable modes:
00217         if (m_msr_data.robot.control == FRI_CTRL_POSITION
00218             || m_msr_data.robot.control == FRI_CTRL_JNT_IMP) {
00219           m_cmd_data.cmd.cmdFlags = FRI_CMD_JNTPOS;
00220           for (unsigned int i = 0; i < LBR_MNJ; i++) {
00221             // see note above with !isPowerOn()
00222             // the user manual speaks of 'mimic msr.data.msrCmdJntPos' which is ambiguous.
00223             // on the other hand, the friremote.cpp will send this whenever (!isPowerOn() || state != FRI_STATE_CMD)
00224             // so we mimic the kuka reference code here...
00225             m_cmd_data.cmd.jntPos[i] = m_msr_data.data.cmdJntPos[i]
00226                 + m_msr_data.data.cmdJntPosFriOffset[i];
00227           }
00228         }
00229         // Additional flags are set in joint impedance mode:
00230         if (m_msr_data.robot.control == FRI_CTRL_JNT_IMP) {
00231           m_cmd_data.cmd.cmdFlags |= FRI_CMD_JNTTRQ;
00232           m_cmd_data.cmd.cmdFlags |= FRI_CMD_JNTSTIFF | FRI_CMD_JNTDAMP;
00233           for (unsigned int i = 0; i < LBR_MNJ; i++) {
00234             m_cmd_data.cmd.addJntTrq[i] = 0.0;
00235             m_cmd_data.cmd.jntStiffness[i] = 0;
00236             m_cmd_data.cmd.jntDamping[i] = 0.7;
00237           }
00238         }
00239         if (m_msr_data.robot.control == FRI_CTRL_CART_IMP) {
00240           m_cmd_data.cmd.cmdFlags = FRI_CMD_CARTPOS | FRI_CMD_TCPFT;
00241           m_cmd_data.cmd.cmdFlags |= FRI_CMD_CARTSTIFF | FRI_CMD_CARTDAMP;
00242           for (unsigned int i = 0; i < FRI_CART_FRM_DIM; i++)
00243             m_cmd_data.cmd.cartPos[i] = m_msr_data.data.msrCartPos[i];
00244           for (unsigned int i = 0; i < FRI_CART_VEC; i++)
00245             m_cmd_data.cmd.addTcpFT[i] = 0.0;
00246           for (unsigned int i = 0; i < FRI_CART_VEC / 2; i++) {
00247             //Linear part;
00248             m_cmd_data.cmd.cartStiffness[i] = 0;
00249             m_cmd_data.cmd.cartDamping[i] = 0.7;
00250             //rotational part;
00251             m_cmd_data.cmd.cartStiffness[i + FRI_CART_VEC / 2] = 0;
00252             m_cmd_data.cmd.cartDamping[i + FRI_CART_VEC / 2] = 0.7;
00253           }
00254         }
00255       }
00256       //Only send if state is in FRI_STATE_CMD and drives are powerd
00257       if ((m_msr_data.intf.state == FRI_STATE_CMD) && isPowerOn()) {
00258         //Valid ports in joint position and joint impedance mode
00259         if (m_msr_data.robot.control == FRI_CTRL_POSITION
00260             || m_msr_data.robot.control == FRI_CTRL_JNT_IMP) {
00261           //Read desired positions
00262           if (port_JointPositionCommand.read(jnt_pos_cmd_) == RTT::NewData) {
00263             if (jnt_pos_cmd_.size() == LBR_MNJ) {
00264               for (unsigned int i = 0; i < LBR_MNJ; i++)
00265                 m_cmd_data.cmd.jntPos[i] = jnt_pos_cmd_[i];
00266             } else
00267               RTT::log(RTT::Warning) << "Size of " << port_JointPositionCommand.getName()
00268                   << " not equal to " << LBR_MNJ << RTT::endlog();
00269 
00270           }
00271         }
00272         //Valid ports only in joint impedance mode
00273         if (m_msr_data.robot.control == FRI_CTRL_JNT_IMP) {
00274           //Read desired additional joint torques
00275           if (port_JointTorqueCommand.read(jnt_trq_cmd_)
00276               == RTT::NewData) {
00277             //Check size
00278             if (jnt_trq_cmd_.size() == LBR_MNJ) {
00279               for (unsigned int i = 0; i < LBR_MNJ; i++)
00280                 m_cmd_data.cmd.addJntTrq[i] = jnt_trq_cmd_[i];
00281             } else
00282               RTT::log(RTT::Warning) << "Size of " << port_JointTorqueCommand.getName()
00283                   << " not equal to " << LBR_MNJ << RTT::endlog();
00284 
00285           }
00286           //Read desired joint impedance
00287           if (port_JointImpedanceCommand.read(jnt_imp_cmd) == RTT::NewData) {
00288             for (unsigned int i = 0; i < LBR_MNJ; i++) {
00289               m_cmd_data.cmd.jntStiffness[i] =
00290                   jnt_imp_cmd.stiffness[i];
00291               m_cmd_data.cmd.jntDamping[i] = jnt_imp_cmd.damping[i];
00292             }
00293           }
00294         } else if (m_msr_data.robot.control == FRI_CTRL_CART_IMP) {
00295           if (port_CartesianPositionCommand.read(cart_pos_cmd) == RTT::NewData) {
00296             KDL::Rotation rot = KDL::Rotation::Quaternion(
00297                 cart_pos_cmd.orientation.x, cart_pos_cmd.orientation.y,
00298                 cart_pos_cmd.orientation.z, cart_pos_cmd.orientation.w);
00299             m_cmd_data.cmd.cartPos[0] = rot.data[0];
00300             m_cmd_data.cmd.cartPos[1] = rot.data[1];
00301             m_cmd_data.cmd.cartPos[2] = rot.data[2];
00302             m_cmd_data.cmd.cartPos[4] = rot.data[3];
00303             m_cmd_data.cmd.cartPos[5] = rot.data[4];
00304             m_cmd_data.cmd.cartPos[6] = rot.data[5];
00305             m_cmd_data.cmd.cartPos[8] = rot.data[6];
00306             m_cmd_data.cmd.cartPos[9] = rot.data[7];
00307             m_cmd_data.cmd.cartPos[10] = rot.data[8];
00308 
00309             m_cmd_data.cmd.cartPos[3] = cart_pos_cmd.position.x;
00310             m_cmd_data.cmd.cartPos[7] = cart_pos_cmd.position.y;
00311             m_cmd_data.cmd.cartPos[11] = cart_pos_cmd.position.z;
00312           }
00313 
00314           if (port_CartesianWrenchCommand.read(cart_wrench_cmd) == RTT::NewData) {
00315             m_cmd_data.cmd.addTcpFT[0] = cart_wrench_cmd.force.x;
00316             m_cmd_data.cmd.addTcpFT[1] = cart_wrench_cmd.force.y;
00317             m_cmd_data.cmd.addTcpFT[2] = cart_wrench_cmd.force.z;
00318             m_cmd_data.cmd.addTcpFT[3] = cart_wrench_cmd.torque.z;
00319             m_cmd_data.cmd.addTcpFT[4] = cart_wrench_cmd.torque.y;
00320             m_cmd_data.cmd.addTcpFT[5] = cart_wrench_cmd.torque.x;
00321           }
00322 
00323           if (port_CartesianImpedanceCommand.read(cart_imp_cmd) == RTT::NewData) {
00324             m_cmd_data.cmd.cartStiffness[0] = cart_imp_cmd.stiffness.linear.x;
00325             m_cmd_data.cmd.cartStiffness[1] = cart_imp_cmd.stiffness.linear.y;
00326             m_cmd_data.cmd.cartStiffness[2] = cart_imp_cmd.stiffness.linear.z;
00327             m_cmd_data.cmd.cartStiffness[5] = cart_imp_cmd.stiffness.angular.x;
00328             m_cmd_data.cmd.cartStiffness[4] = cart_imp_cmd.stiffness.angular.y;
00329             m_cmd_data.cmd.cartStiffness[3] = cart_imp_cmd.stiffness.angular.z;
00330             m_cmd_data.cmd.cartDamping[0] = cart_imp_cmd.damping.linear.x;
00331             m_cmd_data.cmd.cartDamping[1] = cart_imp_cmd.damping.linear.y;
00332             m_cmd_data.cmd.cartDamping[2] = cart_imp_cmd.damping.linear.z;
00333             m_cmd_data.cmd.cartDamping[5] = cart_imp_cmd.damping.angular.x;
00334             m_cmd_data.cmd.cartDamping[4] = cart_imp_cmd.damping.angular.y;
00335             m_cmd_data.cmd.cartDamping[3] = cart_imp_cmd.damping.angular.z;
00336           }
00337         } else if (m_msr_data.robot.control == FRI_CTRL_OTHER) {
00338           this->error();
00339         }
00340       }                                         //End command mode
00341 
00342       //Put robot and fri state on the ports(no parsing)
00343       port_RobotState.write(m_msr_data.robot);
00344       port_FRIState.write(m_msr_data.intf);
00345 
00346       port_JointPosition.write(jnt_pos_);
00347       port_JointVelocity.write(jnt_vel_);
00348       port_JointTorque.write(jnt_trq_);
00349       port_GravityTorque.write(grav_trq_);
00350 
00351       port_CartesianPosition.write(cart_pos);
00352       port_CartesianVelocity.write(cart_twist);
00353       port_CartesianWrench.write(cart_wrench);
00354 
00355       port_Jacobian.write(jac_);
00356       port_MassMatrix.write(mass);
00357 
00358       fri_send();
00359     }
00360 
00361     this->trigger();
00362     // End of user code
00363   }
00364 
00365 
00366   RTT::InputPort<lwr_fri::CartesianImpedance > port_CartesianImpedanceCommand;
00367   RTT::InputPort<geometry_msgs::Wrench > port_CartesianWrenchCommand;
00368   RTT::InputPort<geometry_msgs::Pose > port_CartesianPositionCommand;
00369   RTT::InputPort<lwr_fri::FriJointImpedance > port_JointImpedanceCommand;
00370   RTT::InputPort<Eigen::VectorXd > port_JointPositionCommand;
00371   RTT::InputPort<Eigen::VectorXd > port_JointTorqueCommand;
00372   RTT::InputPort<std_msgs::Int32 > port_KRL_CMD;
00373 
00374   RTT::OutputPort<geometry_msgs::Wrench > port_CartesianWrench;
00375   RTT::OutputPort<tFriRobotState > port_RobotState;
00376   RTT::OutputPort<tFriIntfState > port_FRIState;
00377   RTT::OutputPort<Eigen::VectorXd > port_JointVelocity;
00378   RTT::OutputPort<geometry_msgs::Twist > port_CartesianVelocity;
00379   RTT::OutputPort<geometry_msgs::Pose > port_CartesianPosition;
00380   RTT::OutputPort<Matrix77d > port_MassMatrix;
00381   RTT::OutputPort<KDL::Jacobian > port_Jacobian;
00382   RTT::OutputPort<Eigen::VectorXd > port_JointTorque;
00383   RTT::OutputPort<Eigen::VectorXd > port_GravityTorque;
00384   RTT::OutputPort<Eigen::VectorXd > port_JointPosition;
00385 
00386   int prop_fri_port;
00387 
00388 
00389   // Start of user code userData
00390   Eigen::VectorXd jnt_pos_;
00391   Eigen::VectorXd jnt_pos_old_;
00392   Eigen::VectorXd jnt_trq_;
00393   Eigen::VectorXd grav_trq_;
00394   Eigen::VectorXd jnt_vel_;
00395 
00396   Eigen::VectorXd jnt_pos_cmd_;
00397   Eigen::VectorXd jnt_trq_cmd_;
00398 
00399   KDL::Jacobian jac_;
00400 
00401   KDL::Frame T_old;
00402 
00403   int m_socket, m_remote_port, m_control_mode;
00404   std::string joint_names_prefix;
00405   uint16_t counter, fri_state_last;
00406   struct sockaddr_in m_remote_addr;
00407   socklen_t m_sock_addr_len;
00408 
00409   tFriMsrData m_msr_data;
00410   tFriCmdData m_cmd_data;
00411 
00412   int fri_recv() {
00413     int n = rt_dev_recvfrom(m_socket, (void*) &m_msr_data, sizeof(m_msr_data),
00414         0, (sockaddr*) &m_remote_addr, &m_sock_addr_len);
00415     if (sizeof(tFriMsrData) != n) {
00416       RTT::log(RTT::Error) << "bad packet length: " << n << ", expected: "
00417           << sizeof(tFriMsrData) << RTT::endlog();
00418       return -1;
00419     }
00420     return 0;
00421   }
00422 
00423   int fri_send() {
00424     if (0
00425         > rt_dev_sendto(m_socket, (void*) &m_cmd_data, sizeof(m_cmd_data), 0,
00426             (sockaddr*) &m_remote_addr, m_sock_addr_len)) {
00427       RTT::log(RTT::Error) << "Sending datagram failed."
00428           << ntohs(m_remote_addr.sin_port) << RTT::endlog();
00429       return -1;
00430     }
00431     return 0;
00432   }
00433 
00434   bool isPowerOn() { return m_msr_data.robot.power!=0; }
00435 
00436   int fri_create_socket() {
00437 
00438     if (m_socket != 0)
00439       rt_dev_close(m_socket);
00440     m_socket = rt_dev_socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
00441     rt_dev_setsockopt(m_socket, SOL_SOCKET, SO_REUSEADDR, 0, 0);
00442 
00443     struct sockaddr_in local_addr;
00444     bzero((char *) &local_addr, sizeof(local_addr));
00445     local_addr.sin_family = AF_INET;
00446     local_addr.sin_addr.s_addr = INADDR_ANY;
00447     local_addr.sin_port = htons(prop_fri_port);
00448 
00449     if (rt_dev_bind(m_socket, (sockaddr*) &local_addr, sizeof(sockaddr_in)) < 0) {
00450       RTT::log(RTT::Error) << "Binding of port failed with errno " << errno << RTT::endlog();
00451       return -1;
00452     }
00453 
00454     return 0;
00455   }
00456   // End of user code
00457 
00458 };
00459 
00460 ORO_CREATE_COMPONENT(FRIComponent)
00461

lwr_fri
Author(s): Konrad Banachowicz
autogenerated on Mon Oct 6 2014 01:58:05