Beispiel #1
0
// Returns a vector v orthonormal to unit vector u
void GetOrtho( const VectorR3& u,  VectorR3& v )
{
	if ( u.x > 0.5 || u.x<-0.5 || u.y > 0.5 || u.y<-0.5 ) {
		v.Set ( u.y, -u.x, 0.0 );
	}
	else {
		v.Set ( 0.0, u.z, -u.y);
	}
	v.Normalize();
	return;
}
Beispiel #2
0
// Returns a righthanded orthonormal basis to complement vector u
void GetOrtho( const VectorR3& u,  VectorR3& v, VectorR3& w)
{
	if ( u.x > 0.5 || u.x<-0.5 || u.y > 0.5 || u.y<-0.5 ) {
		v.Set ( u.y, -u.x, 0.0 );
	}
	else {
		v.Set ( 0.0, u.z, -u.y);
	}
	v.Normalize();
	w = u;
	w *= v;
	w.Normalize();
	// w.NormalizeFast();
	return;
}
bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
                                   const double endEffectorTargetOrientation[4],
                                   const double endEffectorWorldPosition[3],
                                   const double endEffectorWorldOrientation[4],
               const double* q_current, int numQ,int endEffectorIndex,
               double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, const double dampIk[6])
{
	bool useAngularPart = (ikMethod==IK2_VEL_DLS_WITH_ORIENTATION || ikMethod==IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE) ? true : false;

    Jacobian ikJacobian(useAngularPart,numQ);

    ikJacobian.Reset();

    bool UseJacobianTargets1 = false;
    
    if ( UseJacobianTargets1 ) {
        ikJacobian.SetJtargetActive();
    }
    else {
        ikJacobian.SetJendActive();
    }
    VectorR3 targets;
    targets.Set(endEffectorTargetPosition[0],endEffectorTargetPosition[1],endEffectorTargetPosition[2]);
    ikJacobian.ComputeJacobian(&targets);						// Set up Jacobian and deltaS vectors
    
    // Set one end effector world position from Bullet
    VectorRn deltaS(3);
    for (int i = 0; i < 3; ++i)
    {
        deltaS.Set(i,dampIk[i]*(endEffectorTargetPosition[i]-endEffectorWorldPosition[i]));
    }
    
    // Set one end effector world orientation from Bullet
    VectorRn deltaR(3);
    if (useAngularPart)
    {
        btQuaternion startQ(endEffectorWorldOrientation[0],endEffectorWorldOrientation[1],endEffectorWorldOrientation[2],endEffectorWorldOrientation[3]);
        btQuaternion endQ(endEffectorTargetOrientation[0],endEffectorTargetOrientation[1],endEffectorTargetOrientation[2],endEffectorTargetOrientation[3]);
        btQuaternion deltaQ = endQ*startQ.inverse();
        float angle = deltaQ.getAngle();
        btVector3 axis = deltaQ.getAxis();
        if (angle > PI) {
            angle -= 2.0*PI;
        }
        else if (angle < -PI) {
            angle += 2.0*PI;
        }
        float angleDot = angle;
        btVector3 angularVel = angleDot*axis.normalize();
        for (int i = 0; i < 3; ++i)
        {
            deltaR.Set(i,dampIk[i+3]*angularVel[i]);
        }
    }
    
    {
        
		if (useAngularPart)
		{
			VectorRn deltaC(6);
			MatrixRmn completeJacobian(6,numQ);
			for (int i = 0; i < 3; ++i)
			{
				deltaC.Set(i,deltaS[i]);
				deltaC.Set(i+3,deltaR[i]);
				for (int j = 0; j < numQ; ++j)
				{
					completeJacobian.Set(i,j,linear_jacobian[i*numQ+j]);
					completeJacobian.Set(i+3,j,angular_jacobian[i*numQ+j]);
				}
			}
			ikJacobian.SetDeltaS(deltaC);
			ikJacobian.SetJendTrans(completeJacobian);
		} else
		{
			VectorRn deltaC(3);
			MatrixRmn completeJacobian(3,numQ);
			for (int i = 0; i < 3; ++i)
			{
				deltaC.Set(i,deltaS[i]);
				for (int j = 0; j < numQ; ++j)
				{
					completeJacobian.Set(i,j,linear_jacobian[i*numQ+j]);
				}
			}
			ikJacobian.SetDeltaS(deltaC);
			ikJacobian.SetJendTrans(completeJacobian);
		}
    }
    
    // Calculate the change in theta values
    switch (ikMethod) {
        case IK2_JACOB_TRANS:
            ikJacobian.CalcDeltaThetasTranspose();		// Jacobian transpose method
            break;
		case IK2_DLS:
        case IK2_VEL_DLS:
		case IK2_VEL_DLS_WITH_ORIENTATION:
            //ikJacobian.CalcDeltaThetasDLS();			// Damped least squares method
            assert(m_data->m_dampingCoeff.GetLength()==numQ);
            ikJacobian.CalcDeltaThetasDLS2(m_data->m_dampingCoeff);
            break;
        case IK2_VEL_DLS_WITH_NULLSPACE:
        case IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE:
            assert(m_data->m_nullSpaceVelocity.GetLength()==numQ);
            ikJacobian.CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity);
            break;
        case IK2_DLS_SVD:
            ikJacobian.CalcDeltaThetasDLSwithSVD();
            break;
        case IK2_PURE_PSEUDO:
            ikJacobian.CalcDeltaThetasPseudoinverse();	// Pure pseudoinverse method
            break;
        case IK2_SDLS:
            ikJacobian.CalcDeltaThetasSDLS();			// Selectively damped least squares method
            break;
        default:
            ikJacobian.ZeroDeltaThetas();
            break;
    }
    
    // Use for velocity IK, update theta dot
    //ikJacobian.UpdateThetaDot();
    
    // Use for position IK, incrementally update theta
    //ikJacobian.UpdateThetas();
    
    // Apply the change in the theta values
    //ikJacobian.UpdatedSClampValue(&targets);
    
    for (int i=0;i<numQ;i++)
    {
        // Use for velocity IK
        q_new[i] = ikJacobian.dTheta[i] + q_current[i];
        
        // Use for position IK
        //q_new[i] = m_data->m_ikNodes[i]->GetTheta();
    }
    return true;
}