CartImpTrajectory.cpp
Go to the documentation of this file.
00001 /*
00002  * CartImpTrajectory.cpp
00003  *
00004  *  Created on: 31-08-2011
00005  *      Author: konradb3
00006  */
00007 
00008 #include <rtt/Component.hpp>
00009 
00010 #include "CartImpTrajectory.h"
00011 #include <tf_conversions/tf_kdl.h>
00012 
00013 namespace lwr {
00014 
00015 CartImpTrajectory::CartImpTrajectory(const std::string& name) : TaskContext(name, PreOperational) {
00016   this->addPort("TrajectoryCommand", port_cart_imp_trajectory_cmd);
00017   this->addPort("CartesianPosition", port_cart_pos_msr);
00018   this->addPort("DesiredCartesianPosition", port_desired_cartesian_position);
00019   
00020   this->addPort("CartesianImpedanceCommand", port_cartesian_impedance_cmd);
00021   this->addPort("CartesianWrenchCommand", port_cart_wrench_cmd);
00022   this->addPort("CartesianPositionCommand", port_cart_position_cmd);
00023   
00024   this->addPort("ToolFrame", port_tool_frame);
00025 
00026 }
00027 
00028 CartImpTrajectory::~CartImpTrajectory() {
00029 
00030 }
00031 
00032 bool CartImpTrajectory::configureHook() {
00033         return true;
00034 }
00035 
00036 void CartImpTrajectory::cleanupHook() {
00037 
00038 }
00039 
00040 bool CartImpTrajectory::startHook() {
00041 
00042   setpoint_.impedance.stiffness.force.x = 1000;
00043   setpoint_.impedance.stiffness.force.y = 1000;
00044   setpoint_.impedance.stiffness.force.z = 1000;
00045 
00046   setpoint_.impedance.stiffness.torque.x = 100;
00047   setpoint_.impedance.stiffness.torque.y = 100;
00048   setpoint_.impedance.stiffness.torque.z = 100;
00049 
00050   setpoint_.impedance.damping.force.x = 0.7;
00051   setpoint_.impedance.damping.force.y = 0.7;
00052   setpoint_.impedance.damping.force.z = 0.7;
00053 
00054   setpoint_.impedance.damping.torque.x = 0.70;
00055   setpoint_.impedance.damping.torque.y = 0.70;
00056   setpoint_.impedance.damping.torque.z = 0.70;
00057 
00058   setpoint_.wrench.force.x = 0.0;
00059   setpoint_.wrench.force.y = 0.0;
00060   setpoint_.wrench.force.z = 0.0;
00061 
00062   setpoint_.wrench.torque.x = 0.0;
00063   setpoint_.wrench.torque.y = 0.0;
00064   setpoint_.wrench.torque.z = 0.0;
00065 
00066   setpoint_.velocity.linear.x = 0.0;
00067   setpoint_.velocity.linear.y = 0.0;
00068   setpoint_.velocity.linear.z = 0.0;
00069   
00070   setpoint_.velocity.angular.x = 0.0;
00071   setpoint_.velocity.angular.y = 0.0;
00072   setpoint_.velocity.angular.z = 0.0;
00073 
00074   tool_frame.M = KDL::Rotation::RPY(0.0, 0.0, 0.0);
00075   tool_frame.p = KDL::Vector(-0.0, 0.0, -0.0);
00076   
00077   if(port_cart_pos_msr.read(setpoint_.pose) == RTT::NoData) {
00078           return false;
00079         }
00080 
00081   std::cout << "initial pose : [ " << setpoint_.pose.position.x << " " << setpoint_.pose.position.y << " " << setpoint_.pose.position.z << " ]  [ " << setpoint_.pose.orientation.x << " " << setpoint_.pose.orientation.y << " " << setpoint_.pose.orientation.z << " " << setpoint_.pose.orientation.w << " ]" << std::endl;
00082   
00083   geometry_msgs::Pose tool_frame_msg;
00084   
00085   tf::PoseKDLToMsg(tool_frame, tool_frame_msg);
00086   
00087   port_tool_frame.write(tool_frame_msg);
00088   
00089   tf::PoseMsgToKDL(setpoint_.pose, cart_pos_cmd);
00090   
00091   cart_pos_cmd = cart_pos_cmd * tool_frame;
00092   
00093   tf::PoseKDLToMsg(cart_pos_cmd, setpoint_.pose);
00094   
00095   last_point_ = setpoint_;
00096 
00097   valid_trajectory_ = false;
00098   dt_ = 0.001;
00099 
00100   return true;
00101 }
00102 
00103 void CartImpTrajectory::stopHook() {
00104 
00105 }
00106 
00107 void CartImpTrajectory::updateHook() {
00108 
00109   if(port_cart_imp_trajectory_cmd.read(trajectory_tmp_) == RTT::NewData) {
00110     if(trajectory_tmp_.trajectory.size() == 0) {
00111       valid_trajectory_ = false;
00112     } else {
00113       trajectory_ = trajectory_tmp_;
00114       trajectory_index_ = 0;
00115       port_desired_cartesian_position.read(setpoint_.pose);
00116       last_point_ = setpoint_;
00117       last_point_.time_from_start = ros::Duration(0);
00118       time_ = 0;
00119         std::cout << "initial pose : [ " << setpoint_.pose.position.x << " " << setpoint_.pose.position.y << " " << setpoint_.pose.position.z << " ]  [ " << setpoint_.pose.orientation.x << " " << setpoint_.pose.orientation.y << " " << setpoint_.pose.orientation.z << " " << setpoint_.pose.orientation.w << " ]" << std::endl;
00120       valid_trajectory_ = true;
00121     }
00122   }
00123 
00124   if(valid_trajectory_ == true) {
00126     if(trajectory_.trajectory[trajectory_index_].time_from_start.toSec() <= ((double)time_ * dt_)) {
00127       if(trajectory_.trajectory.size() <= (trajectory_index_ + 1)) {
00128        // setpoint_ = trajectory_.trajectory[trajectory_index_];
00129         valid_trajectory_ = false;
00130       } else {
00131         last_point_ = trajectory_.trajectory[trajectory_index_];
00132         ++trajectory_index_;
00133       }
00134     }
00135 
00136     if(valid_trajectory_ == true) {
00137       setpoint_ = sampleInterpolation();
00138     }
00139     
00140     ++time_;
00141   }
00142   
00143   port_cart_position_cmd.write(setpoint_.pose);
00144   port_cartesian_impedance_cmd.write(setpoint_.impedance);
00145   port_cart_wrench_cmd.write(setpoint_.wrench);
00146 }
00147 
00148 double CartImpTrajectory::linearlyInterpolate(double time, 
00149                                                     double startTime, 
00150                                                     double endTime, 
00151                                                     double startValue, 
00152                                                     double endValue) {
00153   return startValue + 
00154     (time - startTime)*
00155     (endValue - startValue)/(endTime - startTime);
00156 }
00157 
00158 lwr_impedance_controller::CartImpTrajectoryPoint 
00159 CartImpTrajectory::sampleInterpolation() {
00160     lwr_impedance_controller::CartImpTrajectoryPoint next_point;
00161 
00162     double timeFromStart = (double)time_ * dt_;
00163     double segStartTime = last_point_.time_from_start.toSec();
00164     double segEndTime = trajectory_.trajectory[trajectory_index_].time_from_start.toSec();
00165     
00166  //std::cout << "initial pose : [ " << trajectory_.trajectory[trajectory_index_].pose.position.x << " " << trajectory_.trajectory[trajectory_index_].pose.position.y << " " << trajectory_.trajectory[trajectory_index_].pose.position.z << " ]  [ " << trajectory_.trajectory[trajectory_index_].pose.orientation.x << " " << trajectory_.trajectory[trajectory_index_].pose.orientation.y << " " << trajectory_.trajectory[trajectory_index_].pose.orientation.z << " " << trajectory_.trajectory[trajectory_index_].pose.orientation.w << " ]" << std::endl;
00167 
00168     next_point = setpoint_;
00169  
00170     // interpolate position
00171     // x
00172     next_point.pose.position.x = linearlyInterpolate
00173       (timeFromStart, segStartTime, segEndTime, 
00174        last_point_.pose.position.x, trajectory_.trajectory[trajectory_index_].pose.position.x);
00175     // y
00176     next_point.pose.position.y = linearlyInterpolate
00177       (timeFromStart, segStartTime, segEndTime, 
00178        last_point_.pose.position.y, trajectory_.trajectory[trajectory_index_].pose.position.y); 
00179     // z
00180     next_point.pose.position.z = linearlyInterpolate
00181       (timeFromStart, segStartTime, segEndTime, 
00182        last_point_.pose.position.z, trajectory_.trajectory[trajectory_index_].pose.position.z); 
00183        
00184     // interpolate orientation
00185     // x
00186     next_point.pose.orientation.x = linearlyInterpolate
00187       (timeFromStart, segStartTime, segEndTime, 
00188        last_point_.pose.orientation.x, trajectory_.trajectory[trajectory_index_].pose.orientation.x); 
00189     // y
00190     next_point.pose.orientation.y = linearlyInterpolate
00191       (timeFromStart, segStartTime, segEndTime, 
00192        last_point_.pose.orientation.y, trajectory_.trajectory[trajectory_index_].pose.orientation.y);
00193     // z
00194     next_point.pose.orientation.z = linearlyInterpolate
00195       (timeFromStart, segStartTime, segEndTime, 
00196        last_point_.pose.orientation.z, trajectory_.trajectory[trajectory_index_].pose.orientation.z); 
00197     // w
00198     next_point.pose.orientation.w = linearlyInterpolate
00199       (timeFromStart, segStartTime, segEndTime, 
00200        last_point_.pose.orientation.w, trajectory_.trajectory[trajectory_index_].pose.orientation.w); 
00201 
00202     //interpolate stiffness
00203     // x
00204     next_point.impedance.stiffness.force.x = linearlyInterpolate
00205       (timeFromStart, segStartTime, segEndTime, 
00206        last_point_.impedance.stiffness.force.x, trajectory_.trajectory[trajectory_index_].impedance.stiffness.force.x); 
00207        
00208     next_point.impedance.stiffness.force.y = linearlyInterpolate
00209       (timeFromStart, segStartTime, segEndTime, 
00210        last_point_.impedance.stiffness.force.y, trajectory_.trajectory[trajectory_index_].impedance.stiffness.force.y); 
00211        
00212     next_point.impedance.stiffness.force.y = linearlyInterpolate
00213       (timeFromStart, segStartTime, segEndTime, 
00214        last_point_.impedance.stiffness.force.z, trajectory_.trajectory[trajectory_index_].impedance.stiffness.force.z);
00215        
00216     next_point.impedance.stiffness.torque.x = linearlyInterpolate
00217       (timeFromStart, segStartTime, segEndTime, 
00218        last_point_.impedance.stiffness.torque.x, trajectory_.trajectory[trajectory_index_].impedance.stiffness.torque.x); 
00219        
00220     next_point.impedance.stiffness.torque.y = linearlyInterpolate
00221       (timeFromStart, segStartTime, segEndTime, 
00222        last_point_.impedance.stiffness.torque.y, trajectory_.trajectory[trajectory_index_].impedance.stiffness.torque.y);
00223        
00224     next_point.impedance.stiffness.torque.z = linearlyInterpolate
00225       (timeFromStart, segStartTime, segEndTime, 
00226        last_point_.impedance.stiffness.torque.z, trajectory_.trajectory[trajectory_index_].impedance.stiffness.torque.z);  
00227 /*
00228     next_point.impedance.damping = trajectory_.trajectory[trajectory_index_].impedance.damping;
00229     next_point.wrench = trajectory_.trajectory[trajectory_index_].wrench;
00230 */
00231  // RTT::Logger::log(RTT::Logger::Error) << "pos : " << next_point.pose.position.x << " y " << next_point.pose.position.y << " z " << next_point.pose.position.z << RTT::endlog();
00232 //  RTT::Logger::log(RTT::Logger::Error) << "stf : " << next_point.impedance.stiffness.force.x << " y " << next_point.impedance.stiffness.force.x << " z " << next_point.impedance.stiffness.force.x << " rx " << next_point.impedance.stiffness.torque.x << " rz " << next_point.impedance.stiffness.torque.x << " rz " << next_point.impedance.stiffness.torque.z << RTT::endlog();
00233 //  RTT::Logger::log(RTT::Logger::Error) << "dmp : " << next_point.impedance.damping.force.x << " y " << next_point.impedance.damping.force.x << " z " << next_point.impedance.damping.force.x << " rx " << next_point.impedance.damping.torque.x << " rz " << next_point.impedance.damping.torque.x << " rz " << next_point.impedance.damping.torque.z << RTT::endlog();
00234   return next_point;
00235 }
00236 
00237 }
00238 ORO_CREATE_COMPONENT(lwr::CartImpTrajectory)


lwr_impedance_controller
Author(s): Konrad Banachowicz
autogenerated on Mon Oct 6 2014 02:01:41