Fawkes API  Fawkes Development Version
arm_dummy.cpp
1 
2 /***************************************************************************
3  * arm_dummy.cpp - Class for a Kinova Jaco arm, simulating a dummy
4  *
5  * Created: Mon Aug 04 19:58:22 2014
6  * Copyright 2014 Bahram Maleki-Fard
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "arm_dummy.h"
24 
25 #include <unistd.h>
26 
27 #define READY_J0 (282.522400)
28 #define READY_J1 (154.470856)
29 #define READY_J2 (44.191490)
30 #define READY_J3 (230.081223)
31 #define READY_J4 (83.242500)
32 #define READY_J5 (77.796173)
33 
34 #define RETRACT_J0 (270.527344)
35 #define RETRACT_J1 (150.205078)
36 #define RETRACT_J2 (25.042963)
37 #define RETRACT_J3 (267.451172)
38 #define RETRACT_J4 (5.800781)
39 #define RETRACT_J5 (99.448242)
40 
41 namespace fawkes {
42 
43 /** @class JacoArmDummy <plugins/jaco/arm_dummy.h>
44  * Class for simulating a dummy Kinova Jaco Arm.
45  * Each command is accepted, simply storing its values and returning them
46  * when a getter is called. This class does not operate any actual arm
47  * (whether a real one nor even a simulated 3D model).
48  *
49  * @author Bahram Maleki-Fard
50  */
51 
52 /** Constructor.
53  * @param name The name of the arm we want to connect to
54  */
55 JacoArmDummy::JacoArmDummy(const char *name)
56 {
57  name_ = name;
58  initialized_ = true;
59 
60  // initialize target vectors for READY and RETRACT positions
61  pos_ready_.push_back(READY_J0);
62  pos_ready_.push_back(READY_J1);
63  pos_ready_.push_back(READY_J2);
64  pos_ready_.push_back(READY_J3);
65  pos_ready_.push_back(READY_J4);
66  pos_ready_.push_back(READY_J5);
67  pos_retract_.push_back(RETRACT_J0);
68  pos_retract_.push_back(RETRACT_J1);
69  pos_retract_.push_back(RETRACT_J2);
70  pos_retract_.push_back(RETRACT_J3);
71  pos_retract_.push_back(RETRACT_J4);
72  pos_retract_.push_back(RETRACT_J5);
73 
74  // initialize position vectors
75  coords_.assign(6, 0.f);
76  joints_.assign(6, 0.f);
77  fingers_.assign(3, 0.f);
78 }
79 
80 /** Destructor. */
82 {
83 }
84 
85 void
87 {
88  goto_ready();
89 }
90 
91 bool
93 {
94  return true;
95 }
96 
97 bool
99 {
100  return initialized_;
101 }
102 
103 void
104 JacoArmDummy::get_coords(std::vector<float> &to)
105 {
106  to = coords_;
107 }
108 
109 void
110 JacoArmDummy::get_joints(std::vector<float> &to) const
111 {
112  to = joints_;
113 }
114 
115 void
116 JacoArmDummy::get_fingers(std::vector<float> &to) const
117 {
118  to = fingers_;
119 }
120 
121 void
123 {
124 }
125 
126 void
127 JacoArmDummy::push_joystick(unsigned int button)
128 {
129 }
130 
131 void
133 {
134 }
135 
136 /** Move the arm along the given trajectory.
137  * Calls goto_joints() 33Hz (default Fawkes loop time)
138  * @see #goto_joints
139  *
140  * @param trajec the trajectory
141  * @param fingers target finger positions
142  */
143 void
144 JacoArmDummy::goto_trajec(std::vector<std::vector<float>> *trajec, std::vector<float> &fingers)
145 {
146  for (unsigned int i = 0; i < trajec->size(); ++i) {
147  goto_joints(trajec->at(i), fingers);
148  usleep(10e3);
149  }
150 }
151 
152 /** Move the arm to given configuration.
153  * No real movement for "dummy" arm though, it just sets these values to the
154  * current ones.
155  *
156  * @param joints target joint angles
157  * @param fingers target finger positions
158  * @param followup defines if this is a singular trajectory-point, or a consecutive one. Setting to "false"
159  * acuires control of the arm and sets the mode to "angular" each time. Because of that,
160  * it needs to be "true" if it is a "followup" trajectory point.
161  */
162 void
163 JacoArmDummy::goto_joints(std::vector<float> &joints, std::vector<float> &fingers, bool followup)
164 {
165  if (followup)
166  usleep(10e3);
167 
168  joints_ = joints;
169  fingers_ = fingers;
170 }
171 
172 /** Move the arm to given configuration.
173  * No real movement for "dummy" arm though, it just sets these values to the
174  * current ones.
175  *
176  * @param coords target fingertip coordinations
177  * @param fingers target finger positions
178  */
179 void
180 JacoArmDummy::goto_coords(std::vector<float> &coords, std::vector<float> &fingers)
181 {
182  coords_ = coords;
183  fingers_ = fingers;
184 }
185 
186 void
188 {
189  goto_joints(pos_ready_, fingers_);
190 }
191 
192 void
194 {
195  goto_joints(pos_retract_, fingers_);
196 }
197 
198 } // end of namespace fawkes
fawkes::JacoArmDummy::push_joystick
virtual void push_joystick(unsigned int button)
Simulate a push of a button on the joystick of the Kinova Jaco arm.
Definition: arm_dummy.cpp:127
fawkes::JacoArmDummy::get_fingers
virtual void get_fingers(std::vector< float > &to) const
Get the position values of the fingers.
Definition: arm_dummy.cpp:116
fawkes::JacoArm::name_
std::string name_
the name of this arm
Definition: arm.h:127
fawkes::JacoArmDummy::stop
virtual void stop()
Stop the current movement.
Definition: arm_dummy.cpp:122
fawkes::JacoArmDummy::get_joints
virtual void get_joints(std::vector< float > &to) const
Get the joint angles of the arm.
Definition: arm_dummy.cpp:110
fawkes::JacoArmDummy::goto_coords
virtual void goto_coords(std::vector< float > &coords, std::vector< float > &fingers)
Move the arm to given configuration.
Definition: arm_dummy.cpp:180
fawkes::JacoArmDummy::initialize
virtual void initialize()
Initialize the arm.
Definition: arm_dummy.cpp:86
fawkes
fawkes::JacoArmDummy::~JacoArmDummy
virtual ~JacoArmDummy()
Destructor.
Definition: arm_dummy.cpp:81
fawkes::JacoArmDummy::release_joystick
virtual void release_joystick()
Simulate releasing the joystick of the Kinova Jaco arm.
Definition: arm_dummy.cpp:132
fawkes::JacoArmDummy::goto_trajec
virtual void goto_trajec(std::vector< std::vector< float >> *trajec, std::vector< float > &fingers)
Move the arm along the given trajectory.
Definition: arm_dummy.cpp:144
fawkes::JacoArmDummy::goto_ready
virtual void goto_ready()
Move the arm to READY position.
Definition: arm_dummy.cpp:187
fawkes::JacoArmDummy::initialized
virtual bool initialized()
Check if arm is initialized.
Definition: arm_dummy.cpp:98
fawkes::JacoArmDummy::goto_retract
virtual void goto_retract()
Move the arm to RETRACT position.
Definition: arm_dummy.cpp:193
fawkes::JacoArmDummy::JacoArmDummy
JacoArmDummy(const char *name)
Constructor.
Definition: arm_dummy.cpp:55
fawkes::JacoArmDummy::get_coords
virtual void get_coords(std::vector< float > &to)
Get the cartesian coordinates of the arm.
Definition: arm_dummy.cpp:104
fawkes::JacoArmDummy::goto_joints
virtual void goto_joints(std::vector< float > &joints, std::vector< float > &fingers, bool followup=false)
Move the arm to given configuration.
Definition: arm_dummy.cpp:163
fawkes::JacoArm::initialized_
bool initialized_
track if the arm has been initialized or not
Definition: arm.h:128
fawkes::JacoArmDummy::final
virtual bool final()
Check if movement is final.
Definition: arm_dummy.cpp:92