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 #if 0 /* just to make Emacs auto-indent happy */
43 }
44 #endif
45 
46 /** @class JacoArmDummy <plugins/jaco/arm_dummy.h>
47  * Class for simulating a dummy Kinova Jaco Arm.
48  * Each command is accepted, simply storing its values and returning them
49  * when a getter is called. This class does not operate any actual arm
50  * (whether a real one nor even a simulated 3D model).
51  *
52  * @author Bahram Maleki-Fard
53  */
54 
55 /** Constructor.
56  * @param name The name of the arm we want to connect to
57  */
58 JacoArmDummy::JacoArmDummy(const char *name)
59 {
60  __name = name;
61  __initialized = true;
62 
63  // initialize target vectors for READY and RETRACT positions
64  __pos_ready.push_back(READY_J0);
65  __pos_ready.push_back(READY_J1);
66  __pos_ready.push_back(READY_J2);
67  __pos_ready.push_back(READY_J3);
68  __pos_ready.push_back(READY_J4);
69  __pos_ready.push_back(READY_J5);
70  __pos_retract.push_back(RETRACT_J0);
71  __pos_retract.push_back(RETRACT_J1);
72  __pos_retract.push_back(RETRACT_J2);
73  __pos_retract.push_back(RETRACT_J3);
74  __pos_retract.push_back(RETRACT_J4);
75  __pos_retract.push_back(RETRACT_J5);
76 
77  // initialize position vectors
78  __coords.assign(6, 0.f);
79  __joints.assign(6, 0.f);
80  __fingers.assign(3, 0.f);
81 }
82 
83 /** Destructor. */
85 {
86 }
87 
88 void
90 {
91  goto_ready();
92 }
93 
94 
95 bool
97 {
98  return true;
99 }
100 
101 bool
103 {
104  return __initialized;
105 }
106 
107 
108 void
109 JacoArmDummy::get_coords(std::vector<float> &to)
110 {
111  to = __coords;
112 }
113 
114 void
115 JacoArmDummy::get_joints(std::vector<float> &to) const
116 {
117  to = __joints;
118 }
119 
120 void
121 JacoArmDummy::get_fingers(std::vector<float> &to) const
122 {
123  to = __fingers;
124 }
125 
126 
127 void
129 {
130 }
131 
132 void
133 JacoArmDummy::push_joystick(unsigned int button)
134 {
135 }
136 
137 void
139 {
140 }
141 
142 /** Move the arm along the given trajectory.
143  * Calls goto_joints() 33Hz (default Fawkes loop time)
144  * @see #goto_joints
145  *
146  * @param trajec the trajectory
147  * @param fingers target finger positions
148  */
149 void
150 JacoArmDummy::goto_trajec(std::vector< std::vector<float> >* trajec, std::vector<float> &fingers)
151 {
152  for( unsigned int i=0; i<trajec->size(); ++i ) {
153  goto_joints(trajec->at(i), fingers);
154  usleep(10e3);
155  }
156 }
157 
158 /** Move the arm to given configuration.
159  * No real movement for "dummy" arm though, it just sets these values to the
160  * current ones.
161  *
162  * @param joints target joint angles
163  * @param fingers target finger positions
164  * @param followup defines if this is a singular trajectory-point, or a consecutive one. Setting to "false"
165  * acuires control of the arm and sets the mode to "angular" each time. Because of that,
166  * it needs to be "true" if it is a "followup" trajectory point.
167  */
168 void
169 JacoArmDummy::goto_joints(std::vector<float> &joints, std::vector<float> &fingers, bool followup)
170 {
171  if(followup)
172  usleep(10e3);
173 
174  __joints = joints;
175  __fingers = fingers;
176 }
177 
178 /** Move the arm to given configuration.
179  * No real movement for "dummy" arm though, it just sets these values to the
180  * current ones.
181  *
182  * @param coords target fingertip coordinations
183  * @param fingers target finger positions
184  */
185 void
186 JacoArmDummy::goto_coords(std::vector<float> &coords, std::vector<float> &fingers)
187 {
188  __coords = coords;
189  __fingers = fingers;
190 }
191 
192 void
194 {
195  goto_joints(__pos_ready, __fingers);
196 }
197 
198 void
200 {
201  goto_joints(__pos_retract, __fingers);
202 }
203 
204 
205 } // end of namespace fawkes
virtual bool initialized()
Check if arm is initialized.
Definition: arm_dummy.cpp:102
virtual void goto_ready()
Move the arm to READY position.
Definition: arm_dummy.cpp:193
JacoArmDummy(const char *name)
Constructor.
Definition: arm_dummy.cpp:58
virtual ~JacoArmDummy()
Destructor.
Definition: arm_dummy.cpp:84
virtual void release_joystick()
Simulate releasing the joystick of the Kinova Jaco arm.
Definition: arm_dummy.cpp:138
Fawkes library namespace.
virtual void goto_retract()
Move the arm to RETRACT position.
Definition: arm_dummy.cpp:199
std::string __name
the name of this arm
Definition: arm.h:127
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:169
virtual void get_coords(std::vector< float > &to)
Get the cartesian coordinates of the arm.
Definition: arm_dummy.cpp:109
virtual bool final()
Check if movement is final.
Definition: arm_dummy.cpp:96
bool __initialized
track if the arm has been initialized or not
Definition: arm.h:128
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:133
virtual void stop()
Stop the current movement.
Definition: arm_dummy.cpp:128
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:150
virtual void get_fingers(std::vector< float > &to) const
Get the position values of the fingers.
Definition: arm_dummy.cpp:121
virtual void goto_coords(std::vector< float > &coords, std::vector< float > &fingers)
Move the arm to given configuration.
Definition: arm_dummy.cpp:186
virtual void get_joints(std::vector< float > &to) const
Get the joint angles of the arm.
Definition: arm_dummy.cpp:115
virtual void initialize()
Initialize the arm.
Definition: arm_dummy.cpp:89