diff --git a/examples/SharedMemory/IKTrajectoryHelper.cpp b/examples/SharedMemory/IKTrajectoryHelper.cpp index a6c114a44..3e4537ea9 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.cpp +++ b/examples/SharedMemory/IKTrajectoryHelper.cpp @@ -194,14 +194,22 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3], break; } - m_data->m_ikJacobian->UpdateThetas(); + // Use for velocity IK, update theta dot + m_data->m_ikJacobian->UpdateThetaDot(); + + // Use for position IK, incrementally update theta + //m_data->m_ikJacobian->UpdateThetas(); // Apply the change in the theta values //m_data->m_ikJacobian->UpdatedSClampValue(&targets); for (int i=0;im_ikNodes[i]->GetTheta()*dt + q_current[i]; + + // Use for position IK + //q_new[i] = m_data->m_ikNodes[i]->GetTheta(); } return true; } diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp index c47fc3cc6..6fd6c6aa6 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp +++ b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp @@ -170,6 +170,24 @@ void Jacobian::UpdateThetas() tree->Compute(); } +void Jacobian::UpdateThetaDot() +{ + // Traverse the tree to find all joints + // Update the joint angles + Node* n = tree->GetRoot(); + while ( n ) { + if ( n->IsJoint() ) { + int i = n->GetJointNum(); + n->UpdateTheta( dTheta[i] ); + + } + n = tree->GetSuccessor( n ); + } + + // Update the positions and rotation axes of all joints/effectors + tree->Compute(); +} + void Jacobian::CalcDeltaThetas() { switch (CurrentUpdateMode) { diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.h b/examples/ThirdPartyLibs/BussIK/Jacobian.h index 2663e62c5..38aa475c0 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.h +++ b/examples/ThirdPartyLibs/BussIK/Jacobian.h @@ -72,6 +72,7 @@ public: void CalcThetasDotDLS(float dt); void UpdateThetas(); + void UpdateThetaDot(); double UpdateErrorArray(VectorR3* targets); // Returns sum of errors const VectorRn& GetErrorArray() const { return errorArray; } void UpdatedSClampValue(VectorR3* targets); diff --git a/examples/ThirdPartyLibs/BussIK/Node.h b/examples/ThirdPartyLibs/BussIK/Node.h index 5a49d278a..67ee4f91e 100644 --- a/examples/ThirdPartyLibs/BussIK/Node.h +++ b/examples/ThirdPartyLibs/BussIK/Node.h @@ -56,6 +56,10 @@ public: delta = actualDelta; #endif return theta; } + + double UpdateTheta( double& delta ) { + theta = delta; + return theta; } const VectorR3& GetS() const { return s; } const VectorR3& GetW() const { return w; }