abstract_robot.cpp
Go to the documentation of this file.
1 #include "robot/abstract_robot.h"
2 
3 
4 Abstract_robot::Abstract_robot(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(), size.getY(), tf.getOrigin().getZ()))); // ++
11  bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-size.getX(), size.getY(), tf.getOrigin().getZ()))); // +-
12  bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-size.getX(), -size.getY(), tf.getOrigin().getZ()))); //-+
13  bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3( size.getX(), -size.getY(), tf.getOrigin().getZ()))); // --
14 
15  // root
16  robot_root_bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.113f, 0.095f, 0))); // ++
17  robot_root_bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.113f, 0.095f, 0))); // +-
18  robot_root_bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.113f, -0.095f, 0)));
19  robot_root_bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.113f, -0.095f, 0)));
20 }
21 
22 float Abstract_robot::area_calculation(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 }
Abstract_robot::root_tf_
tf2::Transform root_tf_
Robot root on table top.
Definition: impl/abstract_robot.h:30
Abstract_robot::area_calculation
float area_calculation(tf2::Transform &A, tf2::Transform &B, tf2::Transform &C)
Triangle area calculator.
Definition: abstract_robot.cpp:22
abstract_robot.h
Abstract_robot::bounds_
std::vector< tf2::Transform > bounds_
Bounds of table top surface.
Definition: impl/abstract_robot.h:31
Abstract_robot::size
tf2::Vector3 & size()
Definition: impl/abstract_robot.h:59
Abstract_robot::Abstract_robot
Abstract_robot(std::string &name, tf2::Transform tf, tf2::Vector3 size)
Definition: impl/abstract_robot.h:38
Abstract_robot::tf
tf2::Transform & tf()
Definition: impl/abstract_robot.h:58
Abstract_robot::robot_root_bounds_
std::vector< tf2::Transform > robot_root_bounds_
Bounds of robot arm as sub-region of table top.
Definition: impl/abstract_robot.h:32


multi_cell_builder
Author(s): MA
autogenerated on Thu Jan 12 2023 23:45:43