SimulatedKatana.cpp
Go to the documentation of this file.
00001 /*
00002  * UOS-ROS packages - Robot Operating System code by the University of Osnabrück
00003  * Copyright (C) 2010  University of Osnabrück
00004  *
00005  * This program is free software; you can redistribute it and/or
00006  * modify it under the terms of the GNU General Public License
00007  * as published by the Free Software Foundation; either version 2
00008  * of the License, or (at your option) any later version.
00009  *
00010  * This program is distributed in the hope that it will be useful,
00011  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00012  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00013  * GNU General Public License for more details.
00014  *
00015  * You should have received a copy of the GNU General Public License
00016  * along with this program; if not, write to the Free Software
00017  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
00018  *
00019  * SimulatedKatana.cpp
00020  *
00021  *  Created on: 20.12.2010
00022  *      Author: Martin Günther <mguenthe@uos.de>
00023  */
00024 
00025 #include "../include/katana/SimulatedKatana.h"
00026 
00027 namespace katana
00028 {
00029 
00030 SimulatedKatana::SimulatedKatana() :
00031   AbstractKatana()
00032 {
00033   // Creates a "hold current position" trajectory.
00034   boost::shared_ptr<SpecifiedTrajectory> hold_ptr(new SpecifiedTrajectory(1));
00035   SpecifiedTrajectory &hold = *hold_ptr;
00036   hold[0].start_time = ros::Time::now().toSec() - 0.001;
00037   hold[0].duration = 0.0;
00038   hold[0].splines.resize(NUM_MOTORS);
00039 
00040   hold[0].splines[0].coef[0] = -3.022;
00041   hold[0].splines[1].coef[0] = 2.163;
00042   hold[0].splines[2].coef[0] = -2.207;
00043   hold[0].splines[3].coef[0] = -2.026;
00044   hold[0].splines[4].coef[0] = -2.990;
00045   hold[0].splines[5].coef[0] = GRIPPER_OPEN_ANGLE;
00046 
00047   current_trajectory_ = hold_ptr;
00048 }
00049 
00050 SimulatedKatana::~SimulatedKatana()
00051 {
00052 }
00053 
00054 void SimulatedKatana::refreshEncoders()
00055 {
00056   const SpecifiedTrajectory &traj = *current_trajectory_;
00057 
00058   // Determines which segment of the trajectory to use
00059   size_t seg = 0;
00060   while (seg + 1 < traj.size() && traj[seg + 1].start_time <= ros::Time::now().toSec())
00061   {
00062     seg++;
00063   }
00064 
00065   for (size_t j = 0; j < traj[seg].splines.size(); j++)
00066   {
00067     double pos_t, vel_t, acc_t;
00068     sampleSplineWithTimeBounds(traj[seg].splines[j].coef, traj[seg].duration, ros::Time::now().toSec()
00069         - traj[seg].start_time, pos_t, vel_t, acc_t);
00070 
00071     motor_angles_[j] = pos_t;
00072     motor_velocities_[j] = vel_t;
00073   }
00074 }
00075 
00076 bool SimulatedKatana::executeTrajectory(boost::shared_ptr<SpecifiedTrajectory> traj_ptr, boost::function<bool ()> isPreemptRequested)
00077 {
00078   // ------- wait until start time
00079   ros::Time::sleepUntil(ros::Time(traj_ptr->at(0).start_time));
00080 
00081   current_trajectory_ = traj_ptr;
00082   return true;
00083 }
00084 
00085 void SimulatedKatana::moveGripper(double openingAngle)
00086 {
00087   static const double DURATION = 2.877065;   // time to open/close gripper, measured on real Katana
00088 
00089   if (openingAngle < GRIPPER_CLOSED_ANGLE || GRIPPER_OPEN_ANGLE < openingAngle)
00090   {
00091     ROS_ERROR("Desired opening angle %f is out of range [%f, %f]", openingAngle, GRIPPER_CLOSED_ANGLE, GRIPPER_OPEN_ANGLE);
00092     return;
00093   }
00094 
00095   // create a new trajectory
00096   boost::shared_ptr<SpecifiedTrajectory> new_traj_ptr(new SpecifiedTrajectory(1));
00097   SpecifiedTrajectory &new_traj = *new_traj_ptr;
00098   new_traj[0].start_time = ros::Time::now().toSec();
00099   new_traj[0].duration = DURATION;
00100   new_traj[0].splines.resize(NUM_MOTORS);
00101 
00102   // hold all joints at their current position
00103   for (size_t j = 0; j < NUM_MOTORS; ++j)
00104     new_traj[0].splines[j].coef[0] = motor_angles_[j];
00105 
00106   // move the gripper to the desired angle
00107   new_traj[0].splines[GRIPPER_INDEX].target_position = openingAngle;
00108   getCubicSplineCoefficients(motor_angles_[GRIPPER_INDEX], 0.0, openingAngle, 0.0, DURATION,
00109                              new_traj[0].splines[GRIPPER_INDEX].coef);
00110 
00111   current_trajectory_ = new_traj_ptr;
00112   return;
00113 }
00114 
00115 bool SimulatedKatana::moveJoint(int jointIndex, double turningAngle){
00116   ROS_ERROR("moveJoint() not yet implemented for SimulatedKatana!");
00117   return false;
00118 }
00119 
00120 bool SimulatedKatana::someMotorCrashed() {
00121   return false;
00122 }
00123 
00124 bool SimulatedKatana::allJointsReady() {
00125   return true;
00126 }
00127 
00128 bool SimulatedKatana::allMotorsReady() {
00129   return true;
00130 }
00131 
00132 
00133 }


katana
Author(s): Martin Günther
autogenerated on Thu Aug 27 2015 13:40:35