75e86051c2
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.
347 lines
6.6 KiB
C++
347 lines
6.6 KiB
C++
/*
|
|
*
|
|
* 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 );
|
|
}
|