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
41namespace 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 */
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
85void
87{
88 goto_ready();
89}
90
91bool
93{
94 return true;
95}
96
97bool
99{
100 return initialized_;
101}
102
103void
104JacoArmDummy::get_coords(std::vector<float> &to)
105{
106 to = coords_;
107}
108
109void
110JacoArmDummy::get_joints(std::vector<float> &to) const
111{
112 to = joints_;
113}
114
115void
116JacoArmDummy::get_fingers(std::vector<float> &to) const
117{
118 to = fingers_;
119}
120
121void
123{
124}
125
126void
127JacoArmDummy::push_joystick(unsigned int button)
128{
129}
130
131void
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 */
143void
144JacoArmDummy::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 */
162void
163JacoArmDummy::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 */
179void
180JacoArmDummy::goto_coords(std::vector<float> &coords, std::vector<float> &fingers)
181{
182 coords_ = coords;
183 fingers_ = fingers;
184}
185
186void
188{
189 goto_joints(pos_ready_, fingers_);
190}
191
192void
194{
195 goto_joints(pos_retract_, fingers_);
196}
197
198} // end of namespace fawkes
virtual bool final()
Check if movement is final.
Definition: arm_dummy.cpp:92
virtual void goto_retract()
Move the arm to RETRACT position.
Definition: arm_dummy.cpp:193
virtual void release_joystick()
Simulate releasing the joystick of the Kinova Jaco arm.
Definition: arm_dummy.cpp:132
virtual void goto_ready()
Move the arm to READY position.
Definition: arm_dummy.cpp:187
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
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
virtual ~JacoArmDummy()
Destructor.
Definition: arm_dummy.cpp:81
virtual void initialize()
Initialize the arm.
Definition: arm_dummy.cpp:86
JacoArmDummy(const char *name)
Constructor.
Definition: arm_dummy.cpp:55
virtual void get_coords(std::vector< float > &to)
Get the cartesian coordinates of the arm.
Definition: arm_dummy.cpp:104
virtual void stop()
Stop the current movement.
Definition: arm_dummy.cpp:122
virtual bool initialized()
Check if arm is initialized.
Definition: arm_dummy.cpp:98
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
virtual void goto_coords(std::vector< float > &coords, std::vector< float > &fingers)
Move the arm to given configuration.
Definition: arm_dummy.cpp:180
virtual void get_joints(std::vector< float > &to) const
Get the joint angles of the arm.
Definition: arm_dummy.cpp:110
virtual void get_fingers(std::vector< float > &to) const
Get the position values of the fingers.
Definition: arm_dummy.cpp:116
std::string name_
the name of this arm
Definition: arm.h:121
bool initialized_
track if the arm has been initialized or not
Definition: arm.h:122
Fawkes library namespace.