HEBI C++ API  3.9.0
goal.hpp
Go to the documentation of this file.
1 #pragma once
2 
3 #include <Eigen/Dense>
4 
5 namespace hebi {
6 namespace experimental {
7 namespace arm {
8 
9 // A class that specifies a goal position of one or more waypoint and/or
10 // auxilary states.
11 // Static create methods are provided for various cases; in the case that
12 // velocities or accelerations are omitted, the default behavior is to leave
13 // these unconstrained except for a final "0" state. For aux states, this is
14 // left as unchanged. For times, this is left to the Arm object to fill in a
15 // heuristic.
16 class Goal {
17 
18 public:
20  // Single waypoint static create functions
22 
23  // Single waypoint, default vel/accel, no time
24  static Goal createFromPosition(const Eigen::VectorXd& positions) {
25  return Goal(Eigen::VectorXd(0),
26  toMatrix(positions),
27  nanWithZeroRight(positions.size(), 1),
28  nanWithZeroRight(positions.size(), 1),
29  Eigen::MatrixXd(0, 0));
30  }
31 
32  // Single waypoint, default vel/accel
33  static Goal createFromPosition(double time, const Eigen::VectorXd& positions) {
34  return Goal(toVector(time),
35  toMatrix(positions),
36  nanWithZeroRight(positions.size(), 1),
37  nanWithZeroRight(positions.size(), 1),
38  Eigen::MatrixXd(0, 0));
39  }
40 
41  // Single waypoint, no time
42  static Goal createFromWaypoint(const Eigen::VectorXd& positions,
43  const Eigen::VectorXd& velocities,
44  const Eigen::VectorXd& accelerations) {
45  return Goal(Eigen::VectorXd(0),
46  toMatrix(positions),
47  toMatrix(velocities),
48  toMatrix(accelerations),
49  Eigen::MatrixXd(0, 0));
50  }
51 
52  // Single waypoint
53  static Goal createFromWaypoint(double time,
54  const Eigen::VectorXd& positions,
55  const Eigen::VectorXd& velocities,
56  const Eigen::VectorXd& accelerations) {
57  return Goal(toVector(time),
58  toMatrix(positions),
59  toMatrix(velocities),
60  toMatrix(accelerations),
61  Eigen::MatrixXd(0, 0));
62  }
63 
64  // Single waypoints + aux state, no time
65  static Goal createFromWaypointWithAux(const Eigen::VectorXd& positions,
66  const Eigen::VectorXd& velocities,
67  const Eigen::VectorXd& accelerations,
68  const Eigen::VectorXd& aux) {
69  return Goal(Eigen::VectorXd(0),
70  toMatrix(positions),
71  toMatrix(velocities),
72  toMatrix(accelerations),
73  toMatrix(aux));
74  }
75 
76  // Single waypoints + aux state
77  static Goal createFromWaypointWithAux(double time,
78  const Eigen::VectorXd& positions,
79  const Eigen::VectorXd& velocities,
80  const Eigen::VectorXd& accelerations,
81  const Eigen::VectorXd& aux) {
82  return Goal(toVector(time),
83  toMatrix(positions),
84  toMatrix(velocities),
85  toMatrix(accelerations),
86  toMatrix(aux));
87  }
88 
90  // Multiple waypoint static create functions
92 
93  // Multiple waypoints, default vel/accel, no time
94  static Goal createFromPositions(const Eigen::MatrixXd& positions) {
95  return Goal(Eigen::VectorXd(0),
96  positions,
97  nanWithZeroRight(positions.rows(), positions.cols()),
98  nanWithZeroRight(positions.rows(), positions.cols()),
99  Eigen::MatrixXd(0, 0));
100  }
101 
102  // Multiple waypoints, default vel/accel
103  static Goal createFromPositions(const Eigen::VectorXd& times,
104  const Eigen::MatrixXd& positions) {
105  return Goal(times,
106  positions,
107  nanWithZeroRight(positions.rows(), positions.cols()),
108  nanWithZeroRight(positions.rows(), positions.cols()),
109  Eigen::MatrixXd(0, 0));
110  }
111 
112  // Multiple waypoints, no time
113  static Goal createFromWaypoints(const Eigen::MatrixXd& positions,
114  const Eigen::MatrixXd& velocities,
115  const Eigen::MatrixXd& accelerations) {
116  return Goal(Eigen::VectorXd(0),
117  positions,
118  velocities,
120  Eigen::MatrixXd(0, 0));
121  }
122 
123  // Multiple waypoints
124  static Goal createFromWaypoints(const Eigen::VectorXd& times,
125  const Eigen::MatrixXd& positions,
126  const Eigen::MatrixXd& velocities,
127  const Eigen::MatrixXd& accelerations) {
128  return Goal(times,
129  positions,
130  velocities,
132  Eigen::MatrixXd(0, 0));
133  }
134 
135  // Multiple waypoints + aux state, no time
136  static Goal createFromWaypointsWithAux(const Eigen::MatrixXd& positions,
137  const Eigen::MatrixXd& velocities,
138  const Eigen::MatrixXd& accelerations,
139  const Eigen::MatrixXd& aux) {
140  return Goal(Eigen::VectorXd(0),
141  positions,
142  velocities,
144  aux);
145  }
146 
147  // Multiple waypoints + aux state
148  static Goal createFromWaypointsWithAux(const Eigen::VectorXd& times,
149  const Eigen::MatrixXd& positions,
150  const Eigen::MatrixXd& velocities,
151  const Eigen::MatrixXd& accelerations,
152  const Eigen::MatrixXd& aux) {
153  return Goal(times,
154  positions,
155  velocities,
157  aux);
158  }
159 
160  const Eigen::VectorXd& times() const { return times_; }
161  const Eigen::MatrixXd& positions() const { return positions_; }
162  const Eigen::MatrixXd& velocities() const { return velocities_; }
163  const Eigen::MatrixXd& accelerations() const { return accelerations_; }
164  const Eigen::MatrixXd& aux() const { return aux_; }
165 
166 private:
167 
168  Goal(const Eigen::VectorXd& times,
169  const Eigen::MatrixXd& positions,
170  const Eigen::MatrixXd& velocities,
171  const Eigen::MatrixXd& accelerations,
172  const Eigen::MatrixXd& aux)
173  : times_(times),
174  positions_(positions),
175  velocities_(velocities),
176  accelerations_(accelerations),
177  aux_(aux) {}
178 
179  // Helper function to create unconstrained points along a motion, with nan at the right side.
180  static Eigen::MatrixXd nanWithZeroRight(size_t num_joints, size_t num_waypoints) {
181  double nan = std::numeric_limits<double>::quiet_NaN();
182  Eigen::MatrixXd matrix(num_joints, num_waypoints);
183  matrix.setConstant(nan);
184  matrix.rightCols<1>().setZero();
185  return matrix;
186  }
187 
188  static Eigen::VectorXd toVector(double scalar) {
189  Eigen::VectorXd vector(1);
190  vector[0] = scalar;
191  return vector;
192  }
193 
194  static Eigen::MatrixXd toMatrix(const Eigen::VectorXd& vector) {
195  Eigen::MatrixXd matrix(vector.size(), 1);
196  matrix.col(0) = vector;
197  return matrix;
198  }
199 
200  const Eigen::VectorXd times_{0};
201  const Eigen::MatrixXd positions_{0, 0};
202  const Eigen::MatrixXd velocities_{0, 0};
203  const Eigen::MatrixXd accelerations_{0, 0};
204  const Eigen::MatrixXd aux_{0, 0};
205 };
206 
207 } // namespace arm
208 } // namespace experimental
209 } // namespace hebi
static Goal createFromPosition(double time, const Eigen::VectorXd &positions)
Definition: goal.hpp:33
const Eigen::VectorXd & times() const
Definition: goal.hpp:160
const Eigen::MatrixXd & positions() const
Definition: goal.hpp:161
Definition: arm.cpp:8
static Goal createFromWaypointsWithAux(const Eigen::VectorXd &times, const Eigen::MatrixXd &positions, const Eigen::MatrixXd &velocities, const Eigen::MatrixXd &accelerations, const Eigen::MatrixXd &aux)
Definition: goal.hpp:148
static Goal createFromWaypointWithAux(double time, const Eigen::VectorXd &positions, const Eigen::VectorXd &velocities, const Eigen::VectorXd &accelerations, const Eigen::VectorXd &aux)
Definition: goal.hpp:77
const Eigen::MatrixXd & accelerations() const
Definition: goal.hpp:163
static Goal createFromPositions(const Eigen::VectorXd &times, const Eigen::MatrixXd &positions)
Definition: goal.hpp:103
static Goal createFromWaypoint(const Eigen::VectorXd &positions, const Eigen::VectorXd &velocities, const Eigen::VectorXd &accelerations)
Definition: goal.hpp:42
static Goal createFromWaypointsWithAux(const Eigen::MatrixXd &positions, const Eigen::MatrixXd &velocities, const Eigen::MatrixXd &accelerations, const Eigen::MatrixXd &aux)
Definition: goal.hpp:136
static Goal createFromWaypointWithAux(const Eigen::VectorXd &positions, const Eigen::VectorXd &velocities, const Eigen::VectorXd &accelerations, const Eigen::VectorXd &aux)
Definition: goal.hpp:65
static Goal createFromPosition(const Eigen::VectorXd &positions)
Definition: goal.hpp:24
static Goal createFromWaypoint(double time, const Eigen::VectorXd &positions, const Eigen::VectorXd &velocities, const Eigen::VectorXd &accelerations)
Definition: goal.hpp:53
const Eigen::MatrixXd & aux() const
Definition: goal.hpp:164
static Goal createFromWaypoints(const Eigen::VectorXd &times, const Eigen::MatrixXd &positions, const Eigen::MatrixXd &velocities, const Eigen::MatrixXd &accelerations)
Definition: goal.hpp:124
Definition: goal.hpp:16
static Goal createFromWaypoints(const Eigen::MatrixXd &positions, const Eigen::MatrixXd &velocities, const Eigen::MatrixXd &accelerations)
Definition: goal.hpp:113
static Goal createFromPositions(const Eigen::MatrixXd &positions)
Definition: goal.hpp:94
const Eigen::MatrixXd & velocities() const
Definition: goal.hpp:162