mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-15 06:00:12 +00:00
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.
102 lines
3.0 KiB
C++
102 lines
3.0 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 "LinearR2.h"
|
|
|
|
|
|
#include <assert.h>
|
|
|
|
// ******************************************************
|
|
// * VectorR2 class - math library functions *
|
|
// * * * * * * * * * * * * * * * * * * * * * * * * * * **
|
|
|
|
const VectorR2 VectorR2::Zero(0.0, 0.0);
|
|
const VectorR2 VectorR2::UnitX( 1.0, 0.0);
|
|
const VectorR2 VectorR2::UnitY( 0.0, 1.0);
|
|
const VectorR2 VectorR2::NegUnitX(-1.0, 0.0);
|
|
const VectorR2 VectorR2::NegUnitY( 0.0,-1.0);
|
|
|
|
const Matrix2x2 Matrix2x2::Identity(1.0, 0.0, 0.0, 1.0);
|
|
|
|
// ******************************************************
|
|
// * Matrix2x2 class - math library functions *
|
|
// * * * * * * * * * * * * * * * * * * * * * * * * * * **
|
|
|
|
|
|
// ******************************************************
|
|
// * LinearMapR2 class - math library functions *
|
|
// * * * * * * * * * * * * * * * * * * * * * * * * * * **
|
|
|
|
|
|
LinearMapR2 LinearMapR2::Inverse() const // Returns inverse
|
|
{
|
|
|
|
|
|
register double detInv = 1.0/(m11*m22 - m12*m21) ;
|
|
|
|
return( LinearMapR2( m22*detInv, -m21*detInv, -m12*detInv, m11*detInv ) );
|
|
}
|
|
|
|
LinearMapR2& LinearMapR2::Invert() // Converts into inverse.
|
|
{
|
|
register double detInv = 1.0/(m11*m22 - m12*m21) ;
|
|
|
|
double temp;
|
|
temp = m11*detInv;
|
|
m11= m22*detInv;
|
|
m22=temp;
|
|
m12 = -m12*detInv;
|
|
m21 = -m22*detInv;
|
|
|
|
return ( *this );
|
|
}
|
|
|
|
VectorR2 LinearMapR2::Solve(const VectorR2& u) const // Returns solution
|
|
{
|
|
// Just uses Inverse() for now.
|
|
return ( Inverse()*u );
|
|
}
|
|
|
|
// ******************************************************
|
|
// * RotationMapR2 class - math library functions *
|
|
// * * * * * * * * * * * * * * * * * * * * * * * * * * **
|
|
|
|
|
|
|
|
// ***************************************************************
|
|
// * 2-space vector and matrix utilities *
|
|
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
|
|
|
|
|
|
|
|
|
|
// ***************************************************************
|
|
// Stream Output Routines *
|
|
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
|
|
|
|
ostream& operator<< ( ostream& os, const VectorR2& u )
|
|
{
|
|
return (os << "<" << u.x << "," << u.y << ">");
|
|
}
|
|
|
|
|