Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

preparation for KUKA IK tracking example

Browse Source
This commit is contained in:
Erwin Coumans
2016-08-30 14:44:11 -07:00
parent 900fd86d58
commit a30ff20e6b
12 changed files with 535 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
examples/ThirdPartyLibs/BussIK/Jacobian.cpp
View File

@@ -36,7 +36,7 @@ using namespace std;
void Arrow(const VectorR3& tail, const VectorR3& head);
//extern RestPositionOn;
extern VectorR3 target1[];
//extern VectorR3 target1[];
// Optimal damping values have to be determined in an ad hoc manner (Yuck!)
const double Jacobian::DefaultDampingLambda = 0.6; // Optimal for the "Y" shape (any lower gives jitter)
@@ -96,7 +96,7 @@ void Jacobian::Reset()
// Compute the deltaS vector, dS, (the error in end effector positions
// Compute the J and K matrices (the Jacobians)
void Jacobian::ComputeJacobian()
void Jacobian::ComputeJacobian(VectorR3* targets)
{
// Traverse tree to find all end effectors
VectorR3 temp;
@@ -104,7 +104,7 @@ void Jacobian::ComputeJacobian()
while ( n ) {
if ( n->IsEffector() ) {
int i = n->GetEffectorNum();
const VectorR3& targetPos = target1[i];
const VectorR3& targetPos = targets[i];
// Compute the delta S value (differences from end effectors to target positions.
temp = targetPos;
@@ -418,7 +418,7 @@ void Jacobian::CalcdTClampedFromdS()
}
}
double Jacobian::UpdateErrorArray()
double Jacobian::UpdateErrorArray(VectorR3* targets)
{
double totalError = 0.0;
@@ -428,7 +428,7 @@ double Jacobian::UpdateErrorArray()
while ( n ) {
if ( n->IsEffector() ) {
int i = n->GetEffectorNum();
const VectorR3& targetPos = target1[i];
const VectorR3& targetPos = targets[i];
temp = targetPos;
temp -= n->GetS();
double err = temp.Norm();
@@ -440,7 +440,7 @@ double Jacobian::UpdateErrorArray()
return totalError;
}
void Jacobian::UpdatedSClampValue()
void Jacobian::UpdatedSClampValue(VectorR3* targets)
{
// Traverse tree to find all end effectors
VectorR3 temp;
@@ -448,7 +448,7 @@ void Jacobian::UpdatedSClampValue()
while ( n ) {
if ( n->IsEffector() ) {
int i = n->GetEffectorNum();
const VectorR3& targetPos = target1[i];
const VectorR3& targetPos = targets[i];
// Compute the delta S value (differences from end effectors to target positions.
// While we are at it, also update the clamping values in dSclamp;

6
examples/ThirdPartyLibs/BussIK/Jacobian.h
View File

@@ -55,7 +55,7 @@ class Jacobian {
public:
Jacobian(Tree*);
void ComputeJacobian();
void ComputeJacobian(VectorR3* targets);
const MatrixRmn& ActiveJacobian() const { return *Jactive; }
void SetJendActive() { Jactive = &Jend; } // The default setting is Jend.
void SetJtargetActive() { Jactive = &Jtarget; }
@@ -69,9 +69,9 @@ public:
void CalcDeltaThetasSDLS();
void UpdateThetas();
double UpdateErrorArray(); // Returns sum of errors
double UpdateErrorArray(VectorR3* targets); // Returns sum of errors
const VectorRn& GetErrorArray() const { return errorArray; }
void UpdatedSClampValue();
void UpdatedSClampValue(VectorR3* targets);
void SetCurrentMode( UpdateMode mode ) { CurrentUpdateMode = mode; }
UpdateMode GetCurrentMode() const { return CurrentUpdateMode; }