ArmKine.cpp
#include "armKine.h"
#include <math.h>
#include <stdio.h>
#include <corecrt_math_defines.h>
#define UR5_PARAMS
#define IKFAST_MAIN
namespace ur_kinematics {
namespace {
const double ZERO_THRESH = 0.00000001;
int SIGN(double x) {
return (x > 0) - (x < 0);
}
const double PI = M_PI;
//#define UR10_PARAMS
#ifdef UR10_PARAMS
const double d1 = 0.1273;
const double a2 = -0.612;
const double a3 = -0.5723;
const double d4 = 0.163941;
const double d5 = 0.1157;
const double d6 = 0.0922;
#endif
//#define UR5_PARAMS
#ifdef UR5_PARAMS
const double d1 = 0.089159;
const double a2 = -0.42500;
const double a3 = -0.39225;
const double d4 = 0.10915;
const double d5 = 0.09465;
const double d6 = 0.0823;
#endif
//#define UR3_PARAMS
#ifdef UR3_PARAMS
const double d1 = 0.1519;
const double a2 = -0.24365;
const double a3 = -0.21325;
const double d4 = 0.11235;
const double d5 = 0.08535;
const double d6 = 0.0819;
#endif
}
void forward(const double* q, double* T) {
double s1 = sin(*q), c1 = cos(*q); q++;
double q23 = *q, q234 = *q, s2 = sin(*q), c2 = cos(*q); q++;
double s3 = sin(*q), c3 = cos(*q); q23 += *q; q234 += *q; q++;
double s4 = sin(*q), c4 = cos(*q); q234 += *q; q++;
double s5 = sin(*q), c5 = cos(*q); q++;
double s6 = sin(*q), c6 = cos(*q);
double s23 = sin(q23), c23 = cos(q23);
double s234 = sin(q234), c234 = cos(q234);
*T = c234 * c1*s5 - c5 * s1; T++;
*T = c6 * (s1*s5 + c234 * c1*c5) - s234 * c1*s6; T++;
*T = -s6 * (s1*s5 + c234 * c1*c5) - s234 * c1*c6; T++;
*T = d6 * c234*c1*s5 - a3 * c23*c1 - a2 * c1*c2 - d6 * c5*s1 - d5 * s234*c1 - d4 * s1; T++;
*T = c1 * c5 + c234 * s1*s5; T++;
*T = -c6 * (c1*s5 - c234 * c5*s1) - s234 * s1*s6; T++;
*T = s6 * (c1*s5 - c234 * c5*s1) - s234 * c6*s1; T++;
*T = d6 * (c1*c5 + c234 * s1*s5) + d4 * c1 - a3 * c23*s1 - a2 * c2*s1 - d5 * s234*s1; T++;
*T = -s234 * s5; T++;
*T = -c234 * s6 - s234 * c5*c6; T++;
*T = s234 * c5*s6 - c234 * c6; T++;
*T = d1 + a3 * s23 + a2 * s2 - d5 * (c23*c4 - s23 * s4) - d6 * s5*(c23*s4 + s23 * c4); T++;
*T = 0.0; T++; *T = 0.0; T++; *T = 0.0; T++; *T = 1.0;
}
void forward_all(const double* q, double* T1, double* T2, double* T3,
double* T4, double* T5, double* T6) {
double s1 = sin(*q), c1 = cos(*q); q++; // q1
double q23 = *q, q234 = *q, s2 = sin(*q), c2 = cos(*q); q++; // q2
double s3 = sin(*q), c3 = cos(*q); q23 += *q; q234 += *q; q++; // q3
q234 += *q; q++; // q4
double s5 = sin(*q), c5 = cos(*q); q++; // q5
double s6 = sin(*q), c6 = cos(*q); // q6
double s23 = sin(q23), c23 = cos(q23);
double s234 = sin(q234), c234 = cos(q234);
if (T1 != NULL) {
*T1 = c1; T1++;
*T1 = 0; T1++;
*T1 = s1; T1++;
*T1 = 0; T1++;
*T1 = s1; T1++;
*T1 = 0; T1++;
*T1 = -c1; T1++;
*T1 = 0; T1++;
*T1 = 0; T1++;
*T1 = 1; T1++;
*T1 = 0; T1++;
*T1 = d1; T1++;
*T1 = 0; T1++;
*T1 = 0; T1++;
*T1 = 0; T1++;
*T1 = 1; T1++;
}
if (T2 != NULL) {
*T2 = c1 * c2; T2++;
*T2 = -c1 * s2; T2++;
*T2 = s1; T2++;
*T2 = a2 * c1*c2; T2++;
*T2 = c2 * s1; T2++;
*T2 = -s1 * s2; T2++;
*T2 = -c1; T2++;
*T2 = a2 * c2*s1; T2++;
*T2 = s2; T2++;
*T2 = c2; T2++;
*T2 = 0; T2++;
*T2 = d1 + a2 * s2; T2++;
*T2 = 0; T2++;
*T2 = 0; T2++;
*T2 = 0; T2++;
*T2 = 1; T2++;
}
if (T3 != NULL) {
*T3 = c23 * c1; T3++;
*T3 = -s23 * c1; T3++;
*T3 = s1; T3++;
*T3 = c1 * (a3*c23 + a2 * c2); T3++;
*T3 = c23 * s1; T3++;
*T3 = -s23 * s1; T3++;
*T3 = -c1; T3++;
*T3 = s1 * (a3*c23 + a2 * c2); T3++;
*T3 = s23; T3++;
*T3 = c23; T3++;
*T3 = 0; T3++;
*T3 = d1 + a3 * s23 + a2 * s2; T3++;
*T3 = 0; T3++;
*T3 = 0; T3++;
*T3 = 0; T3++;
*T3 = 1; T3++;
}
if (T4 != NULL) {
*T4 = c234 * c1; T4++;
*T4 = s1; T4++;
*T4 = s234 * c1; T4++;
*T4 = c1 * (a3*c23 + a2 * c2) + d4 * s1; T4++;
*T4 = c234 * s1; T4++;
*T4 = -c1; T4++;
*T4 = s234 * s1; T4++;
*T4 = s1 * (a3*c23 + a2 * c2) - d4 * c1; T4++;
*T4 = s234; T4++;
*T4 = 0; T4++;
*T4 = -c234; T4++;
*T4 = d1 + a3 * s23 + a2 * s2; T4++;
*T4 = 0; T4++;
*T4 = 0; T4++;
*T4 = 0; T4++;
*T4 = 1; T4++;
}
if (T5 != NULL) {
*T5 = s1 * s5 + c234 * c1*c5; T5++;
*T5 = -s234 * c1; T5++;
*T5 = c5 * s1 - c234 * c1*s5; T5++;
*T5 = c1 * (a3*c23 + a2 * c2) + d4 * s1 + d5 * s234*c1; T5++;
*T5 = c234 * c5*s1 - c1 * s5; T5++;
*T5 = -s234 * s1; T5++;
*T5 = -c1 * c5 - c234 * s1*s5; T5++;
*T5 = s1 * (a3*c23 + a2 * c2) - d4 * c1 + d5 * s234*s1; T5++;
*T5 = s234 * c5; T5++;
*T5 = c234; T5++;
*T5 = -s234 * s5; T5++;
*T5 = d1 + a3 * s23 + a2 * s2 - d5 * c234; T5++;
*T5 = 0; T5++;
*T5 = 0; T5++;
*T5 = 0; T5++;
*T5 = 1; T5++;
}
if (T6 != NULL) {
*T6 = c6 * (s1*s5 + c234 * c1*c5) - s234 * c1*s6; T6++;
*T6 = -s6 * (s1*s5 + c234 * c1*c5) - s234 * c1*c6; T6++;
*T6 = c5 * s1 - c234 * c1*s5; T6++;
*T6 = d6 * (c5*s1 - c234 * c1*s5) + c1 * (a3*c23 + a2 * c2) + d4 * s1 + d5 * s234*c1; T6++;
*T6 = -c6 * (c1*s5 - c234 * c5*s1) - s234 * s1*s6; T6++;
*T6 = s6 * (c1*s5 - c234 * c5*s1) - s234 * c6*s1; T6++;
*T6 = -c1 * c5 - c234 * s1*s5; T6++;
*T6 = s1 * (a3*c23 + a2 * c2) - d4 * c1 - d6 * (c1*c5 + c234 * s1*s5) + d5 * s234*s1; T6++;
*T6 = c234 * s6 + s234 * c5*c6; T6++;
*T6 = c234 * c6 - s234 * c5*s6; T6++;
*T6 = -s234 * s5; T6++;
*T6 = d1 + a3 * s23 + a2 * s2 - d5 * c234 - d6 * s234*s5; T6++;
*T6 = 0; T6++;
*T6 = 0; T6++;
*T6 = 0; T6++;
*T6 = 1; T6++;
}
}
int inverse(const double* T, double* q_sols, double q6_des) {
int num_sols = 0;
double T02 = -*T; T++; double T00 = *T; T++; double T01 = *T; T++; double T03 = -*T; T++;
double T12 = -*T; T++; double T10 = *T; T++; double T11 = *T; T++; double T13 = -*T; T++;
double T22 = *T; T++; double T20 = -*T; T++; double T21 = -*T; T++; double T23 = *T;
// shoulder rotate joint (q1) //
double q1[2];
{
double A = d6 * T12 - T13;
double B = d6 * T02 - T03;
double R = A * A + B * B;
if (fabs(A) < ZERO_THRESH) {
double div;
if (fabs(fabs(d4) - fabs(B)) < ZERO_THRESH)
div = -SIGN(d4)*SIGN(B);
else
div = -d4 / B;
double arcsin = asin(div);
if (fabs(arcsin) < ZERO_THRESH)
arcsin = 0.0;
if (arcsin < 0.0)
q1[0] = arcsin + 2.0*PI;
else
q1[0] = arcsin;
q1[1] = PI - arcsin;
}
else if (fabs(B) < ZERO_THRESH) {
double div;
if (fabs(fabs(d4) - fabs(A)) < ZERO_THRESH)
div = SIGN(d4)*SIGN(A);
else
div = d4 / A;
double arccos = acos(div);
q1[0] = arccos;
q1[1] = 2.0*PI - arccos;
}
else if (d4*d4 > R) {
return num_sols;
}
else {
double arccos = acos(d4 / sqrt(R));
double arctan = atan2(-B, A);
double pos = arccos + arctan;
double neg = -arccos + arctan;
if (fabs(pos) < ZERO_THRESH)
pos = 0.0;
if (fabs(neg) < ZERO_THRESH)
neg = 0.0;
if (pos >= 0.0)
q1[0] = pos;
else
q1[0] = 2.0*PI + pos;
if (neg >= 0.0)
q1[1] = neg;
else
q1[1] = 2.0*PI + neg;
}
}
// wrist 2 joint (q5) //
double q5[2][2];
{
for (int i = 0; i < 2; i++) {
double numer = (T03*sin(q1[i]) - T13 * cos(q1[i]) - d4);
double div;
if (fabs(fabs(numer) - fabs(d6)) < ZERO_THRESH)
div = SIGN(numer) * SIGN(d6);
else
div = numer / d6;
double arccos = acos(div);
q5[i][0] = arccos;
q5[i][1] = 2.0*PI - arccos;
}
}
{
for (int i = 0; i < 2; i++) {
for (int j = 0; j < 2; j++) {
double c1 = cos(q1[i]), s1 = sin(q1[i]);
double c5 = cos(q5[i][j]), s5 = sin(q5[i][j]);
double q6;
// wrist 3 joint (q6) //
if (fabs(s5) < ZERO_THRESH)
q6 = q6_des;
else {
q6 = atan2(SIGN(s5)*-(T01*s1 - T11 * c1),
SIGN(s5)*(T00*s1 - T10 * c1));
if (fabs(q6) < ZERO_THRESH)
q6 = 0.0;
if (q6 < 0.0)
q6 += 2.0*PI;
}
double q2[2], q3[2], q4[2];
// RRR joints(q2, q3, q4)
double c6 = cos(q6), s6 = sin(q6);
double x04x = -s5 * (T02*c1 + T12 * s1) - c5 * (s6*(T01*c1 + T11 * s1) - c6 * (T00*c1 + T10 * s1));
double x04y = c5 * (T20*c6 - T21 * s6) - T22 * s5;
double p13x = d5 * (s6*(T00*c1 + T10 * s1) + c6 * (T01*c1 + T11 * s1)) - d6 * (T02*c1 + T12 * s1) +
T03 * c1 + T13 * s1;
double p13y = T23 - d1 - d6 * T22 + d5 * (T21*c6 + T20 * s6);
double c3 = (p13x*p13x + p13y * p13y - a2 * a2 - a3 * a3) / (2.0*a2*a3);
if (fabs(fabs(c3) - 1.0) < ZERO_THRESH)
c3 = SIGN(c3);
else if (fabs(c3) > 1.0) {
// TODO NO SOLUTION
continue;
}
double arccos = acos(c3);
q3[0] = arccos;
q3[1] = 2.0*PI - arccos;
double denom = a2 * a2 + a3 * a3 + 2 * a2*a3*c3;
double s3 = sin(arccos);
double A = (a2 + a3 * c3), B = a3 * s3;
q2[0] = atan2((A*p13y - B * p13x) / denom, (A*p13x + B * p13y) / denom);
q2[1] = atan2((A*p13y + B * p13x) / denom, (A*p13x - B * p13y) / denom);
double c23_0 = cos(q2[0] + q3[0]);
double s23_0 = sin(q2[0] + q3[0]);
double c23_1 = cos(q2[1] + q3[1]);
double s23_1 = sin(q2[1] + q3[1]);
q4[0] = atan2(c23_0*x04y - s23_0 * x04x, x04x*c23_0 + x04y * s23_0);
q4[1] = atan2(c23_1*x04y - s23_1 * x04x, x04x*c23_1 + x04y * s23_1);
for (int k = 0; k < 2; k++) {
if (fabs(q2[k]) < ZERO_THRESH)
q2[k] = 0.0;
else if (q2[k] < 0.0) q2[k] += 2.0*PI;
if (fabs(q4[k]) < ZERO_THRESH)
q4[k] = 0.0;
else if (q4[k] < 0.0) q4[k] += 2.0*PI;
q_sols[num_sols * 6 + 0] = q1[i]; q_sols[num_sols * 6 + 1] = q2[k];
q_sols[num_sols * 6 + 2] = q3[k]; q_sols[num_sols * 6 + 3] = q4[k];
q_sols[num_sols * 6 + 4] = q5[i][j]; q_sols[num_sols * 6 + 5] = q6;
num_sols++;
}
}
}
}
return num_sols;
}
};
#define IKFAST_HAS_LIBRARY
#include "ikfast.h"
using namespace ikfast;
// check if the included ikfast version matches what this file was compiled with
#define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)x]
IKFAST_COMPILE_ASSERT(IKFAST_VERSION == 0x10000048);
#ifdef IKFAST_NAMESPACE
namespace IKFAST_NAMESPACE {
#endif
void to_mat44(double * mat4_4, const IkReal* eetrans, const IkReal* eerot)
{
for (int i = 0; i < 3; ++i) {
mat4_4[i * 4 + 0] = eerot[i * 3 + 0];
mat4_4[i * 4 + 1] = eerot[i * 3 + 1];
mat4_4[i * 4 + 2] = eerot[i * 3 + 2];
mat4_4[i * 4 + 3] = eetrans[i];
}
mat4_4[3 * 4 + 0] = 0;
mat4_4[3 * 4 + 1] = 0;
mat4_4[3 * 4 + 2] = 0;
mat4_4[3 * 4 + 3] = 1;
}
void from_mat44(const double * mat4_4, IkReal* eetrans, IkReal* eerot)
{
for (int i = 0; i < 3; ++i) {
eerot[i * 3 + 0] = mat4_4[i * 4 + 0];
eerot[i * 3 + 1] = mat4_4[i * 4 + 1];
eerot[i * 3 + 2] = mat4_4[i * 4 + 2];
eetrans[i] = mat4_4[i * 4 + 3];
}
}
IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
if (!pfree) return false;
int n = GetNumJoints();
double q_sols[8 * 6];
double T[16];
to_mat44(T, eetrans, eerot);
int num_sols = ur_kinematics::inverse(T, q_sols, pfree[0]);
std::vector<int> vfree(0);
for (int i = 0; i < num_sols; ++i) {
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(n);
for (int j = 0; j < n; ++j) vinfos[j].foffset = q_sols[i*n + j];
solutions.AddSolution(vinfos, vfree);
}
return num_sols > 0;
}
IKFAST_API void ComputeFk(const IkReal* j, IkReal* eetrans, IkReal* eerot)
{
double T[16];
ur_kinematics::forward(j, T);
from_mat44(T, eetrans, eerot);
}
IKFAST_API int GetNumFreeParameters() { return 1; }
IKFAST_API int* GetFreeParameters() { static int freeparams[] = { 5 }; return freeparams; }
IKFAST_API int GetNumJoints() { return 6; }
IKFAST_API int GetIkRealSize() { return sizeof(IkReal); }
#ifdef IKFAST_NAMESPACE
} // end namespace
#endif
#ifndef IKFAST_NO_MAIN
using namespace std;
using namespace ur_kinematics;
int main(int argc, char* argv[])
{
double q[6] = { 0.0, 0.0, 1.0, 0.0, 1.0, 0.0 };
double* T = new double[16];
forward(q, T);
for (int i = 0; i < 4; i++) {
for (int j = i * 4; j < (i + 1) * 4; j++)
printf("%1.3f ", T[j]);
printf("\n");
}
double q_sols[8 * 6];
int num_sols;
num_sols = inverse(T, q_sols);
for (int i = 0; i < num_sols; i++)
printf("%1.6f %1.6f %1.6f %1.6f %1.6f %1.6f\n",
q_sols[i * 6 + 0], q_sols[i * 6 + 1], q_sols[i * 6 + 2], q_sols[i * 6 + 3], q_sols[i * 6 + 4], q_sols[i * 6 + 5]);
for (int i = 0; i <= 4; i++)
printf("%f ", PI / 2.0*i);
printf("\n");
return 0;
}
#endifikfast.h
#pragma once
// -*- coding: utf-8 -*-
// Copyright (C) 2012-2014 Rosen Diankov <rosen.diankov@gmail.com>
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/** \brief Header file for all ikfast c++ files/shared objects.
The ikfast inverse kinematics compiler is part of OpenRAVE.
The file is divided into two sections:
- <b>Common</b> - the abstract classes section that all ikfast share regardless of their settings
- <b>Library Specific</b> - the library-specific definitions, which depends on the precision/settings that the
library was compiled with
The defines are as follows, they are also used for the ikfast C++ class:
- IKFAST_HEADER_COMMON - common classes
- IKFAST_HAS_LIBRARY - if defined, will include library-specific functions. by default this is off
- IKFAST_CLIBRARY - Define this linking statically or dynamically to get correct visibility.
- IKFAST_NO_MAIN - Remove the ``main`` function, usually used with IKFAST_CLIBRARY
- IKFAST_ASSERT - Define in order to get a custom assert called when NaNs, divides by zero, and other invalid
conditions are detected.
- IKFAST_REAL - Use to force a custom real number type for IkReal.
- IKFAST_NAMESPACE - Enclose all functions and classes in this namespace, the ``main`` function is excluded.
*/
#include <vector>
#include <list>
#include <stdexcept>
#include <cmath>
#ifndef IKFAST_HEADER_COMMON
#define IKFAST_HEADER_COMMON
/// should be the same as ikfast.__version__
/// if 0x10000000 bit is set, then the iksolver assumes 6D transforms are done without the manipulator offset taken into
/// account (allows to reuse IK when manipulator offset changes)
#define IKFAST_VERSION 0x10000048
namespace ikfast
{
/// \brief holds the solution for a single dof
template < typename T >
class IkSingleDOFSolutionBase
{
public:
IkSingleDOFSolutionBase() : fmul(0), foffset(0), freeind(-1), maxsolutions(1)
{
indices[0] = indices[1] = indices[2] = indices[3] = indices[4] = -1;
}
T fmul, foffset; ///< joint value is fmul*sol[freeind]+foffset
signed char freeind; ///< if >= 0, mimics another joint
unsigned char jointtype; ///< joint type, 0x01 is revolute, 0x11 is slider
unsigned char maxsolutions; ///< max possible indices, 0 if controlled by free index or a free joint itself
unsigned char indices[5]; ///< unique index of the solution used to keep track on what part it came from. sometimes a
///solution can be repeated for different indices. store at least another repeated root
};
/// \brief The discrete solutions are returned in this structure.
///
/// Sometimes the joint axes of the robot can align allowing an infinite number of solutions.
/// Stores all these solutions in the form of free variables that the user has to set when querying the solution. Its
/// prototype is:
template < typename T >
class IkSolutionBase
{
public:
virtual ~IkSolutionBase()
{
}
/// \brief gets a concrete solution
///
/// \param[out] solution the result
/// \param[in] freevalues values for the free parameters \se GetFree
virtual void GetSolution(T* solution, const T* freevalues) const = 0;
/// \brief std::vector version of \ref GetSolution
virtual void GetSolution(std::vector< T >& solution, const std::vector< T >& freevalues) const
{
solution.resize(GetDOF());
GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL);
}
/// \brief Gets the indices of the configuration space that have to be preset before a full solution can be returned
///
/// 0 always points to the first value accepted by the ik function.
/// \return vector of indices indicating the free parameters
virtual const std::vector< int >& GetFree() const = 0;
/// \brief the dof of the solution
virtual const int GetDOF() const = 0;
};
/// \brief manages all the solutions
template < typename T >
class IkSolutionListBase
{
public:
virtual ~IkSolutionListBase()
{
}
/// \brief add one solution and return its index for later retrieval
///
/// \param vinfos Solution data for each degree of freedom of the manipulator
/// \param vfree If the solution represents an infinite space, holds free parameters of the solution that users can
/// freely set. The indices are of the configuration that the IK solver accepts rather than the entire robot, ie 0
/// points to the first value accepted.
virtual size_t AddSolution(const std::vector< IkSingleDOFSolutionBase< T > >& vinfos,
const std::vector< int >& vfree) = 0;
/// \brief returns the solution pointer
virtual const IkSolutionBase< T >& GetSolution(size_t index) const = 0;
/// \brief returns the number of solutions stored
virtual size_t GetNumSolutions() const = 0;
/// \brief clears all current solutions, note that any memory addresses returned from \ref GetSolution will be
/// invalidated.
virtual void Clear() = 0;
};
/// \brief holds function pointers for all the exported functions of ikfast
template < typename T >
class IkFastFunctions
{
public:
IkFastFunctions()
: _ComputeIk(NULL)
, _ComputeIk2(NULL)
, _ComputeFk(NULL)
, _GetNumFreeParameters(NULL)
, _GetFreeParameters(NULL)
, _GetNumJoints(NULL)
, _GetIkRealSize(NULL)
, _GetIkFastVersion(NULL)
, _GetIkType(NULL)
, _GetKinematicsHash(NULL)
{
}
virtual ~IkFastFunctions()
{
}
typedef bool(*ComputeIkFn)(const T*, const T*, const T*, IkSolutionListBase< T >&);
ComputeIkFn _ComputeIk;
typedef bool(*ComputeIk2Fn)(const T*, const T*, const T*, IkSolutionListBase< T >&, void*);
ComputeIk2Fn _ComputeIk2;
typedef void(*ComputeFkFn)(const T*, T*, T*);
ComputeFkFn _ComputeFk;
typedef int(*GetNumFreeParametersFn)();
GetNumFreeParametersFn _GetNumFreeParameters;
typedef int* (*GetFreeParametersFn)();
GetFreeParametersFn _GetFreeParameters;
typedef int(*GetNumJointsFn)();
GetNumJointsFn _GetNumJoints;
typedef int(*GetIkRealSizeFn)();
GetIkRealSizeFn _GetIkRealSize;
typedef const char* (*GetIkFastVersionFn)();
GetIkFastVersionFn _GetIkFastVersion;
typedef int(*GetIkTypeFn)();
GetIkTypeFn _GetIkType;
typedef const char* (*GetKinematicsHashFn)();
GetKinematicsHashFn _GetKinematicsHash;
};
// Implementations of the abstract classes, user doesn't need to use them
/// \brief Default implementation of \ref IkSolutionBase
template < typename T >
class IkSolution : public IkSolutionBase< T >
{
public:
IkSolution(const std::vector< IkSingleDOFSolutionBase< T > >& vinfos, const std::vector< int >& vfree)
{
_vbasesol = vinfos;
_vfree = vfree;
}
virtual void GetSolution(T* solution, const T* freevalues) const
{
for (std::size_t i = 0; i < _vbasesol.size(); ++i)
{
if (_vbasesol[i].freeind < 0)
solution[i] = _vbasesol[i].foffset;
else
{
solution[i] = freevalues[_vbasesol[i].freeind] * _vbasesol[i].fmul + _vbasesol[i].foffset;
if (solution[i] > T(3.14159265358979))
{
solution[i] -= T(6.28318530717959);
}
else if (solution[i] < T(-3.14159265358979))
{
solution[i] += T(6.28318530717959);
}
}
}
}
virtual void GetSolution(std::vector< T >& solution, const std::vector< T >& freevalues) const
{
solution.resize(GetDOF());
GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL);
}
virtual const std::vector< int >& GetFree() const
{
return _vfree;
}
virtual const int GetDOF() const
{
return static_cast<int>(_vbasesol.size());
}
virtual void Validate() const
{
for (size_t i = 0; i < _vbasesol.size(); ++i)
{
if (_vbasesol[i].maxsolutions == (unsigned char)-1)
{
throw std::runtime_error("max solutions for joint not initialized");
}
if (_vbasesol[i].maxsolutions > 0)
{
if (_vbasesol[i].indices[0] >= _vbasesol[i].maxsolutions)
{
throw std::runtime_error("index >= max solutions for joint");
}
if (_vbasesol[i].indices[1] != (unsigned char)-1 && _vbasesol[i].indices[1] >= _vbasesol[i].maxsolutions)
{
throw std::runtime_error("2nd index >= max solutions for joint");
}
}
if (!std::isfinite(_vbasesol[i].foffset))
{
throw std::runtime_error("foffset was not finite");
}
}
}
virtual void GetSolutionIndices(std::vector< unsigned int >& v) const
{
v.resize(0);
v.push_back(0);
for (int i = (int)_vbasesol.size() - 1; i >= 0; --i)
{
if (_vbasesol[i].maxsolutions != (unsigned char)-1 && _vbasesol[i].maxsolutions > 1)
{
for (size_t j = 0; j < v.size(); ++j)
{
v[j] *= _vbasesol[i].maxsolutions;
}
size_t orgsize = v.size();
if (_vbasesol[i].indices[1] != (unsigned char)-1)
{
for (size_t j = 0; j < orgsize; ++j)
{
v.push_back(v[j] + _vbasesol[i].indices[1]);
}
}
if (_vbasesol[i].indices[0] != (unsigned char)-1)
{
for (size_t j = 0; j < orgsize; ++j)
{
v[j] += _vbasesol[i].indices[0];
}
}
}
}
}
std::vector< IkSingleDOFSolutionBase< T > > _vbasesol; ///< solution and their offsets if joints are mimiced
std::vector< int > _vfree;
};
/// \brief Default implementation of \ref IkSolutionListBase
template < typename T >
class IkSolutionList : public IkSolutionListBase< T >
{
public:
virtual size_t AddSolution(const std::vector< IkSingleDOFSolutionBase< T > >& vinfos, const std::vector< int >& vfree)
{
size_t index = _listsolutions.size();
_listsolutions.push_back(IkSolution< T >(vinfos, vfree));
return index;
}
virtual const IkSolutionBase< T >& GetSolution(size_t index) const
{
if (index >= _listsolutions.size())
{
throw std::runtime_error("GetSolution index is invalid");
}
typename std::list< IkSolution< T > >::const_iterator it = _listsolutions.begin();
std::advance(it, index);
return *it;
}
virtual size_t GetNumSolutions() const
{
return _listsolutions.size();
}
virtual void Clear()
{
_listsolutions.clear();
}
protected:
std::list< IkSolution< T > > _listsolutions;
};
}
#endif // OPENRAVE_IKFAST_HEADER
// The following code is dependent on the C++ library linking with.
#ifdef IKFAST_HAS_LIBRARY
// defined when creating a shared object/dll
#ifdef IKFAST_CLIBRARY
#ifdef _MSC_VER
#define IKFAST_API extern "C" __declspec(dllexport)
#else
#define IKFAST_API extern "C"
#endif
#else
#define IKFAST_API
#endif
#ifdef IKFAST_NAMESPACE
namespace IKFAST_NAMESPACE
{
#endif
#ifdef IKFAST_REAL
typedef IKFAST_REAL IkReal;
#else
typedef double IkReal;
#endif
/** \brief Computes all IK solutions given a end effector coordinates and the free joints.
- ``eetrans`` - 3 translation values. For iktype **TranslationXYOrientation3D**, the z-axis is the orientation.
- ``eerot``
- For **Transform6D** it is 9 values for the 3x3 rotation matrix.
- For **Direction3D**, **Ray4D**, and **TranslationDirection5D**, the first 3 values represent the target direction.
- For **TranslationXAxisAngle4D**, **TranslationYAxisAngle4D**, and **TranslationZAxisAngle4D** the first value
represents the angle.
- For **TranslationLocalGlobal6D**, the diagonal elements ([0],[4],[8]) are the local translation inside the end
effector coordinate system.
*/
IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree,
ikfast::IkSolutionListBase< IkReal >& solutions);
/** \brief Similar to ComputeIk except takes OpenRAVE boost::shared_ptr<RobotBase::Manipulator>*
*/
IKFAST_API bool ComputeIk2(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree,
ikfast::IkSolutionListBase< IkReal >& solutions, void* pOpenRAVEManip);
/// \brief Computes the end effector coordinates given the joint values. This function is used to double check ik.
IKFAST_API void ComputeFk(const IkReal* joints, IkReal* eetrans, IkReal* eerot);
/// \brief returns the number of free parameters users has to set apriori
IKFAST_API int GetNumFreeParameters();
/// \brief the indices of the free parameters indexed by the chain joints
IKFAST_API int* GetFreeParameters();
/// \brief the total number of indices of the chain
IKFAST_API int GetNumJoints();
/// \brief the size in bytes of the configured number type
IKFAST_API int GetIkRealSize();
/// \brief the ikfast version used to generate this file
IKFAST_API const char* GetIkFastVersion();
/// \brief the ik type ID
IKFAST_API int GetIkType();
/// \brief a hash of all the chain values used for double checking that the correct IK is used.
IKFAST_API const char* GetKinematicsHash();
#ifdef IKFAST_NAMESPACE
}
#endif
#endif // IKFAST_HAS_LIBRARY
armKine.h
#pragma once
#ifndef UR_KIN_H
#define UR_KIN_H
// These kinematics find the tranfrom from the base link to the end effector.
// Though the raw D-H parameters specify a transform from the 0th link to the 6th link,
// offset transforms are specified in this formulation.
// To work with the raw D-H kinematics, use the inverses of the transforms below.
// Transform from base link to 0th link
// -1, 0, 0, 0
// 0, -1, 0, 0
// 0, 0, 1, 0
// 0, 0, 0, 1
// Transform from 6th link to end effector
// 0, -1, 0, 0
// 0, 0, -1, 0
// 1, 0, 0, 0
// 0, 0, 0, 1
namespace ur_kinematics {
// @param q The 6 joint values
// @param T The 4x4 end effector pose in row-major ordering
void forward(const double* q, double* T);
// @param q The 6 joint values
// @param Ti The 4x4 link i pose in row-major ordering. If NULL, nothing is stored.
void forward_all(const double* q, double* T1, double* T2, double* T3,
double* T4, double* T5, double* T6);
// @param T The 4x4 end effector pose in row-major ordering
// @param q_sols An 8x6 array of doubles returned, all angles should be in [0,2*PI)
// @param q6_des An optional parameter which designates what the q6 value should take
// in case of an infinite solution on that joint.
// @return Number of solutions found (maximum of 8)
int inverse(const double* T, double* q_sols, double q6_des = 0.0);
};
#endif //UR_KIN_H结果得到：
和RoboDK验证（区别应该来自于参数精度，实际UR参数应该从系统calibration文件中获取）
【参考】【UR六轴机械臂运动学逆解算法C++（MoveIT!）_机械臂逆解 c++_RealMadrid1920的博客-CSDN博客】