307 lines
10 KiB
C++
307 lines
10 KiB
C++
// SPDX-License-Identifier: LGPL-2.1-or-later
|
|
|
|
/***************************************************************************
|
|
* Copyright (c) 2002 Jürgen Riegel <juergen.riegel@web.de> *
|
|
* *
|
|
* This file is part of the FreeCAD CAx development system. *
|
|
* *
|
|
* This library is free software; you can redistribute it and/or *
|
|
* modify it under the terms of the GNU Library General Public *
|
|
* License as published by the Free Software Foundation; either *
|
|
* version 2 of the License, or (at your option) any later version. *
|
|
* *
|
|
* This library is distributed in the hope that it will be useful, *
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
|
|
* GNU Library General Public License for more details. *
|
|
* *
|
|
* You should have received a copy of the GNU Library General Public *
|
|
* License along with this library; see the file COPYING.LIB. If not, *
|
|
* write to the Free Software Foundation, Inc., 59 Temple Place, *
|
|
* Suite 330, Boston, MA 02111-1307, USA *
|
|
* *
|
|
***************************************************************************/
|
|
|
|
#include "kdl_cp/chainfksolverpos_recursive.hpp"
|
|
#include "kdl_cp/chainiksolverpos_nr_jl.hpp"
|
|
#include "kdl_cp/chainiksolvervel_pinv.hpp"
|
|
|
|
#include <Base/FileInfo.h>
|
|
#include <Base/Reader.h>
|
|
#include <Base/Stream.h>
|
|
#include <Base/Tools.h>
|
|
#include <Base/Writer.h>
|
|
|
|
#include "Robot6Axis.h"
|
|
#include "RobotAlgos.h"
|
|
|
|
namespace Robot
|
|
{
|
|
|
|
// clang-format off
|
|
// some default roboter
|
|
AxisDefinition KukaIR500[6] = {
|
|
// a ,alpha ,d ,theta ,rotDir ,maxAngle ,minAngle ,AxisVelocity
|
|
{500 ,-90 ,1045 ,0 , -1 ,+185 ,-185 ,156 }, // Axis 1
|
|
{1300 ,0 ,0 ,0 , 1 ,+35 ,-155 ,156 }, // Axis 2
|
|
{55 ,+90 ,0 ,-90 , 1 ,+154 ,-130 ,156 }, // Axis 3
|
|
{0 ,-90 ,-1025,0 , 1 ,+350 ,-350 ,330 }, // Axis 4
|
|
{0 ,+90 ,0 ,0 , 1 ,+130 ,-130 ,330 }, // Axis 5
|
|
{0 ,+180 ,-300 ,0 , 1 ,+350 ,-350 ,615 } // Axis 6
|
|
};
|
|
// clang-format on
|
|
|
|
|
|
TYPESYSTEM_SOURCE(Robot::Robot6Axis, Base::Persistence)
|
|
|
|
Robot6Axis::Robot6Axis()
|
|
: Actual(KDL::JntArray(6))
|
|
, Min(KDL::JntArray(6))
|
|
, Max(KDL::JntArray(6))
|
|
{
|
|
// set to default kuka 500
|
|
setKinematic(KukaIR500);
|
|
}
|
|
|
|
void Robot6Axis::setKinematic(const AxisDefinition KinDef[6])
|
|
{
|
|
KDL::Chain temp;
|
|
|
|
|
|
for (int i = 0; i < 6; i++) {
|
|
temp.addSegment(
|
|
KDL::Segment(
|
|
KDL::Joint(KDL::Joint::RotZ),
|
|
KDL::Frame::DH(
|
|
KinDef[i].a,
|
|
Base::toRadians<double>(KinDef[i].alpha),
|
|
KinDef[i].d,
|
|
Base::toRadians<double>(KinDef[i].theta)
|
|
)
|
|
)
|
|
);
|
|
RotDir[i] = KinDef[i].rotDir;
|
|
Max(i) = Base::toRadians<double>(KinDef[i].maxAngle);
|
|
Min(i) = Base::toRadians<double>(KinDef[i].minAngle);
|
|
Velocity[i] = KinDef[i].velocity;
|
|
}
|
|
|
|
// for now and testing
|
|
Kinematic = temp;
|
|
|
|
// get the actual TCP out of the axis
|
|
calcTcp();
|
|
}
|
|
|
|
double Robot6Axis::getMaxAngle(int Axis)
|
|
{
|
|
return Base::toDegrees<double>(Max(Axis));
|
|
}
|
|
|
|
double Robot6Axis::getMinAngle(int Axis)
|
|
{
|
|
return Base::toDegrees<double>(Min(Axis));
|
|
}
|
|
|
|
void split(std::string const& string, const char delimiter, std::vector<std::string>& destination)
|
|
{
|
|
std::string::size_type last_position(0);
|
|
std::string::size_type position(0);
|
|
|
|
for (std::string::const_iterator it(string.begin()); it != string.end(); ++it, ++position) {
|
|
if (*it == delimiter) {
|
|
destination.push_back(string.substr(last_position, position - last_position));
|
|
last_position = position + 1;
|
|
}
|
|
}
|
|
destination.push_back(string.substr(last_position, position - last_position));
|
|
}
|
|
|
|
void Robot6Axis::readKinematic(const char* FileName)
|
|
{
|
|
char buf[120];
|
|
Base::FileInfo fi(FileName);
|
|
Base::ifstream in(fi);
|
|
if (!in) {
|
|
return;
|
|
}
|
|
|
|
std::vector<std::string> destination;
|
|
AxisDefinition temp[6];
|
|
|
|
// over read the header
|
|
in.getline(buf, 119, '\n');
|
|
// read 6 Axis
|
|
for (auto& i : temp) {
|
|
in.getline(buf, 79, '\n');
|
|
destination.clear();
|
|
split(std::string(buf), ',', destination);
|
|
if (destination.size() < 8) {
|
|
continue;
|
|
}
|
|
// transfer the values in kinematic structure
|
|
i.a = atof(destination[0].c_str());
|
|
i.alpha = atof(destination[1].c_str());
|
|
i.d = atof(destination[2].c_str());
|
|
i.theta = atof(destination[3].c_str());
|
|
i.rotDir = atof(destination[4].c_str());
|
|
i.maxAngle = atof(destination[5].c_str());
|
|
i.minAngle = atof(destination[6].c_str());
|
|
i.velocity = atof(destination[7].c_str());
|
|
}
|
|
|
|
setKinematic(temp);
|
|
}
|
|
|
|
unsigned int Robot6Axis::getMemSize() const
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
void Robot6Axis::Save(Base::Writer& writer) const
|
|
{
|
|
for (unsigned int i = 0; i < 6; i++) {
|
|
Base::Placement Tip = toPlacement(Kinematic.getSegment(i).getFrameToTip());
|
|
writer.Stream() << writer.ind() << "<Axis "
|
|
<< "Px=\"" << Tip.getPosition().x << "\" "
|
|
<< "Py=\"" << Tip.getPosition().y << "\" "
|
|
<< "Pz=\"" << Tip.getPosition().z << "\" "
|
|
<< "Q0=\"" << Tip.getRotation()[0] << "\" "
|
|
<< "Q1=\"" << Tip.getRotation()[1] << "\" "
|
|
<< "Q2=\"" << Tip.getRotation()[2] << "\" "
|
|
<< "Q3=\"" << Tip.getRotation()[3] << "\" "
|
|
<< "rotDir=\"" << RotDir[i] << "\" "
|
|
<< "maxAngle=\"" << Base::toDegrees<double>(Max(i)) << "\" "
|
|
<< "minAngle=\"" << Base::toDegrees<double>(Min(i)) << "\" "
|
|
<< "AxisVelocity=\"" << Velocity[i] << "\" "
|
|
<< "Pos=\"" << Actual(i) << "\"/>" << std::endl;
|
|
}
|
|
}
|
|
|
|
void Robot6Axis::Restore(Base::XMLReader& reader)
|
|
{
|
|
KDL::Chain Temp;
|
|
Base::Placement Tip;
|
|
|
|
for (unsigned int i = 0; i < 6; i++) {
|
|
// read my Element
|
|
reader.readElement("Axis");
|
|
// get the value of the placement
|
|
Tip = Base::Placement(
|
|
Base::Vector3d(
|
|
reader.getAttribute<double>("Px"),
|
|
reader.getAttribute<double>("Py"),
|
|
reader.getAttribute<double>("Pz")
|
|
),
|
|
Base::Rotation(
|
|
reader.getAttribute<double>("Q0"),
|
|
reader.getAttribute<double>("Q1"),
|
|
reader.getAttribute<double>("Q2"),
|
|
reader.getAttribute<double>("Q3")
|
|
)
|
|
);
|
|
Temp.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ), toFrame(Tip)));
|
|
|
|
|
|
if (reader.hasAttribute("rotDir")) {
|
|
Velocity[i] = reader.getAttribute<double>("rotDir");
|
|
}
|
|
else {
|
|
Velocity[i] = 1.0;
|
|
}
|
|
// read the axis constraints
|
|
Min(i) = Base::toRadians<double>(reader.getAttribute<double>("maxAngle"));
|
|
Max(i) = Base::toRadians<double>(reader.getAttribute<double>("minAngle"));
|
|
if (reader.hasAttribute("AxisVelocity")) {
|
|
Velocity[i] = reader.getAttribute<double>("AxisVelocity");
|
|
}
|
|
else {
|
|
Velocity[i] = 156.0;
|
|
}
|
|
Actual(i) = reader.getAttribute<double>("Pos");
|
|
}
|
|
Kinematic = Temp;
|
|
|
|
calcTcp();
|
|
}
|
|
|
|
bool Robot6Axis::setTo(const Base::Placement& To)
|
|
{
|
|
// Creation of the solvers:
|
|
KDL::ChainFkSolverPos_recursive fksolver1(Kinematic); // Forward position solver
|
|
KDL::ChainIkSolverVel_pinv iksolver1v(Kinematic); // Inverse velocity solver
|
|
KDL::ChainIkSolverPos_NR_JL iksolver1(
|
|
Kinematic,
|
|
Min,
|
|
Max,
|
|
fksolver1,
|
|
iksolver1v,
|
|
100,
|
|
1e-6
|
|
); // Maximum 100 iterations, stop at accuracy 1e-6
|
|
|
|
// Creation of jntarrays:
|
|
KDL::JntArray result(Kinematic.getNrOfJoints());
|
|
|
|
// Set destination frame
|
|
KDL::Frame F_dest = KDL::Frame(
|
|
KDL::Rotation::Quaternion(
|
|
To.getRotation()[0],
|
|
To.getRotation()[1],
|
|
To.getRotation()[2],
|
|
To.getRotation()[3]
|
|
),
|
|
KDL::Vector(To.getPosition()[0], To.getPosition()[1], To.getPosition()[2])
|
|
);
|
|
|
|
// solve
|
|
if (iksolver1.CartToJnt(Actual, F_dest, result) < 0) {
|
|
return false;
|
|
}
|
|
else {
|
|
Actual = result;
|
|
Tcp = F_dest;
|
|
return true;
|
|
}
|
|
}
|
|
|
|
Base::Placement Robot6Axis::getTcp()
|
|
{
|
|
double x, y, z, w;
|
|
Tcp.M.GetQuaternion(x, y, z, w);
|
|
return Base::Placement(Base::Vector3d(Tcp.p[0], Tcp.p[1], Tcp.p[2]), Base::Rotation(x, y, z, w));
|
|
}
|
|
|
|
bool Robot6Axis::calcTcp()
|
|
{
|
|
// Create solver based on kinematic chain
|
|
KDL::ChainFkSolverPos_recursive fksolver = KDL::ChainFkSolverPos_recursive(Kinematic);
|
|
|
|
// Create the frame that will contain the results
|
|
KDL::Frame cartpos;
|
|
|
|
// Calculate forward position kinematics
|
|
int kinematics_status;
|
|
kinematics_status = fksolver.JntToCart(Actual, cartpos);
|
|
if (kinematics_status >= 0) {
|
|
Tcp = cartpos;
|
|
return true;
|
|
}
|
|
else {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
bool Robot6Axis::setAxis(int Axis, double Value)
|
|
{
|
|
Actual(Axis) = RotDir[Axis] * Base::toRadians<double>(Value);
|
|
return calcTcp();
|
|
}
|
|
|
|
double Robot6Axis::getAxis(int Axis)
|
|
{
|
|
return RotDir[Axis] * Base::toDegrees<double>(Actual(Axis));
|
|
}
|
|
|
|
} /* namespace Robot */
|