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


lwr_fri
Author(s): Ruben Smits
autogenerated on Thu Nov 14 2013 11:55:34