Add inverse kinematics example with implementations by Sam Buss.
Uses Kuka IIWA model description and 4 methods: Selectively Damped Least Squares,Damped Least Squares, Jacobi Transpose, Jacobi Pseudo Inverse Tweak some PD values in Inverse Dynamics example and Robot example.
This commit is contained in:
erwin coumans
346
examples/ThirdPartyLibs/BussIK/Misc.cpp
Normal file
346
examples/ThirdPartyLibs/BussIK/Misc.cpp
Normal file
@@ -0,0 +1,346 @@
|
||||
/*
|
||||
*
|
||||
* Mathematics Subpackage (VrMath)
|
||||
*
|
||||
*
|
||||
* Author: Samuel R. Buss, sbuss@ucsd.edu.
|
||||
* Web page: http://math.ucsd.edu/~sbuss/MathCG
|
||||
*
|
||||
*
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*
|
||||
*
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
#include "LinearR3.h"
|
||||
|
||||
#include "../OpenGLWindow/OpenGLInclude.h"
|
||||
|
||||
/****************************************************************
|
||||
Axes
|
||||
*****************************************************************/
|
||||
static float xx[] = {
|
||||
0., 1., 0., 1.
|
||||
};
|
||||
|
||||
static float xy[] = {
|
||||
-.5, .5, .5, -.5
|
||||
};
|
||||
|
||||
static int xorder[] = {
|
||||
1, 2, -3, 4
|
||||
};
|
||||
|
||||
|
||||
static float yx[] = {
|
||||
0., 0., -.5, .5
|
||||
};
|
||||
|
||||
static float yy[] = {
|
||||
0.f, .6f, 1.f, 1.f
|
||||
};
|
||||
|
||||
static int yorder[] = {
|
||||
1, 2, 3, -2, 4
|
||||
};
|
||||
|
||||
|
||||
static float zx[] = {
|
||||
1., 0., 1., 0., .25, .75
|
||||
};
|
||||
|
||||
static float zy[] = {
|
||||
.5, .5, -.5, -.5, 0., 0.
|
||||
};
|
||||
|
||||
static int zorder[] = {
|
||||
1, 2, 3, 4, -5, 6
|
||||
};
|
||||
|
||||
#define LENFRAC 0.10
|
||||
#define BASEFRAC 1.10
|
||||
|
||||
void Axes( float length )
|
||||
{
|
||||
int i, j; /* counters */
|
||||
float fact; /* character scale factor */
|
||||
float base; /* character start location */
|
||||
|
||||
glBegin( GL_LINE_STRIP );
|
||||
glVertex3f( length, 0., 0. );
|
||||
glVertex3f( 0., 0., 0. );
|
||||
glVertex3f( 0., length, 0. );
|
||||
glEnd();
|
||||
glBegin( GL_LINE_STRIP );
|
||||
glVertex3f( 0., 0., 0. );
|
||||
glVertex3f( 0., 0., length );
|
||||
glEnd();
|
||||
|
||||
fact = LENFRAC * length;
|
||||
base = BASEFRAC * length;
|
||||
|
||||
glBegin( GL_LINE_STRIP );
|
||||
for( i = 0; i < 4; i++ )
|
||||
{
|
||||
j = xorder[i];
|
||||
if( j < 0 )
|
||||
{
|
||||
|
||||
glEnd();
|
||||
glBegin( GL_LINE_STRIP );
|
||||
j = -j;
|
||||
}
|
||||
j--;
|
||||
glVertex3f( base + fact*xx[j], fact*xy[j], 0.0 );
|
||||
}
|
||||
glEnd();
|
||||
|
||||
glBegin( GL_LINE_STRIP );
|
||||
for( i = 0; i < 5; i++ )
|
||||
{
|
||||
j = yorder[i];
|
||||
if( j < 0 )
|
||||
{
|
||||
|
||||
glEnd();
|
||||
glBegin( GL_LINE_STRIP );
|
||||
j = -j;
|
||||
}
|
||||
j--;
|
||||
glVertex3f( fact*yx[j], base + fact*yy[j], 0.0 );
|
||||
}
|
||||
glEnd();
|
||||
|
||||
glBegin( GL_LINE_STRIP );
|
||||
for( i = 0; i < 6; i++ )
|
||||
{
|
||||
j = zorder[i];
|
||||
if( j < 0 )
|
||||
{
|
||||
|
||||
glEnd();
|
||||
glBegin( GL_LINE_STRIP );
|
||||
j = -j;
|
||||
}
|
||||
j--;
|
||||
glVertex3f( 0.0, fact*zy[j], base + fact*zx[j] );
|
||||
}
|
||||
glEnd();
|
||||
|
||||
}
|
||||
|
||||
|
||||
/****************************************************************
|
||||
Arrow
|
||||
*****************************************************************/
|
||||
|
||||
/* size of wings as fraction of length: */
|
||||
|
||||
#define WINGS 0.10
|
||||
|
||||
|
||||
/* axes: */
|
||||
|
||||
#define X 1
|
||||
#define Y 2
|
||||
#define Z 3
|
||||
|
||||
|
||||
/* x, y, z, axes: */
|
||||
|
||||
static float axx[3] = { 1., 0., 0. };
|
||||
static float ayy[3] = { 0., 1., 0. };
|
||||
static float azz[3] = { 0., 0., 1. };
|
||||
|
||||
|
||||
|
||||
/* function declarations: */
|
||||
void Arrow( float tail[3], float head[3] );
|
||||
void Arrow( const VectorR3& tail, const VectorR3& head );
|
||||
void cross( float [3], float [3], float [3] );
|
||||
float dot( float [3], float [3] );
|
||||
float unit( float [3], float [3] );
|
||||
|
||||
|
||||
void Arrow( const VectorR3& tail, const VectorR3& head )
|
||||
{
|
||||
float t[3];
|
||||
float h[3];
|
||||
tail.Dump( t );
|
||||
head.Dump( h );
|
||||
Arrow( t, h );
|
||||
}
|
||||
|
||||
|
||||
void Arrow( float tail[3], float head[3] )
|
||||
{
|
||||
float u[3], v[3], w[3]; /* arrow coordinate system */
|
||||
float d; /* wing distance */
|
||||
float x, y, z; /* point to plot */
|
||||
float mag; /* magnitude of major direction */
|
||||
float f; /* fabs of magnitude */
|
||||
int axis; /* which axis is the major */
|
||||
|
||||
|
||||
/* set w direction in u-v-w coordinate system: */
|
||||
|
||||
w[0] = head[0] - tail[0];
|
||||
w[1] = head[1] - tail[1];
|
||||
w[2] = head[2] - tail[2];
|
||||
|
||||
|
||||
/* determine major direction: */
|
||||
|
||||
axis = X;
|
||||
mag = fabs( w[0] );
|
||||
if( (f=fabs(w[1])) > mag )
|
||||
{
|
||||
axis = Y;
|
||||
mag = f;
|
||||
}
|
||||
if( (f=fabs(w[2])) > mag )
|
||||
{
|
||||
axis = Z;
|
||||
mag = f;
|
||||
}
|
||||
|
||||
|
||||
/* set size of wings and turn w into a unit vector: */
|
||||
|
||||
d = WINGS * unit( w, w );
|
||||
|
||||
|
||||
/* draw the shaft of the arrow: */
|
||||
|
||||
glBegin( GL_LINE_STRIP );
|
||||
glVertex3fv( tail );
|
||||
glVertex3fv( head );
|
||||
glEnd();
|
||||
|
||||
/* draw two sets of wings in the non-major directions: */
|
||||
|
||||
if( axis != X )
|
||||
{
|
||||
cross( w, axx, v );
|
||||
(void) unit( v, v );
|
||||
cross( v, w, u );
|
||||
x = head[0] + d * ( u[0] - w[0] );
|
||||
y = head[1] + d * ( u[1] - w[1] );
|
||||
z = head[2] + d * ( u[2] - w[2] );
|
||||
glBegin( GL_LINE_STRIP );
|
||||
glVertex3fv( head );
|
||||
glVertex3f( x, y, z );
|
||||
glEnd();
|
||||
x = head[0] + d * ( -u[0] - w[0] );
|
||||
y = head[1] + d * ( -u[1] - w[1] );
|
||||
z = head[2] + d * ( -u[2] - w[2] );
|
||||
glBegin( GL_LINE_STRIP );
|
||||
glVertex3fv( head );
|
||||
glVertex3f( x, y, z );
|
||||
glEnd();
|
||||
}
|
||||
|
||||
|
||||
if( axis != Y )
|
||||
{
|
||||
cross( w, ayy, v );
|
||||
(void) unit( v, v );
|
||||
cross( v, w, u );
|
||||
x = head[0] + d * ( u[0] - w[0] );
|
||||
y = head[1] + d * ( u[1] - w[1] );
|
||||
z = head[2] + d * ( u[2] - w[2] );
|
||||
glBegin( GL_LINE_STRIP );
|
||||
glVertex3fv( head );
|
||||
glVertex3f( x, y, z );
|
||||
glEnd();
|
||||
x = head[0] + d * ( -u[0] - w[0] );
|
||||
y = head[1] + d * ( -u[1] - w[1] );
|
||||
z = head[2] + d * ( -u[2] - w[2] );
|
||||
glBegin( GL_LINE_STRIP );
|
||||
glVertex3fv( head );
|
||||
glVertex3f( x, y, z );
|
||||
glEnd();
|
||||
}
|
||||
|
||||
if( axis != Z )
|
||||
{
|
||||
cross( w, azz, v );
|
||||
(void) unit( v, v );
|
||||
cross( v, w, u );
|
||||
x = head[0] + d * ( u[0] - w[0] );
|
||||
y = head[1] + d * ( u[1] - w[1] );
|
||||
z = head[2] + d * ( u[2] - w[2] );
|
||||
glBegin( GL_LINE_STRIP );
|
||||
glVertex3fv( head );
|
||||
glVertex3f( x, y, z );
|
||||
glEnd();
|
||||
x = head[0] + d * ( -u[0] - w[0] );
|
||||
y = head[1] + d * ( -u[1] - w[1] );
|
||||
z = head[2] + d * ( -u[2] - w[2] );
|
||||
glBegin( GL_LINE_STRIP );
|
||||
glVertex3fv( head );
|
||||
glVertex3f( x, y, z );
|
||||
glEnd();
|
||||
}
|
||||
|
||||
|
||||
/* done: */
|
||||
|
||||
}
|
||||
|
||||
float dot( float v1[3], float v2[3] )
|
||||
{
|
||||
return( v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2] );
|
||||
}
|
||||
|
||||
|
||||
|
||||
void
|
||||
cross( float v1[3], float v2[3], float vout[3] )
|
||||
{
|
||||
float tmp[3];
|
||||
|
||||
tmp[0] = v1[1]*v2[2] - v2[1]*v1[2];
|
||||
tmp[1] = v2[0]*v1[2] - v1[0]*v2[2];
|
||||
tmp[2] = v1[0]*v2[1] - v2[0]*v1[1];
|
||||
|
||||
vout[0] = tmp[0];
|
||||
vout[1] = tmp[1];
|
||||
vout[2] = tmp[2];
|
||||
}
|
||||
|
||||
|
||||
|
||||
float
|
||||
unit( float vin[3], float vout[3] )
|
||||
{
|
||||
float dist, f ;
|
||||
|
||||
dist = vin[0]*vin[0] + vin[1]*vin[1] + vin[2]*vin[2];
|
||||
|
||||
if( dist > 0.0 )
|
||||
{
|
||||
dist = sqrt( dist );
|
||||
f = 1. / dist;
|
||||
vout[0] = f * vin[0];
|
||||
vout[1] = f * vin[1];
|
||||
vout[2] = f * vin[2];
|
||||
}
|
||||
else
|
||||
{
|
||||
vout[0] = vin[0];
|
||||
vout[1] = vin[1];
|
||||
vout[2] = vin[2];
|
||||
}
|
||||
|
||||
return( dist );
|
||||
}
|
||||
Reference in New Issue
Block a user