The representation for a 6-Axis industry grade robot. More...
#include <Robot6Axis.h>
Public Member Functions | |
bool | calcTcp (void) |
calculate the new Tcp out of the Axis | |
double | getAxis (int Axis) |
double | getMaxAngle (int Axis) |
virtual unsigned int | getMemSize (void) const |
This method is used to get the size of objects It is not meant to have the exact size, it is more or less an estimation which runs fast! Is it two bytes or a GB? | |
double | getMinAngle (int Axis) |
Base::Placement | getTcp (void) |
virtual Base::Type | getTypeId (void) const |
void | readKinematic (const char *FileName) |
read the kinematic parameters of the robot from a file | |
virtual void | Restore (Base::XMLReader &) |
This method is used to restore properties from an XML document. | |
Robot6Axis () | |
virtual void | Save (Base::Writer &) const |
This method is used to save properties to an XML document. | |
bool | setAxis (int Axis, double Value) |
void | setKinematic (const AxisDefinition KinDef[6]) |
set the kinematic parameters of the robot | |
bool | setTo (const Base::Placement &To) |
set the robot to that position, calculates the Axis | |
~Robot6Axis () | |
Static Public Member Functions | |
static void * | create (void) |
static Base::Type | getClassTypeId (void) |
static void | init (void) |
Protected Attributes | |
KDL::JntArray | Actuall |
KDL::Chain | Kinematic |
KDL::JntArray | Max |
KDL::JntArray | Min |
double | RotDir [6] |
KDL::Frame | Tcp |
double | Velocity [6] |
The representation for a 6-Axis industry grade robot.
Definition at line 51 of file Robot6Axis.h.
Robot6Axis::Robot6Axis | ( | ) |
Definition at line 72 of file Robot6Axis.cpp.
References Actuall, Max, Min, and setKinematic().
Robot6Axis::~Robot6Axis | ( | ) |
Definition at line 85 of file Robot6Axis.cpp.
bool Robot6Axis::calcTcp | ( | void | ) |
calculate the new Tcp out of the Axis
Definition at line 265 of file Robot6Axis.cpp.
References Actuall, KDL::ChainFkSolverPos_recursive::JntToCart(), Kinematic, and Tcp.
Referenced by Restore(), setAxis(), and setKinematic().
void * Robot::Robot6Axis::create | ( | void | ) | [static] |
Reimplemented from Base::Persistence.
Definition at line 70 of file Robot6Axis.cpp.
double Robot6Axis::getAxis | ( | int | Axis | ) |
Definition at line 291 of file Robot6Axis.cpp.
References Actuall, M_PI, and RotDir.
Referenced by Robot::RobotObject::onChanged(), Robot::Robot6AxisPy::representation(), Robot::Simulation::reset(), Robot::Simulation::setToTime(), and Robot::Simulation::Simulation().
Base::Type Robot::Robot6Axis::getClassTypeId | ( | void | ) | [static] |
Reimplemented from Base::Persistence.
Definition at line 70 of file Robot6Axis.cpp.
double Robot6Axis::getMaxAngle | ( | int | Axis | ) |
Definition at line 110 of file Robot6Axis.cpp.
Referenced by RobotGui::TaskRobot6Axis::setColor(), and RobotGui::TaskRobot6Axis::setRobot().
unsigned int Robot6Axis::getMemSize | ( | void | ) | const [virtual] |
This method is used to get the size of objects It is not meant to have the exact size, it is more or less an estimation which runs fast! Is it two bytes or a GB?
Implements Base::Persistence.
Definition at line 167 of file Robot6Axis.cpp.
double Robot6Axis::getMinAngle | ( | int | Axis | ) |
Definition at line 115 of file Robot6Axis.cpp.
Referenced by RobotGui::TaskRobot6Axis::setColor(), and RobotGui::TaskRobot6Axis::setRobot().
Base::Placement Robot6Axis::getTcp | ( | void | ) |
Definition at line 258 of file Robot6Axis.cpp.
References KDL::Frame::M, KDL::Frame::p, Tcp, RobotExample::w, MovieTool::x, and MovieTool::y.
Referenced by Robot::RobotObject::onChanged(), Robot::Robot6AxisPy::representation(), and Robot::RobotObject::Restore().
Base::Type Robot::Robot6Axis::getTypeId | ( | void | ) | const [virtual] |
Reimplemented from Base::Persistence.
Definition at line 70 of file Robot6Axis.cpp.
void Robot::Robot6Axis::init | ( | void | ) | [static] |
Reimplemented from Base::Persistence.
Definition at line 70 of file Robot6Axis.cpp.
Referenced by initRobot().
void Robot6Axis::readKinematic | ( | const char * | FileName | ) |
read the kinematic parameters of the robot from a file
Definition at line 136 of file Robot6Axis.cpp.
References Robot::AxisDefinition::a, Robot::AxisDefinition::alpha, Robot::AxisDefinition::d, Robot::AxisDefinition::maxAngle, Robot::AxisDefinition::minAngle, Robot::AxisDefinition::rotDir, setKinematic(), split, Robot::AxisDefinition::theta, and Robot::AxisDefinition::velocity.
Referenced by Robot::RobotObject::onChanged().
void Robot6Axis::Restore | ( | Base::XMLReader & | ) | [virtual] |
This method is used to restore properties from an XML document.
It uses the XMLReader class, which bases on SAX, to read the in Save() written information. Again the Vector as an example:
void PropertyVector::Restore(Base::XMLReader &reader) { // read my Element reader.readElement("PropertyVector"); // get the value of my Attribute _cVec.x = (float)reader.getAttributeAsFloat("valueX"); _cVec.y = (float)reader.getAttributeAsFloat("valueY"); _cVec.z = (float)reader.getAttributeAsFloat("valueZ"); }
Implements Base::Persistence.
Definition at line 195 of file Robot6Axis.cpp.
References Actuall, KDL::Chain::addSegment(), calcTcp(), Base::XMLReader::getAttributeAsFloat(), Base::XMLReader::hasAttribute(), Kinematic, M_PI, Max, Min, Base::XMLReader::readElement(), Robot::toFrame(), and Velocity.
Referenced by Robot::RobotObject::Restore().
void Robot6Axis::Save | ( | Base::Writer & | ) | const [virtual] |
This method is used to save properties to an XML document.
A good example you'll find in PropertyStandard.cpp, e.g. the vector:
void PropertyVector::Save (Writer &writer) const { writer << writer.ind() << "<PropertyVector valueX=\"" << _cVec.x << "\" valueY=\"" << _cVec.y << "\" valueZ=\"" << _cVec.z <<"\"/>" << endl; }
The writer.ind() expression writes the indention, just for pretty printing of the XML. As you see, the writing of the XML document is not done with a DOM implementation because of performance reasons. Therefore the programmer has to take care that a valid XML document is written. This means closing tags and writing UTF-8.
Implements Base::Persistence.
Definition at line 172 of file Robot6Axis.cpp.
References Actuall, KDL::Segment::getFrameToTip(), Base::Placement::getPosition(), Base::Placement::getRotation(), KDL::Chain::getSegment(), Base::Writer::ind(), Kinematic, M_PI, Max, Min, RotDir, Base::Writer::Stream(), Robot::toPlacement(), Velocity, Base::Vector3< _Precision >::x, Base::Vector3< _Precision >::y, and Base::Vector3< _Precision >::z.
Referenced by Robot::RobotObject::Save().
bool Robot6Axis::setAxis | ( | int | Axis, | |
double | Value | |||
) |
Definition at line 284 of file Robot6Axis.cpp.
References Actuall, calcTcp(), M_PI, and RotDir.
Referenced by Robot::RobotObject::onChanged(), Robot::Simulation::reset(), Robot::RobotObject::Restore(), Robot::Robot6AxisPy::setAxis1(), Robot::Robot6AxisPy::setAxis2(), Robot::Robot6AxisPy::setAxis3(), Robot::Robot6AxisPy::setAxis4(), Robot::Robot6AxisPy::setAxis5(), and Robot::Robot6AxisPy::setAxis6().
void Robot6Axis::setKinematic | ( | const AxisDefinition | KinDef[6] | ) |
set the kinematic parameters of the robot
Definition at line 90 of file Robot6Axis.cpp.
References KDL::Chain::addSegment(), calcTcp(), Kinematic, M_PI, Max, Robot::AxisDefinition::maxAngle, Min, Robot::AxisDefinition::minAngle, Robot::AxisDefinition::rotDir, RotDir, Robot::AxisDefinition::velocity, and Velocity.
Referenced by readKinematic(), and Robot6Axis().
bool Robot6Axis::setTo | ( | const Base::Placement & | To | ) |
set the robot to that position, calculates the Axis
Definition at line 235 of file Robot6Axis.cpp.
References Actuall, KDL::ChainIkSolverPos_NR_JL::CartToJnt(), KDL::Chain::getNrOfJoints(), Base::Placement::getPosition(), Base::Placement::getRotation(), Kinematic, Max, Min, KDL::Rotation::Quaternion(), result, and Tcp.
Referenced by Robot::RobotObject::onChanged(), Robot::Simulation::reset(), Robot::RobotObject::Restore(), Robot::Robot6AxisPy::setTcp(), and Robot::Simulation::setToTime().
KDL::JntArray Robot::Robot6Axis::Actuall [protected] |
Definition at line 85 of file Robot6Axis.h.
Referenced by calcTcp(), getAxis(), Restore(), Robot6Axis(), Save(), setAxis(), and setTo().
KDL::Chain Robot::Robot6Axis::Kinematic [protected] |
Definition at line 84 of file Robot6Axis.h.
Referenced by calcTcp(), Restore(), Save(), setKinematic(), and setTo().
KDL::JntArray Robot::Robot6Axis::Max [protected] |
Definition at line 87 of file Robot6Axis.h.
Referenced by getMaxAngle(), Restore(), Robot6Axis(), Save(), setKinematic(), and setTo().
KDL::JntArray Robot::Robot6Axis::Min [protected] |
Definition at line 86 of file Robot6Axis.h.
Referenced by getMinAngle(), Restore(), Robot6Axis(), Save(), setKinematic(), and setTo().
double Robot::Robot6Axis::RotDir[6] [protected] |
Definition at line 91 of file Robot6Axis.h.
Referenced by getAxis(), Save(), setAxis(), and setKinematic().
KDL::Frame Robot::Robot6Axis::Tcp [protected] |
Definition at line 88 of file Robot6Axis.h.
double Robot::Robot6Axis::Velocity[6] [protected] |
Definition at line 90 of file Robot6Axis.h.
Referenced by Restore(), Save(), and setKinematic().