quasi_static.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2018, University of Edinburgh
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 //
8 // * Redistributions of source code must retain the above copyright notice,
9 // this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // * Neither the name of nor the names of its contributors may be used to
14 // endorse or promote products derived from this software without specific
15 // prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 // POSSIBILITY OF SUCH DAMAGE.
28 //
29 
30 #include <cmath>
31 
32 #include <exotica_core/server.h>
33 #include <exotica_core_task_maps/convex_hull.h>
34 #include <exotica_core_task_maps/quasi_static.h>
35 
36 REGISTER_TASKMAP_TYPE("QuasiStatic", exotica::QuasiStatic);
37 
38 namespace exotica
39 {
40 constexpr double eps = 1e-8;
41 
42 QuasiStatic::QuasiStatic() = default;
43 QuasiStatic::~QuasiStatic() = default;
44 
51 double cross(Eigen::VectorXdRefConst a, Eigen::VectorXdRefConst b)
52 {
53  return a(0) * b(1) - a(1) * b(0);
54 }
55 
63 void potential(double& phi, Eigen::VectorXdRefConst A, Eigen::VectorXdRefConst B, Eigen::VectorXdRefConst P)
64 {
65  double C = A.dot(B) - A.dot(P) + B.dot(P) - B.dot(B);
66  double D = -A.dot(B) - A.dot(P) + B.dot(P) + A.dot(A);
67  double E = cross(A, B) - cross(A, P) + cross(B, P);
68  if (fabs(E) <= eps)
69  {
70  phi = 0.0;
71  }
72  else
73  {
74  phi = (atan(C / E) - atan(D / E)) / E;
75  }
76 }
77 
89 void potential(double& phi, Eigen::VectorXdRef jacobian, Eigen::VectorXdRefConst A, Eigen::VectorXdRefConst B, Eigen::VectorXdRefConst P, Eigen::MatrixXdRefConst A_, Eigen::MatrixXdRefConst B_, Eigen::MatrixXdRefConst P_)
90 {
91  double C = A.dot(B) - A.dot(P) + B.dot(P) - B.dot(B);
92  double D = -A.dot(B) - A.dot(P) + B.dot(P) + A.dot(A);
93  double E = cross(A, B) - cross(A, P) + cross(B, P);
94  if (fabs(E) < eps)
95  {
96  phi = 0.0;
97  jacobian.setZero();
98  }
99  else
100  {
101  phi = (atan(C / E) - atan(D / E)) / E;
102  for (int i = 0; i < jacobian.rows(); ++i)
103  {
104  double C_ = A_.col(i).dot(B) + A.dot(B_.col(i)) - A_.col(i).dot(P) - A.dot(P_.col(i)) + B_.col(i).dot(P) + B.dot(P_.col(i)) - 2 * B_.col(i).dot(B);
105  double D_ = -A_.col(i).dot(B) - A.dot(B_.col(i)) - A_.col(i).dot(P) - A.dot(P_.col(i)) + B_.col(i).dot(P) + B.dot(P_.col(i)) + 2 * A_.col(i).dot(A);
106  double E_ = cross(A_.col(i), B) + cross(A, B_.col(i)) - cross(A_.col(i), P) - cross(A, P_.col(i)) + cross(B_.col(i), P) + cross(B, P_.col(i));
107  jacobian(i) = ((C_ / E - E_ * C / E / E) / (C * C / E / E + 1) - (D_ / E - E_ * D / E / E) / (D * D / E / E + 1)) / E - E_ / E * phi;
108  }
109  }
110 }
111 
119 void winding(double& phi, Eigen::VectorXdRefConst A, Eigen::VectorXdRefConst B, Eigen::VectorXdRefConst P)
120 {
121  double C = cross(A - P, B - P);
122  double D = (A - P).dot(B - P);
123  phi = atan2(C, D) / 2.0 / M_PI;
124 }
125 
137 void winding(double& phi, Eigen::VectorXdRef jacobian, Eigen::VectorXdRefConst A, Eigen::VectorXdRefConst B, Eigen::VectorXdRefConst P, Eigen::MatrixXdRefConst A_, Eigen::MatrixXdRefConst B_, Eigen::MatrixXdRefConst P_)
138 {
139  double C = cross(A - P, B - P);
140  double D = (A - P).dot(B - P);
141  phi = atan2(C, D) / 2.0 / M_PI;
142  for (int i = 0; i < jacobian.rows(); ++i)
143  {
144  double C_ = cross(A_.col(i) - P_.col(i), B - P) + cross(A - P, B_.col(i) - P_.col(i));
145  double D_ = (A_.col(i) - P_.col(i)).dot(B - P) + (A - P).dot(B_.col(i) - P_.col(i));
146  jacobian(i) = (C_ * D - C * D_) / (C * C + D * D) / 2.0 / M_PI;
147  }
148 }
149 
150 void QuasiStatic::Update(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi)
151 {
152  if (phi.rows() != 1) ThrowNamed("Wrong size of phi!");
153  phi(0) = 0.0;
154  KDL::Vector kdl_com;
155  double M = 0.0;
156  for (std::weak_ptr<KinematicElement> welement : scene_->GetKinematicTree().GetTree())
157  {
158  std::shared_ptr<KinematicElement> element = welement.lock();
159  if (element->is_robot_link || element->closest_robot_link.lock()) // Only for robot links and attached objects
160  {
161  double mass = element->segment.getInertia().getMass();
162  if (mass > 0)
163  {
164  KDL::Frame cog = KDL::Frame(element->segment.getInertia().getCOG());
165  KDL::Frame com_local = scene_->GetKinematicTree().FK(element, cog, nullptr, KDL::Frame());
166  kdl_com += com_local.p * mass;
167  M += mass;
168  }
169  }
170  }
171  if (M == 0.0) return;
172  kdl_com = kdl_com / M;
173  Eigen::VectorXd com(2);
174  com(0) = kdl_com[0];
175  com(1) = kdl_com[1];
176 
177  Eigen::MatrixXd supports(kinematics[0].Phi.rows(), 2);
178  for (int i = 0; i < kinematics[0].Phi.rows(); ++i)
179  {
180  supports(i, 0) = kinematics[0].Phi(i).p[0];
181  supports(i, 1) = kinematics[0].Phi(i).p[1];
182  }
183 
184  std::list<int> hull = ConvexHull2D(supports);
185 
186  double n = hull.size();
187  double wnd = 0.0;
188  double pot = 0.0;
189  double tmp;
190  for (std::list<int>::iterator it = hull.begin(); it != hull.end();)
191  {
192  int a = *it;
193  int b = ++it == hull.end() ? *hull.begin() : *(it);
194  potential(tmp, supports.row(a).transpose(), supports.row(b).transpose(), com);
195  pot += tmp;
196  winding(tmp, supports.row(a).transpose(), supports.row(b).transpose(), com);
197  wnd += tmp;
198  }
199  wnd = fabs(wnd);
200 
201  if (pot < eps)
202  {
203  if (wnd < 0.5)
204  {
205  phi(0) = -sqrt(-n / pot);
206  }
207  else
208  {
209  if (!parameters_.PositiveOnly)
210  {
211  phi(0) = sqrt(-n / pot);
212  }
213  }
214  }
215 
216  if (debug_)
217  {
218  debug_msg_.markers[0].pose.position.x = com(0);
219  debug_msg_.markers[0].pose.position.y = com(1);
220  debug_msg_.markers[0].pose.position.z = 0.0;
221 
222  debug_msg_.markers[1].points.resize(hull.size() + 1);
223  int ii = 0;
224  for (int i : hull)
225  {
226  debug_msg_.markers[1].points[ii].x = supports(i, 0);
227  debug_msg_.markers[1].points[ii].y = supports(i, 1);
228  debug_msg_.markers[1].points[ii++].z = 0.0;
229  }
230  debug_msg_.markers[1].points[hull.size()] = debug_msg_.markers[1].points[0];
231 
232  debug_pub_.publish(debug_msg_);
233  }
234 }
235 
236 void QuasiStatic::Update(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi, Eigen::MatrixXdRef jacobian)
237 {
238  if (phi.rows() != 1) ThrowNamed("Wrong size of phi!");
239  if (jacobian.rows() != 1 || jacobian.cols() != x.rows()) ThrowNamed("Wrong size of jacobian! " << x.rows());
240  phi(0) = 0.0;
241  jacobian.setZero();
242  Eigen::MatrixXd jacobian_com = Eigen::MatrixXd::Zero(2, jacobian.cols());
243  KDL::Vector kdl_com;
244  double M = 0.0;
245  for (std::weak_ptr<KinematicElement> welement : scene_->GetKinematicTree().GetTree())
246  {
247  std::shared_ptr<KinematicElement> element = welement.lock();
248  if (element->is_robot_link || element->closest_robot_link.lock()) // Only for robot links and attached objects
249  {
250  double mass = element->segment.getInertia().getMass();
251  if (mass > 0)
252  {
253  KDL::Frame cog = KDL::Frame(element->segment.getInertia().getCOG());
254  KDL::Frame com_local = scene_->GetKinematicTree().FK(element, cog, nullptr, KDL::Frame());
255  Eigen::MatrixXd jacobian_com_local = scene_->GetKinematicTree().Jacobian(element, cog, nullptr, KDL::Frame());
256  kdl_com += com_local.p * mass;
257  jacobian_com += jacobian_com_local.topRows(2) * mass;
258  M += mass;
259  }
260  }
261  }
262  if (M == 0.0) return;
263  kdl_com = kdl_com / M;
264  Eigen::VectorXd com(2);
265  com(0) = kdl_com[0];
266  com(1) = kdl_com[1];
267  jacobian_com = jacobian_com / M;
268 
269  Eigen::MatrixXd supports(kinematics[0].Phi.rows(), 2);
270  Eigen::MatrixXd supportsJ(kinematics[0].Phi.rows() * 2, x.rows());
271  for (int i = 0; i < kinematics[0].Phi.rows(); ++i)
272  {
273  supports(i, 0) = kinematics[0].Phi(i).p[0];
274  supports(i, 1) = kinematics[0].Phi(i).p[1];
275  supportsJ.middleRows(i * 2, 2) = kinematics[0].jacobian(i).data.topRows(2);
276  }
277 
278  std::list<int> hull = ConvexHull2D(supports);
279 
280  double n = hull.size();
281  double wnd = 0.0;
282  double pot = 0.0;
283  Eigen::VectorXd potJ = jacobian.row(0);
284  double tmp;
285  Eigen::VectorXd tmpJ = jacobian.row(0);
286  for (std::list<int>::iterator it = hull.begin(); it != hull.end(); ++it)
287  {
288  int a = *it;
289  int b = (std::next(it) == hull.end()) ? *hull.begin() : *(std::next(it));
290  potential(tmp, tmpJ, supports.row(a).transpose(), supports.row(b).transpose(), com, supportsJ.middleRows(a * 2, 2), supportsJ.middleRows(b * 2, 2), jacobian_com);
291  pot += tmp;
292  potJ += tmpJ;
293  winding(tmp, supports.row(a).transpose(), supports.row(b).transpose(), com);
294  wnd += tmp;
295  }
296  wnd = fabs(wnd);
297 
298  if (fabs(pot) > eps)
299  {
300  if (wnd < 0.5)
301  {
302  phi(0) = sqrt(-n / pot);
303  jacobian.row(0) = potJ * (n / (2 * pot * pot * phi(0)));
304  }
305  else
306  {
307  if (!parameters_.PositiveOnly)
308  {
309  phi(0) = -sqrt(-n / pot);
310  jacobian.row(0) = potJ * (n / (2 * pot * pot * phi(0)));
311  }
312  }
313  }
314 
315  if (debug_)
316  {
317  debug_msg_.markers[0].pose.position.x = com(0);
318  debug_msg_.markers[0].pose.position.y = com(1);
319  debug_msg_.markers[0].pose.position.z = 0.0;
320 
321  debug_msg_.markers[1].points.resize(hull.size() + 1);
322  int ii = 0;
323  for (int i : hull)
324  {
325  debug_msg_.markers[1].points[ii].x = supports(i, 0);
326  debug_msg_.markers[1].points[ii].y = supports(i, 1);
327  debug_msg_.markers[1].points[ii].z = 0.0;
328  ++ii;
329  }
330  debug_msg_.markers[1].points[ii] = debug_msg_.markers[1].points[0];
331 
332  debug_pub_.publish(debug_msg_);
333  }
334 }
335 
336 int QuasiStatic::TaskSpaceDim()
337 {
338  return 1;
339 }
340 
341 void QuasiStatic::Initialize()
342 {
343  {
344  visualization_msgs::Marker mrk;
345  mrk.action = visualization_msgs::Marker::ADD;
346  mrk.header.frame_id = "exotica/" + scene_->GetRootFrameName();
347  mrk.id = 1;
348  mrk.type = visualization_msgs::Marker::SPHERE;
349  mrk.scale.x = mrk.scale.y = mrk.scale.z = 0.05;
350  mrk.color.r = 0;
351  mrk.color.g = 1;
352  mrk.color.b = 0;
353  mrk.color.a = 1;
354  debug_msg_.markers.push_back(mrk);
355  }
356  {
357  visualization_msgs::Marker mrk;
358  mrk.action = visualization_msgs::Marker::ADD;
359  mrk.header.frame_id = "exotica/" + scene_->GetRootFrameName();
360  mrk.id = 2;
361  mrk.type = visualization_msgs::Marker::LINE_STRIP;
362  mrk.color.r = 0;
363  mrk.color.g = 0;
364  mrk.color.b = 1;
365  mrk.color.a = 1;
366  mrk.scale.x = 0.02;
367  debug_msg_.markers.push_back(mrk);
368  }
369  if (debug_)
370  {
371  debug_pub_ = Server::Advertise<visualization_msgs::MarkerArray>(ns_ + "/exotica/QuasiStatic", 1, true);
372  }
373 }
374 
375 void QuasiStatic::AssignScene(ScenePtr scene)
376 {
377  scene_ = scene;
378  Initialize();
379 }
380 } // namespace exotica
exotica::Instantiable< QuasiStaticInitializer >::parameters_
C parameters_
convex_hull.h
exotica::QuasiStatic::AssignScene
void AssignScene(ScenePtr scene) override
Definition: quasi_static.cpp:375
exotica::QuasiStatic::Update
void Update(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi) override
Definition: quasi_static.cpp:150
n
n
exotica::QuasiStatic::debug_msg_
visualization_msgs::MarkerArray debug_msg_
Definition: quasi_static.h:57
exotica::potential
void potential(double &phi, Eigen::VectorXdRefConst A, Eigen::VectorXdRefConst B, Eigen::VectorXdRefConst P)
potential Calculates electrostatic potential at pont P induced by a uniformly charged line AB.
Definition: quasi_static.cpp:63
exotica
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
exotica::Object::ns_
std::string ns_
exotica::QuasiStatic::debug_pub_
ros::Publisher debug_pub_
Definition: quasi_static.h:58
atan2
const AutoDiffScalar< Matrix< typename internal::traits< typename internal::remove_all< DerTypeA >::type >::Scalar, Dynamic, 1 > > atan2(const AutoDiffScalar< DerTypeA > &a, const AutoDiffScalar< DerTypeB > &b)
dot
TFSIMD_FORCE_INLINE tfScalar dot(const Quaternion &q1, const Quaternion &q2)
A
exotica::ScenePtr
std::shared_ptr< Scene > ScenePtr
Eigen::VectorXdRef
Eigen::Ref< Eigen::VectorXd > VectorXdRef
Eigen::VectorXdRefConst
const typedef Eigen::Ref< const Eigen::VectorXd > & VectorXdRefConst
exotica::QuasiStatic::Initialize
void Initialize()
Definition: quasi_static.cpp:341
exotica::cross
double cross(Eigen::VectorXdRefConst a, Eigen::VectorXdRefConst b)
cross 2D cross product (z coordinate of a 3D cross product of 2 vectors on a xy plane).
Definition: quasi_static.cpp:51
REGISTER_TASKMAP_TYPE
REGISTER_TASKMAP_TYPE("QuasiStatic", exotica::QuasiStatic)
kinematics
exotica::TaskMap::scene_
ScenePtr scene_
exotica::QuasiStatic
Definition: quasi_static.h:42
exotica::winding
void winding(double &phi, Eigen::VectorXdRefConst A, Eigen::VectorXdRefConst B, Eigen::VectorXdRefConst P)
winding Calculates the winding number around pont P w.r.t. thw line AB.
Definition: quasi_static.cpp:119
exotica::eps
constexpr double eps
Definition: quasi_static.cpp:40
Eigen::MatrixXdRef
Eigen::Ref< Eigen::MatrixXd > MatrixXdRef
Eigen::MatrixXdRefConst
const typedef Eigen::Ref< const Eigen::MatrixXd > & MatrixXdRefConst
exotica::Object::debug_
bool debug_
exotica::QuasiStatic::QuasiStatic
QuasiStatic()
exotica::ConvexHull2D
std::list< int > ConvexHull2D(Eigen::MatrixXdRefConst points)
Definition: convex_hull.h:78
exotica::QuasiStatic::~QuasiStatic
virtual ~QuasiStatic()
quasi_static.h
M_PI
#define M_PI
x
double x
exotica::QuasiStatic::TaskSpaceDim
int TaskSpaceDim() override
Definition: quasi_static.cpp:336
server.h
ThrowNamed
#define ThrowNamed(m)

exotica_core_task_maps
Author(s): Yiming Yang, Michael Camilleri
autogenerated on Fri Aug 2 2024 08:44:10