ceti_robot.cpp
Go to the documentation of this file.
1 #include "robot/ceti_robot.h"
2 #include <regex>
3 
4 CetiRobot::CetiRobot(std::string& name, tf2::Transform tf, tf2::Vector3 size)
5 : AbstractRobot(name, tf, size)
6 , observer_mask_(std::bitset<3>(0))
7 {}
8 
9 
10 bool CetiRobot::checkSingleObjectCollision(tf2::Transform& obj, std::string& str, std::bitset<3>& panel_mask){
11 
12  // 1. Root joint check
13  tf2::Transform A = tf_ * root_tf_ * robot_root_bounds_[0];
14  tf2::Transform B = tf_ * root_tf_ * robot_root_bounds_[1];
15  tf2::Transform C = tf_ * root_tf_ * robot_root_bounds_[2];
16  tf2::Transform D = tf_ * root_tf_ * robot_root_bounds_[3];
17 
18  float full_area = areaCalculation(A,B,C) + areaCalculation(A,D,C);
19  float sum = areaCalculation(A,obj,D) + areaCalculation(D,obj,C) + areaCalculation(C,obj,B) + areaCalculation(obj, B, A);
20  if ((std::floor(sum*1000)/1000.f) <= full_area) {return false; }
21 
22  // 2. table_top check
23  std::stringstream ss;
24  std::regex rx("panda_arm([0-9]+)");
25  std::smatch match;
26  std::regex_match(str, match, rx);
27 
28  ss << "base_" << match[1];
29  A = tf_ * bounds_[0];
30  B = tf_ * bounds_[1];
31  C = tf_ * bounds_[2];
32  D = tf_ * bounds_[3];
33 
34  full_area = areaCalculation(A,B,C) + areaCalculation(A,D,C);
35  sum = areaCalculation(A,obj,D) + areaCalculation(D,obj,C) + areaCalculation(C,obj,B) + areaCalculation(obj, B, A);
36  if ((std::floor(sum*1000)/1000.f) <= full_area){
37  ROS_INFO("%f %f, %f", std::floor(sum*1000)/1000.f, full_area, sum);
38  str = ss.str();
39  return true;
40  }
41 
42  // 3. panel collsision check
43 
44  for(std::size_t i = 0; i < observers_.size(); i++){
45  if(observer_mask_[i] & panel_mask[i]){
46  tf2::Transform A = observers_[i]->worldTf() * observers_[i]->bounds()[0];
47  tf2::Transform B = observers_[i]->worldTf() * observers_[i]->bounds()[1];
48  tf2::Transform C = observers_[i]->worldTf() * observers_[i]->bounds()[2];
49  tf2::Transform D = observers_[i]->worldTf() * observers_[i]->bounds()[3];
50 
51  full_area = areaCalculation(A,B,C) + areaCalculation(A,D,C);
52  sum = areaCalculation(A,obj,D) + areaCalculation(D,obj,C) + areaCalculation(C,obj,B) + areaCalculation(obj, B, A);
53  if ((std::floor(sum*1000)/1000.f) <= full_area) {
54  str = observers_[i]->name();
55  return true;
56  }
57  }
58  }
59 
60  return false;
61 }
62 
64  observers_.clear();
65  tf_ = tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0, tf().getOrigin().getZ()));
66 }
67 
69  for(int i = 0; i < observers_.size(); i++) if(observers_[i]) observers_[i]->update(tf_);
70  // for(int i = 0; i < access_fields_.size(); i++) access_fields_[i]->update(tf_);
71 }
72 
74  tf2::Transform A = tf_ * bounds_[0];
75  tf2::Transform B = tf_ * bounds_[1];
76  tf2::Transform C = tf_ * bounds_[2];
77  tf2::Transform D = tf_ * bounds_[3];
78 
79  float full_area = areaCalculation(A,B,C) + areaCalculation(A,D,C);
80  float sum = areaCalculation(A,rob->tf(),D) + areaCalculation(D,rob->tf(),C) + areaCalculation(C,rob->tf(),B) + areaCalculation(rob->tf(), B, A);
81  if ((std::floor(sum*100)/100.f) <= full_area) return true;
82 
83  for(int i = 0; i < rob->observers().size(); i++) {
84  tf2::Transform WA = rob->observers()[i]->worldTf() * rob->observers()[i]->bounds()[0];
85  tf2::Transform WB = rob->observers()[i]->worldTf() * rob->observers()[i]->bounds()[1];
86  tf2::Transform WC = rob->observers()[i]->worldTf() * rob->observers()[i]->bounds()[2];
87  tf2::Transform WD = rob->observers()[i]->worldTf() * rob->observers()[i]->bounds()[3];
88  sum = areaCalculation(A,WA,D) + areaCalculation(D,WA,C) + areaCalculation(C,WA,B) + areaCalculation(WA, B, A); if ((std::floor(sum*100)/100.f) <= full_area) return true;
89  sum = areaCalculation(A,WB,D) + areaCalculation(D,WB,C) + areaCalculation(C,WB,B) + areaCalculation(WB, B, A); if ((std::floor(sum*100)/100.f) <= full_area) return true;
90  sum = areaCalculation(A,WC,D) + areaCalculation(D,WC,C) + areaCalculation(C,WC,B) + areaCalculation(WC, B, A); if ((std::floor(sum*100)/100.f) <= full_area) return true;
91  sum = areaCalculation(A,WD,D) + areaCalculation(D,WD,C) + areaCalculation(C,WD,B) + areaCalculation(WD, B, A); if ((std::floor(sum*100)/100.f) <= full_area) return true;
92  }
93 
94  for (int i = 0; i < observers_.size(); i++){
95  tf2::Transform WA = observers_[i]->worldTf() * observers_[i]->bounds()[0];
96  tf2::Transform WB = observers_[i]->worldTf() * observers_[i]->bounds()[1];
97  tf2::Transform WC = observers_[i]->worldTf() * observers_[i]->bounds()[2];
98  tf2::Transform WD = observers_[i]->worldTf() * observers_[i]->bounds()[3];
99 
100  full_area = areaCalculation(WA,WB,WC) + areaCalculation(WA,WD,WC);
101  for(int j = 0; j < rob->observers().size(); j++) {
102  tf2::Transform WA2 = rob->observers()[j]->worldTf() * observers_[j]->bounds()[0];
103  tf2::Transform WB2 = rob->observers()[j]->worldTf() * observers_[j]->bounds()[1];
104  tf2::Transform WC2 = rob->observers()[j]->worldTf() * observers_[j]->bounds()[2];
105  tf2::Transform WD2 = rob->observers()[j]->worldTf() * observers_[j]->bounds()[3];
106  sum = areaCalculation(WA,WA2,WD) + areaCalculation(WD,WA2,WC) + areaCalculation(WC,WA2,WB) + areaCalculation(WA2, WB, WA); if ((std::floor(sum*100)/100.f) <= full_area) return true;
107  sum = areaCalculation(WA,WB2,WD) + areaCalculation(WD,WB2,WC) + areaCalculation(WC,WB2,WB) + areaCalculation(WB2, WB, WA); if ((std::floor(sum*100)/100.f) <= full_area) return true;
108  sum = areaCalculation(WA,WC2,WD) + areaCalculation(WD,WC2,WC) + areaCalculation(WC,WC2,WB) + areaCalculation(WC2, WB, WA); if ((std::floor(sum*100)/100.f) <= full_area) return true;
109  sum = areaCalculation(WA,WD2,WD) + areaCalculation(WD,WD2,WC) + areaCalculation(WC,WD2,WB) + areaCalculation(WD2, WB, WA); if ((std::floor(sum*100)/100.f) <= full_area) return true;
110  }
111  }
112  return false;
113 }
CetiRobot::observers
std::vector< std::unique_ptr< AbstractRobotElementDecorator > > & observers()
Get observers.
Definition: ceti_robot.h:53
CetiRobot::inCollision
bool inCollision(CetiRobot *R)
Collsion calculation.
Definition: ceti_robot.cpp:73
CetiRobot::observers_
std::vector< std::unique_ptr< AbstractRobotElementDecorator > > observers_
Wing shared pointers.
Definition: ceti_robot.h:16
ceti_robot.h
AbstractRobot::areaCalculation
float areaCalculation(tf2::Transform &A, tf2::Transform &B, tf2::Transform &C)
Triangle area calculator.
Definition: abstract_robot.cpp:22
AbstractRobot::bounds_
std::vector< tf2::Transform > bounds_
Bounds of table top surface.
Definition: abstract_robot.h:31
CetiRobot::notify
void notify() override
Observer pattern.
Definition: ceti_robot.cpp:68
CetiRobot::observer_mask_
std::bitset< 3 > observer_mask_
Bitmap to set observers.
Definition: ceti_robot.h:17
CetiRobot::reset
void reset()
Resset all robot properties.
Definition: ceti_robot.cpp:63
AbstractRobot::root_tf_
tf2::Transform root_tf_
Robot root on table top.
Definition: abstract_robot.h:30
AbstractRobot
Abstract Robot implementation.
Definition: abstract_robot.h:25
CetiRobot::CetiRobot
CetiRobot(std::string &name, tf2::Transform tf, tf2::Vector3 size)
Ceti robot constructor.
Definition: ceti_robot.cpp:4
CetiRobot::size
tf2::Vector3 & size() override
Definition: ceti_robot.h:34
CetiRobot
Concrete Ceti-Robot.
Definition: ceti_robot.h:14
CetiRobot::checkSingleObjectCollision
bool checkSingleObjectCollision(tf2::Transform &obj, std::string &robot_element, std::bitset< 3 > &panel_mask) override
Collsion calculation for single objects.
Definition: ceti_robot.cpp:10
AbstractRobot::tf_
tf2::Transform tf_
Pose of table.
Definition: abstract_robot.h:29
CetiRobot::tf
tf2::Transform & tf() override
Definition: ceti_robot.h:33
AbstractRobot::robot_root_bounds_
std::vector< tf2::Transform > robot_root_bounds_
Bounds of robot arm as sub-region of table top.
Definition: abstract_robot.h:32


multi_cell_builder
Author(s): Matteo Anedda
autogenerated on Sun Apr 9 2023 23:59:51