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:
46
examples/ThirdPartyLibs/BussIK/VectorRn.cpp
Normal file
46
examples/ThirdPartyLibs/BussIK/VectorRn.cpp
Normal file
@@ -0,0 +1,46 @@
|
||||
/*
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
//
|
||||
// VectorRn: Vector over Rn (Variable length vector)
|
||||
//
|
||||
|
||||
#include "VectorRn.h"
|
||||
|
||||
VectorRn VectorRn::WorkVector;
|
||||
|
||||
double VectorRn::MaxAbs () const
|
||||
{
|
||||
double result = 0.0;
|
||||
double* t = x;
|
||||
for ( long i = length; i>0; i-- ) {
|
||||
if ( (*t) > result ) {
|
||||
result = *t;
|
||||
}
|
||||
else if ( -(*t) > result ) {
|
||||
result = -(*t);
|
||||
}
|
||||
t++;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
Reference in New Issue
Block a user