00001
00002 #include <rtt/TaskContext.hpp>
00003 #include <rtt/Port.hpp>
00004 #include <rtt/Component.hpp>
00005
00006
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
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
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
00098 return true;
00099 }
00100
00101 bool startHook() {
00102
00103
00104 return true;
00105 }
00106
00107 void stopHook() {
00108
00109
00110 }
00111
00112 void updateHook() {
00113 if( true) {
00114 doComm();
00115 }
00116 }
00117
00118 private:
00119
00120 void doComm() {
00121
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
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
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
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
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
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
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
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
00218
00219
00220
00221 m_cmd_data.cmd.jntPos[i] = m_msr_data.data.cmdJntPos[i]
00222 + m_msr_data.data.cmdJntPosFriOffset[i];
00223 }
00224 }
00225
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
00244 m_cmd_data.cmd.cartStiffness[i] = 1000;
00245 m_cmd_data.cmd.cartDamping[i] = 0.7;
00246
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
00253 if ((m_msr_data.intf.state == FRI_STATE_CMD) && isPowerOn()) {
00254
00255 if (m_msr_data.robot.control == FRI_CTRL_POSITION
00256 || m_msr_data.robot.control == FRI_CTRL_JNT_IMP) {
00257
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
00269 if (m_msr_data.robot.control == FRI_CTRL_JNT_IMP) {
00270
00271 if (port_JointTorqueCommand.read(jnt_trq_cmd_)
00272 == RTT::NewData) {
00273
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
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 }
00337
00338
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
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
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
00450
00451 };
00452
00453 ORO_CREATE_COMPONENT(FRIComponent)
00454