abstract_robot.cpp
Go to the documentation of this file.
1 #include "robot/abstract_robot.h"
2 
3 
4 AbstractRobot::AbstractRobot(std::string name, tf2::Transform tf, tf2::Vector3 size)
5 : name_(name)
6 , tf_(tf)
7 , size_(size){
8  root_tf_= tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.22f, 0, tf.getOrigin().getZ()));
9 
10  bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3( size.getX()*0.5f, size.getY()*0.5f, tf.getOrigin().getZ()))); // ++
11  bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-size.getX()*0.5f, size.getY()*0.5f, tf.getOrigin().getZ()))); // +-
12  bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-size.getX()*0.5f, -size.getY()*0.5f, tf.getOrigin().getZ()))); //-+
13  bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3( size.getX()*0.5f, -size.getY()*0.5f, tf.getOrigin().getZ()))); // --
14 
15  // default root
16  robot_root_bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.1f, 0.1f, 0))); // ++
17  robot_root_bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.1f, 0.1f, 0))); // +-
18  robot_root_bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.1f, -0.1f, 0)));
19  robot_root_bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.1f, -0.1f, 0)));
20 }
21 
22 float AbstractRobot::areaCalculation(tf2::Transform& A, tf2::Transform& B, tf2::Transform& C){
23  return std::abs(
24  (B.getOrigin().getX() * A.getOrigin().getY()) - (A.getOrigin().getX() * B.getOrigin().getY()) +
25  (C.getOrigin().getX() * B.getOrigin().getY()) - (B.getOrigin().getX() * C.getOrigin().getY()) +
26  (A.getOrigin().getX() * C.getOrigin().getY()) - (C.getOrigin().getX() * A.getOrigin().getY()))*0.5f;
27 }
AbstractRobot::AbstractRobot
AbstractRobot(std::string name, tf2::Transform tf, tf2::Vector3 size)
Definition: abstract_robot.cpp:4
abstract_robot.h
AbstractRobot::areaCalculation
float areaCalculation(tf2::Transform &A, tf2::Transform &B, tf2::Transform &C)
Triangle area calculator.
Definition: abstract_robot.cpp:22
AbstractRobot::tf
virtual tf2::Transform & tf()=0
AbstractRobot::bounds_
std::vector< tf2::Transform > bounds_
Bounds of table top surface.
Definition: abstract_robot.h:31
AbstractRobot::root_tf_
tf2::Transform root_tf_
Robot root on table top.
Definition: abstract_robot.h:30
AbstractRobot::size
virtual tf2::Vector3 & size()=0
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