SimWall2D.cpp
Go to the documentation of this file.
1 
7 #include <gtsam/geometry/Pose2.h>
9 
10 namespace gtsam {
11 
12 using namespace std;
13 
14 /* ************************************************************************* */
15 void SimWall2D::print(const std::string& s) const {
16  std::cout << "SimWall2D " << s << ":" << std::endl;
17  traits<Point2>::Print(a_, " a");
18  traits<Point2>::Print(b_, " b");
19 }
20 
21 /* ************************************************************************* */
22 bool SimWall2D::equals(const SimWall2D& other, double tol) const {
23  return traits<Point2>::Equals(a_, other.a_, tol) &&
24  traits<Point2>::Equals(b_, other.b_, tol);
25 }
26 
27 /* ************************************************************************* */
28 bool SimWall2D::intersects(const SimWall2D& B, Point2* pt) const {
29  const bool debug = false;
30 
31  const SimWall2D& A = *this;
32 
33  // translate so A.a is at the origin, then rotate so that A.b is along x axis
34  Pose2 transform(Rot2::relativeBearing(A.b() - A.a()), A.a());
35 
36  // normalized points, Aa at origin, Ab at (length, 0.0)
37  double len = A.length();
38  if (debug) cout << "len: " << len << endl;
39  Point2 Ba = transform.transformTo(B.a()),
40  Bb = transform.transformTo(B.b());
41  if (debug) traits<Point2>::Print(Ba, "Ba");
42  if (debug) traits<Point2>::Print(Bb, "Bb");
43 
44  // check sides of line
45  if (Ba.y() * Bb.y() > 0.0 ||
46  (Ba.x() * Bb.x() > 0.0 && Ba.x() < 0.0) ||
47  (Ba.x() > len && Bb.x() > len)) {
48  if (debug) cout << "Failed first check" << endl;
49  return false;
50  }
51 
52  // check conditions for exactly on the same line
53  if (Ba.y() == 0.0 && Ba.x() > 0.0 && Ba.x() < len) {
54  if (pt) *pt = transform.transformFrom(Ba);
55  if (debug) cout << "Ba on the line" << endl;
56  return true;
57  } else if (Bb.y() == 0.0 && Bb.x() > 0.0 && Bb.x() < len) {
58  if (pt) *pt = transform.transformFrom(Bb);
59  if (debug) cout << "Bb on the line" << endl;
60  return true;
61  }
62 
63  // handle vertical case to avoid calculating slope
64  if (std::abs(Ba.x() - Bb.x()) < 1e-5) {
65  if (debug) cout << "vertical line" << endl;
66  if (Ba.x() < len && Ba.x() > 0.0) {
67  if (pt) *pt = transform.transformFrom(Point2(Ba.x(), 0.0));
68  if (debug) cout << " within range" << endl;
69  return true;
70  } else {
71  if (debug) cout << " not within range" << endl;
72  return false;
73  }
74  }
75 
76  // find lower point by y
77  Point2 low(0,0), high(0,0);
78  if (Ba.y() > Bb.y()) {
79  high = Ba;
80  low = Bb;
81  } else {
82  high = Bb;
83  low = Ba;
84  }
85  if (debug) traits<Point2>::Print(high, "high");
86  if (debug) traits<Point2>::Print(low, "low");
87 
88  // find x-intercept
89  double slope = (high.y() - low.y())/(high.x() - low.x());
90  if (debug) cout << "slope " << slope << endl;
91  double xint = (low.x() < high.x()) ? low.x() + std::abs(low.y())/slope : high.x() - std::abs(high.y())/slope;
92  if (debug) cout << "xintercept " << xint << endl;
93  if (xint > 0.0 && xint < len) {
94  if (pt) *pt = transform.transformFrom(Point2(xint, 0.0));
95  return true;
96  } else {
97  if (debug) cout << "xintercept out of range" << endl;
98  return false;
99  }
100 }
101 
102 /* ************************************************************************* */
104  Point2 vec = b_ - a_;
105  return a_ + vec * 0.5 * vec.norm();
106 }
107 
108 /* ************************************************************************* */
110  Point2 vec = b_ - a_;
111  return Point2(vec.y(), -vec.x());
112 }
113 
114 /* ************************************************************************* */
115 Rot2 SimWall2D::reflection(const Point2& init, const Point2& intersection) const {
116  // translate to put the intersection at the origin and the wall along the x axis
117  Rot2 wallAngle = Rot2::relativeBearing(b_ - a_);
118  Pose2 transform(wallAngle, intersection);
119  Point2 t_init = transform.transformTo(init);
120  Point2 t_goal(-t_init.x(), t_init.y());
121  return Rot2::relativeBearing(wallAngle.rotate(t_goal));
122 }
123 
124 /* ***************************************************************** */
125 std::pair<Pose2, bool> moveWithBounce(const Pose2& cur_pose, double step_size,
126  const std::vector<SimWall2D> walls, Sampler& angle_drift,
127  Sampler& reflect_noise, const Rot2& bias) {
128 
129  // calculate angle to change by
130  Rot2 dtheta = Rot2::fromAngle(angle_drift.sample()(0) + bias.theta());
131  Pose2 test_pose = cur_pose.retract((Vector(3) << step_size, 0.0, Rot2::Logmap(dtheta)(0)).finished());
132 
133  // create a segment to use for intersection checking
134  // find the closest intersection
135  SimWall2D traj(test_pose.t(), cur_pose.t());
136  bool collision = false; Point2 intersection(1e+10, 1e+10);
137  SimWall2D closest_wall;
138  for(const SimWall2D& wall: walls) {
139  Point2 cur_intersection;
140  if (wall.intersects(traj,cur_intersection)) {
141  collision = true;
142  if (distance2(cur_pose.t(), cur_intersection) < distance2(cur_pose.t(), intersection)) {
143  intersection = cur_intersection;
144  closest_wall = wall;
145  }
146  }
147  }
148 
149  // reflect off of wall with some noise
150  Pose2 pose(test_pose);
151  if (collision) {
152 
153  // make sure norm is on the robot's side
154  Point2 norm = closest_wall.norm();
155  norm = norm / norm.norm();
156 
157  // Simple check to make sure norm is on side closest robot
158  if (distance2(cur_pose.t(), intersection + norm) > distance2(cur_pose.t(), intersection - norm))
159  norm = - norm;
160 
161  // using the reflection
162  const double inside_bias = 0.05;
163  pose = Pose2(closest_wall.reflection(cur_pose.t(), intersection), intersection + inside_bias * norm);
164 
165  // perturb the rotation for better exploration
166  pose = pose.retract(reflect_noise.sample());
167  }
168 
169  return make_pair(pose, collision);
170 }
171 /* ***************************************************************** */
172 
173 } // \namespace gtsam
static Rot2 relativeBearing(const Point2 &d, OptionalJacobian< 1, 2 > H={})
Definition: Rot2.cpp:119
Vector sample() const
sample from distribution
Definition: Sampler.cpp:59
double length() const
Definition: SimWall2D.h:46
Point2 midpoint() const
Definition: SimWall2D.cpp:103
Vector2 Point2
Definition: Point2.h:32
bool equals(const SimWall2D &other, double tol=1e-9) const
Definition: SimWall2D.cpp:22
std::pair< Pose2, bool > moveWithBounce(const Pose2 &cur_pose, double step_size, const std::vector< SimWall2D > walls, Sampler &angle_drift, Sampler &reflect_noise, const Rot2 &bias)
Definition: SimWall2D.cpp:125
Definition: BFloat16.h:88
static Vector1 Logmap(const Rot2 &r, ChartJacobian H={})
Log map at identity - return the canonical coordinates of this rotation.
Definition: Rot2.cpp:77
static const Point3 pt(1.0, 2.0, 3.0)
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
Point2 a() const
Definition: SimWall2D.h:39
const Point2 & t() const
translation
Definition: Pose2.h:255
static constexpr bool debug
bool intersects(const SimWall2D &wall, Point2 *pt=nullptr) const
Definition: SimWall2D.cpp:28
Eigen::VectorXd Vector
Definition: Vector.h:38
Point2 norm() const
Definition: SimWall2D.cpp:109
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
EIGEN_DONT_INLINE void transform(const Transformation &t, Data &data)
Definition: geometry.cpp:25
traits
Definition: chartTesting.h:28
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
void print(const std::string &s="") const
Definition: SimWall2D.cpp:15
Point2 b() const
Definition: SimWall2D.h:40
Point2 rotate(const Point2 &p, OptionalJacobian< 2, 1 > H1={}, OptionalJacobian< 2, 2 > H2={}) const
Definition: Rot2.cpp:100
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Definition: Lie.h:131
double distance2(const Point2 &p, const Point2 &q, OptionalJacobian< 1, 2 > H1, OptionalJacobian< 1, 2 > H2)
distance between two points
Definition: Point2.cpp:39
Implementation of walls for use with simulators.
static Rot2 fromAngle(double theta)
Named constructor from angle in radians.
Definition: Rot2.h:61
const G double tol
Definition: Group.h:86
Rot2 reflection(const Point2 &init, const Point2 &intersection) const
Definition: SimWall2D.cpp:115
2D Pose
GTSAM_EXPORT Point2 transformTo(const Point2 &point, OptionalJacobian< 2, 3 > Dpose={}, OptionalJacobian< 2, 2 > Dpoint={}) const
Definition: Pose2.cpp:207
size_t len(handle h)
Get the length of a Python object.
Definition: pytypes.h:2244
#define abs(x)
Definition: datatypes.h:17
void Print(const CONTAINER &keys, const string &s, const KeyFormatter &keyFormatter)
Definition: Key.cpp:62
double theta() const
Definition: Rot2.h:186


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:49