SimulatedKatana.cpp
Go to the documentation of this file.
1 /*
2  * UOS-ROS packages - Robot Operating System code by the University of Osnabrück
3  * Copyright (C) 2010 University of Osnabrück
4  *
5  * This program is free software; you can redistribute it and/or
6  * modify it under the terms of the GNU General Public License
7  * as published by the Free Software Foundation; either version 2
8  * of the License, or (at your option) any later version.
9  *
10  * This program is distributed in the hope that it will be useful,
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program; if not, write to the Free Software
17  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
18  *
19  * SimulatedKatana.cpp
20  *
21  * Created on: 20.12.2010
22  * Author: Martin Günther <mguenthe@uos.de>
23  */
24 
25 #include "../include/katana/SimulatedKatana.h"
26 
27 namespace katana
28 {
29 
32 {
33  // Creates a "hold current position" trajectory.
35  SpecifiedTrajectory &hold = *hold_ptr;
36  hold[0].start_time = ros::Time::now().toSec() - 0.001;
37  hold[0].duration = 0.0;
38  hold[0].splines.resize(NUM_MOTORS);
39 
40  hold[0].splines[0].coef[0] = -3.022;
41  hold[0].splines[1].coef[0] = 2.163;
42  hold[0].splines[2].coef[0] = -2.207;
43  hold[0].splines[3].coef[0] = -2.026;
44  hold[0].splines[4].coef[0] = -2.990;
45  hold[0].splines[5].coef[0] = GRIPPER_OPEN_ANGLE;
46 
47  current_trajectory_ = hold_ptr;
48 }
49 
51 {
52 }
53 
55 {
57 
58  // Determines which segment of the trajectory to use
59  size_t seg = 0;
60  while (seg + 1 < traj.size() && traj[seg + 1].start_time <= ros::Time::now().toSec())
61  {
62  seg++;
63  }
64 
65  for (size_t j = 0; j < traj[seg].splines.size(); j++)
66  {
67  double pos_t, vel_t, acc_t;
68  sampleSplineWithTimeBounds(traj[seg].splines[j].coef, traj[seg].duration, ros::Time::now().toSec()
69  - traj[seg].start_time, pos_t, vel_t, acc_t);
70 
71  motor_angles_[j] = pos_t;
72  motor_velocities_[j] = vel_t;
73  }
74 }
75 
76 bool SimulatedKatana::executeTrajectory(boost::shared_ptr<SpecifiedTrajectory> traj_ptr, boost::function<bool ()> isPreemptRequested)
77 {
78  // ------- wait until start time
79  ros::Time::sleepUntil(ros::Time(traj_ptr->at(0).start_time));
80 
81  current_trajectory_ = traj_ptr;
82  return true;
83 }
84 
85 void SimulatedKatana::moveGripper(double openingAngle)
86 {
87  static const double DURATION = 2.877065; // time to open/close gripper, measured on real Katana
88 
89  if (openingAngle < GRIPPER_CLOSED_ANGLE || GRIPPER_OPEN_ANGLE < openingAngle)
90  {
91  ROS_ERROR("Desired opening angle %f is out of range [%f, %f]", openingAngle, GRIPPER_CLOSED_ANGLE, GRIPPER_OPEN_ANGLE);
92  return;
93  }
94 
95  // create a new trajectory
97  SpecifiedTrajectory &new_traj = *new_traj_ptr;
98  new_traj[0].start_time = ros::Time::now().toSec();
99  new_traj[0].duration = DURATION;
100  new_traj[0].splines.resize(NUM_MOTORS);
101 
102  // hold all joints at their current position
103  for (size_t j = 0; j < NUM_MOTORS; ++j)
104  new_traj[0].splines[j].coef[0] = motor_angles_[j];
105 
106  // move the gripper to the desired angle
107  new_traj[0].splines[GRIPPER_INDEX].target_position = openingAngle;
108  getCubicSplineCoefficients(motor_angles_[GRIPPER_INDEX], 0.0, openingAngle, 0.0, DURATION,
109  new_traj[0].splines[GRIPPER_INDEX].coef);
110 
111  current_trajectory_ = new_traj_ptr;
112  return;
113 }
114 
115 bool SimulatedKatana::moveJoint(int jointIndex, double turningAngle){
116  ROS_ERROR("moveJoint() not yet implemented for SimulatedKatana!");
117  return false;
118 }
119 
121  return false;
122 }
123 
125  return true;
126 }
127 
129  return true;
130 }
131 
132 
133 }
static bool sleepUntil(const Time &end)
std::vector< double > motor_angles_
const size_t NUM_MOTORS
The number of motors in the katana (= all 5 "real" joints + the gripper)
std::vector< double > motor_velocities_
virtual bool someMotorCrashed()
void getCubicSplineCoefficients(double start_pos, double start_vel, double end_pos, double end_vel, double time, std::vector< double > &coefficients)
std::vector< Segment > SpecifiedTrajectory
boost::shared_ptr< SpecifiedTrajectory > current_trajectory_
virtual bool executeTrajectory(boost::shared_ptr< SpecifiedTrajectory > traj, boost::function< bool()> isPreemptRequested)
static const double GRIPPER_OPEN_ANGLE
Constants for gripper fully open or fully closed (should be equal to the value in the urdf descriptio...
void sampleSplineWithTimeBounds(const std::vector< double > &coefficients, double duration, double time, double &position, double &velocity, double &acceleration)
static Time now()
virtual bool moveJoint(int jointIndex, double turningAngle)
virtual void moveGripper(double openingAngle)
const size_t GRIPPER_INDEX
The motor index of the gripper (used in all vectors – e.g., motor_angles_)
#define ROS_ERROR(...)
virtual void refreshEncoders()
static const double GRIPPER_CLOSED_ANGLE
Constants for gripper fully open or fully closed (should be equal to the value in the urdf descriptio...


katana
Author(s): Martin Günther
autogenerated on Fri Jan 3 2020 04:01:25