test_contr.cpp
Go to the documentation of this file.
00001 #include "kelsey_sandbox/coll_detect_contr.h"
00002 #include <pluginlib/class_list_macros.h>
00003 
00004 namespace cd_controller {
00005 
00007     bool CollisionDetection::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n) {
00008         rosrt::init();
00009         node_ = n;
00010         robot_state_ = robot;
00011 
00012         std_msgs::Float64 float_temp;
00013         pub_val_.initialize(n, "/blah", 10, 10, float_temp);
00014 
00015         if(!chain_.init(robot, "torso_lift_link", "r_gripper_palm_link"))
00016             return false;
00017 
00018         chain_.toKDL(kdl_chain_);
00019         kin_.reset(new Kin<Joints>(kdl_chain_));
00020         loop_count = 0;
00021 
00022         joint_vel_filter_ = 0.2;
00023         
00024         return true;
00025     }
00026 
00027 
00029     void CollisionDetection::starting() {
00030         qdot_filtered_.setZero();
00031         cd_filter_ = 0.0;
00032         cd_integral_ = 0.0;
00033         last_cycle_time_ = robot_state_->getTime();
00034     }
00035 
00036 
00038     void CollisionDetection::update() {
00039         JointVec q;
00040         chain_.getPositions(q);
00041 
00042         JointVec qdot_raw;
00043         chain_.getVelocities(qdot_raw);
00044         for (int i = 0; i < Joints; ++i)
00045             qdot_filtered_[i] += joint_vel_filter_ * (qdot_raw[i] - qdot_filtered_[i]);
00046         JointVec qdot = qdot_filtered_;
00047         qdot = qdot_raw;
00048 
00049         KDL::JntArray kdl_tau;
00050         kdl_tau.resize(Joints);
00051         chain_.getEfforts(kdl_tau);
00052         JointVec tau;
00053         for(int i=0;i<Joints;i++)
00054             tau(i) = kdl_tau(i);
00055 
00056 
00057         InertiaMat M;
00058         kin_->inertia(q, M);
00059 
00060         ros::Duration dt = robot_state_->getTime() - last_cycle_time_;
00061         last_cycle_time_ = robot_state_->getTime();
00062         cd_integral_ += dt.toSec() * (qdot.transpose() * tau + cd_filter_);
00063         double E = 0.5 * qdot.transpose() * M * qdot + 0.1 * qdot.norm();
00064         cd_filter_ = 50.0 * (E - cd_integral_);
00065 
00066         if(loop_count % 10 == 0) {
00067             std_msgs::Float64::Ptr val_msg;
00068             if (val_msg = pub_val_.allocate()) {  
00069                 val_msg->data = cd_filter_;
00070                 pub_val_.publish(val_msg);
00071             }
00072         }
00073         loop_count++;
00074     }
00075 
00076 
00078     void CollisionDetection::stopping() {}
00079 } // namespace
00080 
00082 PLUGINLIB_DECLARE_CLASS(kelsey_sandbox,CollDetect, 
00083                         cd_controller::CollisionDetection, 
00084                         pr2_controller_interface::Controller)


kelsey_sandbox
Author(s): kelsey
autogenerated on Wed Nov 27 2013 11:52:04