00001 /* 00002 * SpringCoupler.cpp 00003 * 00004 * Created on: 31-08-2011 00005 * Author: konradb3 00006 */ 00007 00008 #include <rtt/Component.hpp> 00009 00010 #include "ForceController.h" 00011 #include <tf_conversions/tf_kdl.h> 00012 #include <Eigen/Dense> 00013 00014 namespace lwr { 00015 00016 ForceController::ForceController(const std::string& name) : TaskContext(name, PreOperational) { 00017 // command inputs 00018 00019 this->addPort("CartesianVelocityCommand", port_cartesian_velocity_command); 00020 this->addEventPort("CartesianWrench", port_cartesian_wrench); 00021 00022 } 00023 00024 ForceController::~ForceController() { 00025 00026 } 00027 00028 bool ForceController::configureHook() { 00029 return true; 00030 } 00031 00032 void ForceController::cleanupHook() { 00033 00034 } 00035 00036 bool ForceController::startHook() { 00037 return true; 00038 } 00039 00040 void ForceController::stopHook() { 00041 00042 } 00043 00044 void ForceController::updateHook() { 00045 00046 port_cartesian_wrench.read(cartesian_wrench); 00047 00048 cartesian_twist.linear.x = 0.0; // -(1.0/100.0) * cartesian_wrench.force.x; 00049 cartesian_twist.linear.y = 0.0; // -(1.0/100.0) * cartesian_wrench.force.y; 00050 cartesian_twist.linear.z = -(1.0/60.0) * cartesian_wrench.force.z; 00051 00052 cartesian_twist.angular.x = 0.0; //-(1.0/5.0) * cartesian_wrench.torque.x; 00053 cartesian_twist.angular.y = 0.0; -(1.0/5.0) * cartesian_wrench.torque.y; 00054 cartesian_twist.angular.z = 0.0; -(1.0/5.0) * cartesian_wrench.torque.z; 00055 00056 port_cartesian_velocity_command.write(cartesian_twist); 00057 } 00058 00059 } 00060 ORO_CREATE_COMPONENT(lwr::ForceController)