diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index a6abd4fd7..bf4ac2233 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -136,6 +136,10 @@ static ExampleEntry gDefaultExamples[]= ExampleEntry(0, "Inverse Kinematics"), ExampleEntry(1, "SDLS", "Selectively Damped Least Squares by Sam Buss. Example configures the IK tree of a Kuka IIWA", InverseKinematicsExampleCreateFunc, IK_SDLS), ExampleEntry(1, "DLS", "Damped Least Squares by Sam Buss. Example configures the IK tree of a Kuka IIWA", InverseKinematicsExampleCreateFunc, IK_DLS), + ExampleEntry(1, "DLS-SVD", "Damped Least Squares with Singular Value Decomposition by Sam Buss. Example configures the IK tree of a Kuka IIWA", InverseKinematicsExampleCreateFunc, IK_DLS_SVD), + + + ExampleEntry(1, "Jacobi Transpose", "Jacobi Transpose by Sam Buss. Example configures the IK tree of a Kuka IIWA", InverseKinematicsExampleCreateFunc, IK_JACOB_TRANS), ExampleEntry(1, "Jacobi Pseudo Inv", "Jacobi Pseudo Inverse Method by Sam Buss. Example configures the IK tree of a Kuka IIWA", InverseKinematicsExampleCreateFunc, IK_PURE_PSEUDO), diff --git a/examples/InverseKinematics/InverseKinematicsExample.cpp b/examples/InverseKinematics/InverseKinematicsExample.cpp index ae1ed8f17..8f902b008 100644 --- a/examples/InverseKinematics/InverseKinematicsExample.cpp +++ b/examples/InverseKinematics/InverseKinematicsExample.cpp @@ -113,6 +113,9 @@ void DoUpdateStep(double Tstep, Tree & treeY, Jacobian *jacob, int ikMethod) { case IK_DLS: jacob->CalcDeltaThetasDLS(); // Damped least squares method break; + case IK_DLS_SVD: + jacob->CalcDeltaThetasDLSwithSVD(); + break; case IK_PURE_PSEUDO: jacob->CalcDeltaThetasPseudoinverse(); // Pure pseudoinverse method break; diff --git a/examples/InverseKinematics/InverseKinematicsExample.h b/examples/InverseKinematics/InverseKinematicsExample.h index ca0d1e3b5..845925ef6 100644 --- a/examples/InverseKinematics/InverseKinematicsExample.h +++ b/examples/InverseKinematics/InverseKinematicsExample.h @@ -1,7 +1,7 @@ #ifndef INVERSE_KINEMATICSEXAMPLE_H #define INVERSE_KINEMATICSEXAMPLE_H -enum Method {IK_JACOB_TRANS=0, IK_PURE_PSEUDO, IK_DLS, IK_SDLS }; +enum Method {IK_JACOB_TRANS=0, IK_PURE_PSEUDO, IK_DLS, IK_SDLS , IK_DLS_SVD}; class CommonExampleInterface* InverseKinematicsExampleCreateFunc(struct CommonExampleOptions& options); diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp index 646513b86..2b0454ea8 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp +++ b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp @@ -8,7 +8,7 @@ * * * Author: Samuel R. Buss, sbuss@ucsd.edu. -* Web page: http://math.ucsd.edu/~sbuss/MathCG +* Web page: http://www.math.ucsd.edu/~sbuss/ResearchWeb/ikmethods/index.html * * This software is provided 'as-is', without any express or implied warranty. @@ -30,7 +30,6 @@ subject to the following restrictions: #include using namespace std; -#include "../OpenGLWindow/OpenGLInclude.h" #include "Jacobian.h" @@ -468,35 +467,7 @@ void Jacobian::UpdatedSClampValue() } } -void Jacobian::DrawEigenVectors() const -{ - int i, j; - VectorR3 tail; - VectorR3 head; - Node *node; - for (i=0; iGetEffector(j); - tail = node->GetS(); - U.GetTriple( j, i, &head ); - head += tail; - glDisable(GL_LIGHTING); - glColor3f(1.0f, 0.2f, 0.0f); - glLineWidth(2.0); - glBegin(GL_LINES); - glVertex3f(tail.x, tail.y, tail.z); - glVertex3f(head.x, head.y, tail.z); - glEnd(); - Arrow(tail, head); - glLineWidth(1.0); - glEnable(GL_LIGHTING); - } - } -} void Jacobian::CompareErrors( const Jacobian& j1, const Jacobian& j2, double* weightedDist1, double* weightedDist2 ) { diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.h b/examples/ThirdPartyLibs/BussIK/Jacobian.h index 1c0bc4572..b1c683f8a 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.h +++ b/examples/ThirdPartyLibs/BussIK/Jacobian.h @@ -9,7 +9,7 @@ * * * Author: Samuel R. Buss, sbuss@ucsd.edu. -* Web page: http://math.ucsd.edu/~sbuss/MathCG +* Web page: http://www.math.ucsd.edu/~sbuss/ResearchWeb/ikmethods/index.html * * This software is provided 'as-is', without any express or implied warranty. @@ -72,7 +72,6 @@ public: double UpdateErrorArray(); // Returns sum of errors const VectorRn& GetErrorArray() const { return errorArray; } void UpdatedSClampValue(); - void DrawEigenVectors() const; void SetCurrentMode( UpdateMode mode ) { CurrentUpdateMode = mode; } UpdateMode GetCurrentMode() const { return CurrentUpdateMode; } diff --git a/examples/ThirdPartyLibs/BussIK/LinearR4.h b/examples/ThirdPartyLibs/BussIK/LinearR4.h index ee995326c..51f57a666 100644 --- a/examples/ThirdPartyLibs/BussIK/LinearR4.h +++ b/examples/ThirdPartyLibs/BussIK/LinearR4.h @@ -195,7 +195,7 @@ public: inline double Diagonal( int ); inline void MakeTranspose(); // Transposes it. - void Matrix4x4::operator*= (const Matrix4x4& B); // Matrix product + void operator*= (const Matrix4x4& B); // Matrix product Matrix4x4& ReNormalize(); }; diff --git a/examples/ThirdPartyLibs/BussIK/Misc.cpp b/examples/ThirdPartyLibs/BussIK/Misc.cpp index 03f34812b..6aa696d55 100644 --- a/examples/ThirdPartyLibs/BussIK/Misc.cpp +++ b/examples/ThirdPartyLibs/BussIK/Misc.cpp @@ -69,75 +69,6 @@ static int zorder[] = { #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 @@ -164,138 +95,14 @@ 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] ) { diff --git a/examples/ThirdPartyLibs/BussIK/Tree.cpp b/examples/ThirdPartyLibs/BussIK/Tree.cpp index a82e7f1d7..6891f0601 100644 --- a/examples/ThirdPartyLibs/BussIK/Tree.cpp +++ b/examples/ThirdPartyLibs/BussIK/Tree.cpp @@ -8,7 +8,7 @@ * * * Author: Samuel R. Buss, sbuss@ucsd.edu. -* Web page: http://math.ucsd.edu/~sbuss/MathCG +* Web page: http://www.math.ucsd.edu/~sbuss/ResearchWeb/ikmethods/index.html * * This software is provided 'as-is', without any express or implied warranty. diff --git a/examples/ThirdPartyLibs/BussIK/Tree.h b/examples/ThirdPartyLibs/BussIK/Tree.h index 208176817..673f41c9e 100644 --- a/examples/ThirdPartyLibs/BussIK/Tree.h +++ b/examples/ThirdPartyLibs/BussIK/Tree.h @@ -8,7 +8,7 @@ * * * Author: Samuel R. Buss, sbuss@ucsd.edu. -* Web page: http://math.ucsd.edu/~sbuss/MathCG +* Web page: http://www.math.ucsd.edu/~sbuss/ResearchWeb/ikmethods/index.html * * This software is provided 'as-is', without any express or implied warranty.