diff --git a/examples/SharedMemory/IKTrajectoryHelper.cpp b/examples/SharedMemory/IKTrajectoryHelper.cpp index 13bd8a2ad..a9af38164 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.cpp +++ b/examples/SharedMemory/IKTrajectoryHelper.cpp @@ -18,7 +18,6 @@ struct IKTrajectoryHelperInternalData VectorRn m_nullSpaceVelocity; b3AlignedObjectArray m_ikNodes; - Jacobian* m_ikJacobian; IKTrajectoryHelperInternalData() { @@ -48,23 +47,21 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3], { bool useAngularPart = (ikMethod==IK2_VEL_DLS_WITH_ORIENTATION || ikMethod==IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE) ? true : false; - m_data->m_ikJacobian = new Jacobian(useAngularPart,numQ); - -// Reset(m_ikTree,m_ikJacobian); + Jacobian ikJacobian(useAngularPart,numQ); - m_data->m_ikJacobian->Reset(); + ikJacobian.Reset(); bool UseJacobianTargets1 = false; if ( UseJacobianTargets1 ) { - m_data->m_ikJacobian->SetJtargetActive(); + ikJacobian.SetJtargetActive(); } else { - m_data->m_ikJacobian->SetJendActive(); + ikJacobian.SetJendActive(); } VectorR3 targets; targets.Set(endEffectorTargetPosition[0],endEffectorTargetPosition[1],endEffectorTargetPosition[2]); - m_data->m_ikJacobian->ComputeJacobian(&targets); // Set up Jacobian and deltaS vectors + ikJacobian.ComputeJacobian(&targets); // Set up Jacobian and deltaS vectors // Set one end effector world position from Bullet VectorRn deltaS(3); @@ -112,8 +109,8 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3], completeJacobian.Set(i+3,j,angular_jacobian[i*numQ+j]); } } - m_data->m_ikJacobian->SetDeltaS(deltaC); - m_data->m_ikJacobian->SetJendTrans(completeJacobian); + ikJacobian.SetDeltaS(deltaC); + ikJacobian.SetJendTrans(completeJacobian); } else { VectorRn deltaC(3); @@ -126,53 +123,53 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3], completeJacobian.Set(i,j,linear_jacobian[i*numQ+j]); } } - m_data->m_ikJacobian->SetDeltaS(deltaC); - m_data->m_ikJacobian->SetJendTrans(completeJacobian); + ikJacobian.SetDeltaS(deltaC); + ikJacobian.SetJendTrans(completeJacobian); } } // Calculate the change in theta values switch (ikMethod) { case IK2_JACOB_TRANS: - m_data->m_ikJacobian->CalcDeltaThetasTranspose(); // Jacobian transpose method + ikJacobian.CalcDeltaThetasTranspose(); // Jacobian transpose method break; case IK2_DLS: case IK2_VEL_DLS: case IK2_VEL_DLS_WITH_ORIENTATION: - m_data->m_ikJacobian->CalcDeltaThetasDLS(); // Damped least squares method + ikJacobian.CalcDeltaThetasDLS(); // Damped least squares method break; case IK2_VEL_DLS_WITH_NULLSPACE: case IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE: assert(m_data->m_nullSpaceVelocity.GetLength()==numQ); - m_data->m_ikJacobian->CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity); + ikJacobian.CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity); break; case IK2_DLS_SVD: - m_data->m_ikJacobian->CalcDeltaThetasDLSwithSVD(); + ikJacobian.CalcDeltaThetasDLSwithSVD(); break; case IK2_PURE_PSEUDO: - m_data->m_ikJacobian->CalcDeltaThetasPseudoinverse(); // Pure pseudoinverse method + ikJacobian.CalcDeltaThetasPseudoinverse(); // Pure pseudoinverse method break; case IK2_SDLS: - m_data->m_ikJacobian->CalcDeltaThetasSDLS(); // Selectively damped least squares method + ikJacobian.CalcDeltaThetasSDLS(); // Selectively damped least squares method break; default: - m_data->m_ikJacobian->ZeroDeltaThetas(); + ikJacobian.ZeroDeltaThetas(); break; } // Use for velocity IK, update theta dot - //m_data->m_ikJacobian->UpdateThetaDot(); + //ikJacobian.UpdateThetaDot(); // Use for position IK, incrementally update theta - //m_data->m_ikJacobian->UpdateThetas(); + //ikJacobian.UpdateThetas(); // Apply the change in the theta values - //m_data->m_ikJacobian->UpdatedSClampValue(&targets); + //ikJacobian.UpdatedSClampValue(&targets); for (int i=0;im_ikJacobian->dTheta[i] + q_current[i]; + q_new[i] = ikJacobian.dTheta[i] + q_current[i]; // Use for position IK //q_new[i] = m_data->m_ikNodes[i]->GetTheta(); @@ -203,4 +200,4 @@ bool IKTrajectoryHelper::computeNullspaceVel(int numQ, const double* q_current, } } return true; -} \ No newline at end of file +}