ETHJointPtr ETHPhysicsEntityController::GetJoint(const std::size_t joindIdx)
{
	if (joindIdx < GetNumJoints())
		return m_joints[joindIdx];
	else
		return ETHJointPtr();
}
Пример #2
0
/*
========================
idJointBuffer::FreeBufferObject
========================
*/
void idJointBuffer::FreeBufferObject() {
	if ( IsMapped() ) {
		UnmapBuffer();
	}

	// if this is a sub-allocation inside a larger buffer, don't actually free anything.
	if ( OwnsBuffer() == false ) {
		ClearWithoutFreeing();
		return;
	}

	if ( apiObject == NULL ) {
		return;
	}

	if ( r_showBuffers.GetBool() ) {
		idLib::Printf( "joint buffer free %p, api %p (%i joints)\n", this, GetAPIObject(), GetNumJoints() );
	}

	GLuint buffer = reinterpret_cast< GLuint > ( apiObject );
	qglBindBufferARB( GL_UNIFORM_BUFFER, 0 );
	qglDeleteBuffersARB( 1, & buffer );

	ClearWithoutFreeing();
}
Пример #3
0
const Eigen::Block<const Eigen::MatrixXd> cRBDModel::GetJointSubspace(int j) const
{
	assert(j >= 0 && j < GetNumJoints());
	int offset = cKinTree::GetParamOffset(mJointMat, j);
	int dim = cKinTree::GetParamSize(mJointMat, j);
	int r = static_cast<int>(mJointSubspaceArr.rows());
	return mJointSubspaceArr.block(0, offset, r, dim);
}
Пример #4
0
tMatrix cRBDModel::GetChildParentMat(int j) const
{
	assert(j >= 0 && j < GetNumJoints());
	tMatrix trans;
	int r = static_cast<int>(trans.rows());
	int c = static_cast<int>(trans.cols());
	trans = mChildParentMatArr.block(j * r, 0, r, c);
	return trans;
}
Пример #5
0
void cRBDModel::InitJointSubspaceArr()
{
	int num_dofs = GetNumDof();
	int num_joints = GetNumJoints();
	mJointSubspaceArr = Eigen::MatrixXd(cSpAlg::gSpVecSize, num_dofs);
	for (int j = 0; j < num_joints; ++j)
	{
		int offset = cKinTree::GetParamOffset(mJointMat, j);
		int dim = cKinTree::GetParamSize(mJointMat, j);
		int r = static_cast<int>(mJointSubspaceArr.rows());
		mJointSubspaceArr.block(0, offset, r, dim) = cRBDUtil::BuildJointSubspace(mJointMat, mPose, j);
	}
}
Пример #6
0
bool
UsdSkelTopology::Validate(std::string* reason) const
{
    TRACE_FUNCTION();

    if(!TF_VERIFY(GetNumJoints() == 0 || _parentIndicesData))
        return false;

    for(size_t i = 0; i < GetNumJoints(); ++i) {
        int parent = _parentIndicesData[i];
        if(parent >= 0) {   
            if(ARCH_UNLIKELY(static_cast<size_t>(parent) >= i)) {
                if(static_cast<size_t>(parent) == i) {
                    if(reason) {
                        *reason = TfStringPrintf(
                            "Joint %zu has itself as its parent.", i);
                    }
                    return false;
                }

                if(reason) {
                    *reason = TfStringPrintf(
                        "Joint %zu has mis-ordered parent %d. Joints are "
                        "expected to be ordered with parent joints always "
                        "coming before children.", i, parent);

                    // XXX: Note that this ordering restriction is a schema
                    // requirement primarily because it simplifies hierarchy
                    // evaluation (see UsdSkelConcatJointTransforms)
                    // But a nice side effect for validation purposes is that
                    // it also ensures that topology is non-cyclic.
                }
                return false;
            }
        }
    }
    return true;
}
Пример #7
0
void cRBDModel::Init(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const tVector& gravity)
{
	assert(joint_mat.rows() == body_defs.rows());
	mGravity = gravity;
	mJointMat = joint_mat;
	mBodyDefs = body_defs;

	int num_dofs = GetNumDof();
	int num_joints = GetNumJoints();
	const int svs = cSpAlg::gSpVecSize;

	mPose = Eigen::VectorXd::Zero(num_dofs);
	mVel = Eigen::VectorXd::Zero(num_dofs);

	tMatrix trans_mat;
	InitJointSubspaceArr();
	mChildParentMatArr = Eigen::MatrixXd::Zero(num_joints * trans_mat.rows(), trans_mat.cols());
	mSpWorldJointTransArr = Eigen::MatrixXd::Zero(num_joints * cSpAlg::gSVTransRows, cSpAlg::gSVTransCols);
	mMassMat = Eigen::MatrixXd::Zero(num_dofs, num_dofs);
	mBiasForce = Eigen::VectorXd::Zero(num_dofs);
	mInertiaBuffer = Eigen::MatrixXd::Zero(num_joints * svs, svs);
}
Пример #8
0
cSpAlg::tSpTrans cRBDModel::GetSpWorldJointTrans(int j) const
{
	assert(j >= 0 && j < GetNumJoints());
	cSpAlg::tSpTrans trans = cSpAlg::GetTrans(mSpWorldJointTransArr, j);
	return trans;
}
Пример #9
0
	for (int j = 0; j < num_joints; ++j)
	{
		int offset = cKinTree::GetParamOffset(mJointMat, j);
		int dim = cKinTree::GetParamSize(mJointMat, j);
		int r = static_cast<int>(mJointSubspaceArr.rows());
		mJointSubspaceArr.block(0, offset, r, dim) = cRBDUtil::BuildJointSubspace(mJointMat, mPose, j);
	}
}

void cRBDModel::UpdateJointSubspaceArr()
{
#if defined(ENABLE_RBD_PROFILER)
	TIMER_PRINT_BEG(Update_Joint_Subspace)
#endif

	int num_joints = GetNumJoints();
	for (int j = 0; j < num_joints; ++j)
	{
		bool const_subspace = cRBDUtil::IsConstJointSubspace(mJointMat, j);
		if (!const_subspace)
		{
			int offset = cKinTree::GetParamOffset(mJointMat, j);
			int dim = cKinTree::GetParamSize(mJointMat, j);
			int r = static_cast<int>(mJointSubspaceArr.rows());
			mJointSubspaceArr.block(0, offset, r, dim) = cRBDUtil::BuildJointSubspace(mJointMat, mPose, j);
		}
	}

#if defined(ENABLE_RBD_PROFILER)
	TIMER_PRINT_END(Update_Joint_Subspace)
#endif
Пример #10
0
const RagdollJoint* Ragdoll::GetJointByIndex(unsigned int index) const
{
   const RagdollJoint* joint = (index >= 0 && index < GetNumJoints()) ? mJoints[index] : NULL;
   return joint;
}
Пример #11
0
int main(int argc, char** argv)
{
    IKREAL_TYPE eerot[9],eetrans[3];

#if IK_VERSION > 54
    // for IKFast 56,61
    unsigned int num_of_joints = GetNumJoints();
    unsigned int num_free_parameters = GetNumFreeParameters();
#else
    // for IKFast 54
    unsigned int num_of_joints = getNumJoints();
    unsigned int num_free_parameters = getNumFreeParameters();
#endif
 
    std::string cmd;
    if (argv[1]) cmd = argv[1];

    //printf("command: %s \n\n", cmd.c_str() );

    if (cmd.compare("ik") == 0) // ik mode
    {
        if( argc == 1+7+num_free_parameters+1 )  // ik, given translation vector and quaternion pose
        {
#if IK_VERSION > 54
            // for IKFast 56,61
            IkSolutionList<IKREAL_TYPE> solutions;
#else
            // for IKFast 54
            std::vector<IKSolution> vsolutions;
#endif
            std::vector<IKREAL_TYPE> vfree(num_free_parameters);

            eetrans[0] = atof(argv[2]);
            eetrans[1] = atof(argv[3]);
            eetrans[2] = atof(argv[4]);

            // Convert input effector pose, in w x y z quaternion notation, to rotation matrix. 
            // Must use doubles, else lose precision compared to directly inputting the rotation matrix.
            double qw = atof(argv[5]);
            double qx = atof(argv[6]);
            double qy = atof(argv[7]);
            double qz = atof(argv[8]);
            const double n = 1.0f/sqrt(qx*qx+qy*qy+qz*qz+qw*qw);
            qw *= n;
            qx *= n;
            qy *= n;
            qz *= n;
            eerot[0] = 1.0f - 2.0f*qy*qy - 2.0f*qz*qz;  eerot[1] = 2.0f*qx*qy - 2.0f*qz*qw;         eerot[2] = 2.0f*qx*qz + 2.0f*qy*qw;
            eerot[3] = 2.0f*qx*qy + 2.0f*qz*qw;         eerot[4] = 1.0f - 2.0f*qx*qx - 2.0f*qz*qz;  eerot[5] = 2.0f*qy*qz - 2.0f*qx*qw;
            eerot[6] = 2.0f*qx*qz - 2.0f*qy*qw;         eerot[7] = 2.0f*qy*qz + 2.0f*qx*qw;         eerot[8] = 1.0f - 2.0f*qx*qx - 2.0f*qy*qy;

            // For debugging, output the matrix
            
            for (unsigned char i=0; i<=8; i++)
            {   // detect -0.0 and replace with 0.0
                if ( ((int&)(eerot[i]) & 0xFFFFFFFF) == 0) eerot[i] = 0.0;
            }
            printf("     Rotation     %f   %f   %f  \n", eerot[0], eerot[1], eerot[2] );
            printf("                  %f   %f   %f  \n", eerot[3], eerot[4], eerot[5] );
            printf("                  %f   %f   %f  \n", eerot[6], eerot[7], eerot[8] );
            printf("\n");
            

            for(std::size_t i = 0; i < vfree.size(); ++i)
                vfree[i] = atof(argv[13+i]);

#if IK_VERSION > 54
            // for IKFast 56,61
            bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
#else
            // for IKFast 54
            bool bSuccess = ik(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, vsolutions);
#endif
            if( !bSuccess ) {
                fprintf(stderr,"Failed to get ik solution\n");
                //return -1;
            }

#if IK_VERSION > 54
            // for IKFast 56,61
            unsigned int num_of_solutions = (int)solutions.GetNumSolutions();
#else
            // for IKFast 54
            unsigned int num_of_solutions = (int)vsolutions.size();
#endif
            printf("Found %d ik solutions:\n", num_of_solutions ); 

            std::vector<IKREAL_TYPE> solvalues(num_of_joints);
            for(std::size_t i = 0; i < num_of_solutions; ++i) {
#if IK_VERSION > 54
                // for IKFast 56,61
                const IkSolutionBase<IKREAL_TYPE>& sol = solutions.GetSolution(i);
                int this_sol_free_params = (int)sol.GetFree().size(); 
#else
                // for IKFast 54
                int this_sol_free_params = (int)vsolutions[i].GetFree().size();
#endif
                printf("sol%d (free=%d): ", (int)i, this_sol_free_params );
                std::vector<IKREAL_TYPE> vsolfree(this_sol_free_params);

#if IK_VERSION > 54
                // for IKFast 56,61
                sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
#else
                // for IKFast 54
                vsolutions[i].GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
#endif
                for( std::size_t j = 0; j < solvalues.size(); ++j)
                    printf("%.15f, ", solvalues[j]);
                printf("\n");
            }

        } 
        else if( argc == 1+12+num_free_parameters+1 )  // ik, given rotation-translation matrix
        {
#if IK_VERSION > 54
            // for IKFast 56,61
            IkSolutionList<IKREAL_TYPE> solutions;
#else
            // for IKFast 54
            std::vector<IKSolution> vsolutions;
#endif
            std::vector<IKREAL_TYPE> vfree(num_free_parameters);

            eerot[0] = atof(argv[2]);  eerot[1] = atof(argv[3]);  eerot[2] = atof(argv[4]);  eetrans[0] = atof(argv[5]);
            eerot[3] = atof(argv[6]);  eerot[4] = atof(argv[7]);  eerot[5] = atof(argv[8]);  eetrans[1] = atof(argv[9]);
            eerot[6] = atof(argv[10]); eerot[7] = atof(argv[11]); eerot[8] = atof(argv[12]); eetrans[2] = atof(argv[13]);

            for(std::size_t i = 0; i < vfree.size(); ++i)
                vfree[i] = atof(argv[14+i]);

            printf("translation: \n");
            for (unsigned int i = 0; i < 3; i++) {
                printf("%lf ", eetrans[i]);
            }
            printf("\n");
            printf("rotation matix: \n");
            for (unsigned int i = 0; i < 9; i++) {
                printf("%lf%s", eerot[i], (i % 3 == 2)?"\n":" ");
            }
            printf("\n");

            printf("vfree:\n");
            for(std::size_t i = 0; i < vfree.size(); ++i)
                printf("%lf ", vfree[i]);
            printf("\n");

#if IK_VERSION > 54
            // for IKFast 56,61
            bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
#else
            // for IKFast 54
            bool bSuccess = ik(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, vsolutions);
#endif
            if( !bSuccess ) {
                fprintf(stderr,"Failed to get ik solution\n");
                return -1;
            }

#if IK_VERSION > 54
            // for IKFast 56,61
            unsigned int num_of_solutions = (int)solutions.GetNumSolutions();
#else
            // for IKFast 54
            unsigned int num_of_solutions = (int)vsolutions.size();
#endif
            printf("Found %d ik solutions:\n", num_of_solutions ); 

            std::vector<IKREAL_TYPE> solvalues(num_of_joints);
            for(std::size_t i = 0; i < num_of_solutions; ++i) {
#if IK_VERSION > 54
                // for IKFast 56,61
                const IkSolutionBase<IKREAL_TYPE>& sol = solutions.GetSolution(i);
                int this_sol_free_params = (int)sol.GetFree().size(); 
#else
                // for IKFast 54
                int this_sol_free_params = (int)vsolutions[i].GetFree().size();
#endif
                printf("sol%d (free=%d): ", (int)i, this_sol_free_params );
                std::vector<IKREAL_TYPE> vsolfree(this_sol_free_params);

#if IK_VERSION > 54
                // for IKFast 56,61
                sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
#else
                // for IKFast 54
                vsolutions[i].GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
#endif
                for( std::size_t j = 0; j < solvalues.size(); ++j)
                    printf("%.15f, ", solvalues[j]);
                printf("\n");
            }

        }
        else {
            printf("\n "
                   "Usage: \n\n " 
                   "         ./compute ik  t0 t1 t2  qw qi qj qk  free0 ...\n\n "
                   "         Returns the ik solutions given the transformation of the end effector specified by \n "
                   "         a 3x1 translation (tX), and a 1x4 quaternion (w + i + j + k). \n "
                   "         There are %d free parameters that have to be specified.\n\n", num_free_parameters );
            printf(" \n "
                   "         ./compute ik  r00 r01 r02 t0  r10 r11 r12 t1  r20 r21 r22 t2  free0 ...\n\n "
                   "         Returns the ik solutions given the transformation of the end effector specified by \n "
                   "         a 3x3 rotation R (rXX), and a 3x1 translation (tX). \n "
                   "         There are %d free parameters that have to be specified.\n\n", num_free_parameters );
            return 1;

        }
    } // endif ik mode

    else if (cmd.compare("fk") == 0) // fk mode
    {
        if( argc != num_of_joints+2 ) {
            printf("\n "
                   "Usage: \n\n "
                   "         ./compute fk j0 j1 ... j%d \n\n"
                   "         Returns the forward kinematic solution given the joint angles (in radians). \n\n", num_of_joints-1 );
            return 1;
        }
        printf("\n\n");

        // Put input joint values into array
        IKREAL_TYPE joints[num_of_joints];
        for (unsigned int i=0; i<num_of_joints; i++)
        {
            joints[i] = atof(argv[i+2]);
        }

#if IK_VERSION > 54
        // for IKFast 56,61
        ComputeFk(joints, eetrans, eerot); // void return
#else
        // for IKFast 54
        fk(joints, eetrans, eerot); // void return
#endif
        printf("Found fk solution for end frame: \n\n");

        printf("  Translation:  x: %f  y: %f  z: %f  \n", eetrans[0], eetrans[1], eetrans[2] );
        printf("\n");
        printf("     Rotation     %f   %f   %f  \n", eerot[0], eerot[1], eerot[2] );
        printf("       Matrix:    %f   %f   %f  \n", eerot[3], eerot[4], eerot[5] );
        printf("                  %f   %f   %f  \n", eerot[6], eerot[7], eerot[8] );
        printf("\n");

        // Display equivalent Euler angles
        float yaw;
        float pitch;
        float roll;
        if ( eerot[5] > 0.998 || eerot[5] < -0.998 ) { // singularity
            yaw = IKatan2( -eerot[6], eerot[0] );
            pitch = 0;
        } else {
            yaw = IKatan2( eerot[2], eerot[8] );
            pitch = IKatan2( eerot[3], eerot[4] );
        }
        roll = IKasin( eerot[5] );
        printf(" Euler angles: \n");
        printf("       Yaw:   %f    ", yaw ); printf("(1st: rotation around vertical blue Z-axis in ROS Rviz) \n");
        printf("       Pitch: %f  \n", pitch ); 
        printf("       Roll:  %f  \n", roll ); 
        printf("\n");

        // Convert rotation matrix to quaternion (Daisuke Miyazaki)
        float q0 = ( eerot[0] + eerot[4] + eerot[8] + 1.0f) / 4.0f;
        float q1 = ( eerot[0] - eerot[4] - eerot[8] + 1.0f) / 4.0f;
        float q2 = (-eerot[0] + eerot[4] - eerot[8] + 1.0f) / 4.0f;
        float q3 = (-eerot[0] - eerot[4] + eerot[8] + 1.0f) / 4.0f;
        if(q0 < 0.0f) q0 = 0.0f;
        if(q1 < 0.0f) q1 = 0.0f;
        if(q2 < 0.0f) q2 = 0.0f;
        if(q3 < 0.0f) q3 = 0.0f;
        q0 = sqrt(q0);
        q1 = sqrt(q1);
        q2 = sqrt(q2);
        q3 = sqrt(q3);
        if(q0 >= q1 && q0 >= q2 && q0 >= q3) {
            q0 *= +1.0f;
            q1 *= SIGN(eerot[7] - eerot[5]);
            q2 *= SIGN(eerot[2] - eerot[6]);
            q3 *= SIGN(eerot[3] - eerot[1]);
        } else if(q1 >= q0 && q1 >= q2 && q1 >= q3) {
            q0 *= SIGN(eerot[7] - eerot[5]);
            q1 *= +1.0f;
            q2 *= SIGN(eerot[3] + eerot[1]);
            q3 *= SIGN(eerot[2] + eerot[6]);
        } else if(q2 >= q0 && q2 >= q1 && q2 >= q3) {
            q0 *= SIGN(eerot[2] - eerot[6]);
            q1 *= SIGN(eerot[3] + eerot[1]);
            q2 *= +1.0f;
            q3 *= SIGN(eerot[7] + eerot[5]);
        } else if(q3 >= q0 && q3 >= q1 && q3 >= q2) {
            q0 *= SIGN(eerot[3] - eerot[1]);
            q1 *= SIGN(eerot[6] + eerot[2]);
            q2 *= SIGN(eerot[7] + eerot[5]);
            q3 *= +1.0f;
        } else {
            printf("Error while converting to quaternion! \n");
        }
        float r = NORM(q0, q1, q2, q3);
        q0 /= r;
        q1 /= r;
        q2 /= r;
        q3 /= r;
        printf("  Quaternion:  %f   %f   %f   %f   \n", q0, q1, q2, q3 );
        printf("               ");
        // print quaternion with convention and +/- signs such that it can be copy-pasted into WolframAlpha.com
        printf("%f ", q0);
        if (q1 > 0) printf("+ %fi ", q1); else if (q1 < 0) printf("- %fi ", -q1); else printf("+ 0.00000i ");
        if (q2 > 0) printf("+ %fj ", q2); else if (q2 < 0) printf("- %fj ", -q2); else printf("+ 0.00000j ");
        if (q3 > 0) printf("+ %fk ", q3); else if (q3 < 0) printf("- %fk ", -q3); else printf("+ 0.00000k ");
        printf("  (alternate convention) \n");
        printf("\n\n");


    }
    else if (cmd.compare("iktiming") == 0) // generate random ik and check time performance
    {
        if( argc != 2 ) {
            printf("\n "
                   "Usage: \n\n "
                   "         ./compute iktiming \n\n"
                   "         For fixed number of iterations, generates random joint angles, then  \n"
                   "         calculates fk, calculates ik, measures average time taken. \n\n", num_of_joints-1 );
            return 1;
        }
        printf("\n\n");

#if IK_VERSION > 54
        // for IKFast 56,61
        IkSolutionList<IKREAL_TYPE> solutions;
#else
        // for IKFast 54
        std::vector<IKSolution> vsolutions;
#endif
        std::vector<IKREAL_TYPE> vfree(num_free_parameters);

        //for(std::size_t i = 0; i < vfree.size(); ++i)
        //    vfree[i] = atof(argv[13+i]);

        srand( (unsigned)time(0) ); // seed random number generator
        float min = -3.14;
        float max = 3.14;
        float rnd;

        IKREAL_TYPE joints[num_of_joints];

        timespec start_time, end_time;
        unsigned int elapsed_time = 0;
        unsigned int sum_time = 0;
        unsigned int fail_count = 0;

#if IK_VERSION > 54
        // for IKFast 56,61
        unsigned int num_of_tests = 10; 
#else
        // for IKFast 54
        unsigned int num_of_tests = 100000; 
#endif

        for (unsigned int i=0; i < num_of_tests; i++)
        {
            // Measure avg time for whole process
            //clock_gettime(CLOCK_REALTIME, &start_time); 

            // Put random joint values into array
            for (unsigned int i=0; i<num_of_joints; i++)
            {
                float rnd = (float)rand() / (float)RAND_MAX;
                joints[i] = min + rnd * (max - min);
            }
            /*
            printf("Joint angles:  ");
            for (unsigned int i=0; i<num_of_joints; i++)
            {
                printf("%f  ", joints[i] );
            }
            printf("\n");
            */

#if IK_VERSION > 54
            // for IKFast 56,61
            ComputeFk(joints, eetrans, eerot); // void return
#else
            // for IKFast 54
            fk(joints, eetrans, eerot); // void return
#endif

            // Measure avg time for IK
            clock_gettime(CLOCK_REALTIME, &start_time);
#if IK_VERSION > 54
            // for IKFast 56,61
            vfree[0] = 5;
           if (!ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions)) {
            fail_count++;
           } else {
            printf("joint angles: \n");
            for (unsigned int i=0; i<num_of_joints; i++)
            {
               printf("%lf ", joints[i]);
            }
            printf("\n");

            printf("translation: \n");
            for (unsigned int i = 0; i < 3; i++) {
                printf("%lf ", eetrans[i]);
            }
            printf("\n");
            printf("rotation matix: \n");
            for (unsigned int i = 0; i < 9; i++) {
                printf("%lf%s", eerot[i], (i % 3 == 2)?"\n":" ");
            }
            printf("\n");

            /*unsigned int num_of_solutions = (int)solutions.GetNumSolutions();
            printf("Found %d ik solutions:\n", num_of_solutions ); 

            std::vector<IKREAL_TYPE> solvalues(num_of_joints);
            for(std::size_t i = 0; i < num_of_solutions; ++i) {
                const IkSolutionBase<IKREAL_TYPE>& sol = solutions.GetSolution(i);
                int this_sol_free_params = (int)sol.GetFree().size(); 
                printf("sol%d (free=%d): ", (int)i, this_sol_free_params );
                std::vector<IKREAL_TYPE> vsolfree(this_sol_free_params);

                sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
                for( std::size_t j = 0; j < solvalues.size(); ++j)
                    printf("%.15f, ", solvalues[j]);
                printf("\n");
            }*/
           }
#else
            // for IKFast 54
            ik(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, vsolutions);
#endif

            /*
#if IK_VERSION > 54
            // for IKFast 56,61
            unsigned int num_of_solutions = (int)solutions.GetNumSolutions();
#else
            // for IKFast 54
            unsigned int num_of_solutions = (int)vsolutions.size();
#endif
            printf("Found %d ik solutions:\n", num_of_solutions ); 
            */

            clock_gettime(CLOCK_REALTIME, &end_time); 
            elapsed_time = (unsigned int)(end_time.tv_nsec - start_time.tv_nsec);
            sum_time += elapsed_time;
        } // endfor

        unsigned int avg_time = (unsigned int)sum_time / (unsigned int)num_of_tests; 
        printf("avg time: %f ms   over %d tests \n", (float)avg_time/1000.0, num_of_tests );
        printf("Failed of %d tests\n", fail_count);

        return 1;
  
    }
    else if (cmd.compare("iktiming2") == 0) // for fixed joint values, check time performance of ik
    {
        if( argc != 2 ) {
            printf("\n "
                   "Usage: \n\n "
                   "         ./compute iktiming2 \n\n"
                   "         For fixed number of iterations, with one set of joint variables, this  \n"
                   "         finds the ik solutions and measures the average time taken. \n\n", num_of_joints-1 );
            return 1;
        }
        printf("\n\n");

#if IK_VERSION > 54
        // for IKFast 56,61
        IkSolutionList<IKREAL_TYPE> solutions;
#else
        // for IKFast 54
        std::vector<IKSolution> vsolutions;
#endif
        std::vector<IKREAL_TYPE> vfree(num_free_parameters);

        //for(std::size_t i = 0; i < vfree.size(); ++i)
        //    vfree[i] = atof(argv[13+i]);

        IKREAL_TYPE joints[num_of_joints];

        timespec start_time, end_time;
        unsigned int elapsed_time = 0;
        unsigned int sum_time = 0;

#if IK_VERSION > 54
        // for IKFast 56,61
        unsigned int num_of_tests = 1000000; 
#else
        // for IKFast 54
        unsigned int num_of_tests = 100000; 
#endif

        // fixed rotation-translation matrix corresponding to an unusual robot pose
        eerot[0] = 0.002569;  eerot[1] = -0.658044;  eerot[2] = -0.752975;  eetrans[0] = 0.121937;
        eerot[3] = 0.001347;  eerot[4] = -0.752975;  eerot[5] = 0.658048;  eetrans[1] = -0.276022;
        eerot[6] = -0.999996; eerot[7] = -0.002705; eerot[8] = -0.001047; eetrans[2] = 0.005685;

        for (unsigned int i=0; i < num_of_tests; i++)
        {
            clock_gettime(CLOCK_REALTIME, &start_time);

#if IK_VERSION > 54
            // for IKFast 56,61
            ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
#else
            // for IKFast 54
            ik(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, vsolutions);
#endif

            /*
#if IK_VERSION > 54
            // for IKFast 56,61
            unsigned int num_of_solutions = (int)solutions.GetNumSolutions();
#else
            // for IKFast 54
            unsigned int num_of_solutions = (int)vsolutions.size();
#endif
            printf("Found %d ik solutions:\n", num_of_solutions ); 
            */

            clock_gettime(CLOCK_REALTIME, &end_time); 
            elapsed_time = (unsigned int)(end_time.tv_nsec - start_time.tv_nsec);
            sum_time += elapsed_time;
        } // endfor

        unsigned int avg_time = (unsigned int)sum_time / (unsigned int)num_of_tests; 
        printf("avg time: %f ms   over %d tests \n", (float)avg_time/1000.0, num_of_tests );

        return 1;

    } else {
        printf("\n"
               "Usage: \n\n");
        printf("         ./compute fk j0 j1 ... j%d \n\n"
               "         Returns the forward kinematic solution given the joint angles (in radians). \n\n", num_of_joints-1 );
        printf("\n"
               "         ./compute ik  t0 t1 t2  qw qi qj qk  free0 ... \n\n"
               "         Returns the ik solutions given the transformation of the end effector specified by \n"
               "         a 3x1 translation (tX), and a 1x4 quaternion (w + i + j + k). \n"
               "         There are %d free parameters that have to be specified. \n\n", num_free_parameters );
        printf(" \n"
               "         ./compute ik  r00 r01 r02 t0  r10 r11 r12 t1  r20 r21 r22 t2  free0 ...\n\n"
               "         Returns the ik solutions given the transformation of the end effector specified by \n"
               "         a 3x3 rotation R (rXX), and a 3x1 translation (tX). \n"
               "         There are %d free parameters that have to be specified. \n\n", num_free_parameters );
        printf("\n"
               "         ./compute iktiming \n\n"
               "         For fixed number of iterations, generates random joint angles, then \n"
               "         calculates fk, calculates ik, measures average time taken. \n\n", num_of_joints-1 );
        printf("\n"
               "         ./compute iktiming2 \n\n"
               "         For fixed number of iterations, with one set of joint variables, this \n"
               "         finds the ik solutions and measures the average time taken. \n\n", num_of_joints-1 );

        return 1;
    }

    return 0;
}
Пример #12
0
/*
========================
idJointBuffer::AllocBufferObject
========================
*/
bool idJointBuffer::AllocBufferObject( const float * joints, int numAllocJoints ) {
	assert( apiObject == NULL );
	assert_16_byte_aligned( joints );

	if ( numAllocJoints <= 0 ) {
		idLib::Error( "idJointBuffer::AllocBufferObject: joints = %i", numAllocJoints );
	}

	numJoints = numAllocJoints;

	bool allocationFailed = false;

	const int numBytes = GetAllocedSize();

	GLuint buffer = 0;
	qglGenBuffersARB( 1, &buffer );
	qglBindBufferARB( GL_UNIFORM_BUFFER, buffer );
	qglBufferDataARB( GL_UNIFORM_BUFFER, numBytes, NULL, GL_STREAM_DRAW_ARB );
	qglBindBufferARB( GL_UNIFORM_BUFFER, 0);
	apiObject = reinterpret_cast< void * >( buffer );

	if ( r_showBuffers.GetBool() ) {
		idLib::Printf( "joint buffer alloc %p, api %p (%i joints)\n", this, GetAPIObject(), GetNumJoints() );
	}

	// copy the data
	if ( joints != NULL ) {
		Update( joints, numAllocJoints );
	}

	return !allocationFailed;
}
Пример #13
0
ms3d_joint_t *msModel::GetJoint(int index)
{
    if(index > GetNumJoints())
        return NULL;
	return &m_joints[index];
}