• 设为首页
  • 点击收藏
  • 手机版
    手机扫一扫访问
    迪恩网络手机版
  • 关注官方公众号
    微信扫一扫关注
    迪恩网络公众号

C++ URDFImporterInterface类代码示例

原作者: [db:作者] 来自: [db:来源] 收藏 邀请

本文整理汇总了C++中URDFImporterInterface的典型用法代码示例。如果您正苦于以下问题:C++ URDFImporterInterface类的具体用法?C++ URDFImporterInterface怎么用?C++ URDFImporterInterface使用的例子?那么恭喜您, 这里精选的类代码示例或许可以为您提供帮助。



在下文中一共展示了URDFImporterInterface类的13个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于我们的系统推荐出更棒的C++代码示例。

示例1: ComputeTotalNumberOfJoints

void ComputeTotalNumberOfJoints(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int linkIndex)
{
    btAlignedObjectArray<int> childIndices;
    u2b.getLinkChildIndices(linkIndex,childIndices);
    printf("link %s has %d children\n", u2b.getLinkName(linkIndex).c_str(),childIndices.size());
    for (int i=0;i<childIndices.size();i++)
    {
        printf("child %d has childIndex%d=%s\n",i,childIndices[i],u2b.getLinkName(childIndices[i]).c_str());
    }
    cache.m_totalNumJoints1 += childIndices.size();
    for (int i=0;i<childIndices.size();i++)
    {
        int childIndex =childIndices[i];
        ComputeTotalNumberOfJoints(u2b,cache,childIndex);
    }
}
开发者ID:Mwenjoy,项目名称:bullet3,代码行数:16,代码来源:URDF2Bullet.cpp


示例2: ConvertURDF2Bullet

void ConvertURDF2Bullet(
    const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation,
    const btTransform& rootTransformInWorldSpace,
    btMultiBodyDynamicsWorld* world1,
    bool createMultiBody, const char* pathPrefix, int flags)
{
    URDF2BulletCachedData cache;

    InitURDF2BulletCache(u2b,cache);
    int urdfLinkIndex = u2b.getRootLinkIndex();
    ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex,rootTransformInWorldSpace,world1,createMultiBody,pathPrefix,flags);

	if (world1 && cache.m_bulletMultiBody)
	{
		btMultiBody* mb = cache.m_bulletMultiBody;
		mb->setHasSelfCollision(false);
		mb->finalizeMultiDof();

		btTransform localInertialFrameRoot = cache.m_urdfLinkLocalInertialFrames[urdfLinkIndex];

		mb->setBaseWorldTransform(rootTransformInWorldSpace*localInertialFrameRoot);
		btAlignedObjectArray<btQuaternion> scratch_q;
		btAlignedObjectArray<btVector3> scratch_m;
		mb->forwardKinematics(scratch_q,scratch_m);
		mb->updateCollisionObjectWorldTransforms(scratch_q,scratch_m);
		
		world1->addMultiBody(mb);
	}
}
开发者ID:RobotLocomotion,项目名称:bullet3,代码行数:29,代码来源:URDF2Bullet.cpp


示例3: printTree

void printTree(const URDFImporterInterface& u2b, int linkIndex, int indentationLevel)
{
    btAlignedObjectArray<int> childIndices;
    u2b.getLinkChildIndices(linkIndex,childIndices);
    
    int numChildren = childIndices.size();
    
    indentationLevel+=2;
    int count = 0;
    for (int i=0;i<numChildren;i++)
    {
        int childLinkIndex = childIndices[i];
        std::string name = u2b.getLinkName(childLinkIndex);
        for(int j=0;j<indentationLevel;j++) printf("  "); //indent
        printf("child(%d).name=%s with childIndex=%d\n",(count++)+1, name.c_str(),childLinkIndex);
        // first grandchild
        printTree(u2b,childLinkIndex,indentationLevel);
    }
}
开发者ID:Mwenjoy,项目名称:bullet3,代码行数:19,代码来源:URDF2Bullet.cpp


示例4: ComputeParentIndices

void ComputeParentIndices(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex, int urdfParentIndex)
{
    cache.m_urdfLinkParentIndices[urdfLinkIndex]=urdfParentIndex;
    cache.m_urdfLinkIndices2BulletLinkIndices[urdfLinkIndex]=cache.m_currentMultiBodyLinkIndex++;

    btAlignedObjectArray<int> childIndices;
    u2b.getLinkChildIndices(urdfLinkIndex,childIndices);
    for (int i=0;i<childIndices.size();i++)
    {
        ComputeParentIndices(u2b,cache,childIndices[i],urdfLinkIndex);
    }
}
开发者ID:RobotLocomotion,项目名称:bullet3,代码行数:12,代码来源:URDF2Bullet.cpp


示例5: ConvertURDF2Bullet

void ConvertURDF2Bullet(
    const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation,
    const btTransform& rootTransformInWorldSpace,
    btMultiBodyDynamicsWorld* world1,
    bool createMultiBody, const char* pathPrefix, int flags, UrdfVisualShapeCache* cachedLinkGraphicsShapes)
{

	URDF2BulletCachedData cache;
    InitURDF2BulletCache(u2b,cache);
    int urdfLinkIndex = u2b.getRootLinkIndex();
	B3_PROFILE("ConvertURDF2Bullet");
	
	UrdfVisualShapeCache cachedLinkGraphicsShapesOut;

	ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex,rootTransformInWorldSpace,world1,createMultiBody,pathPrefix,flags, cachedLinkGraphicsShapes, &cachedLinkGraphicsShapesOut);
	if (cachedLinkGraphicsShapes && cachedLinkGraphicsShapesOut.m_cachedUrdfLinkVisualShapeIndices.size() > cachedLinkGraphicsShapes->m_cachedUrdfLinkVisualShapeIndices.size())
	{
		*cachedLinkGraphicsShapes = cachedLinkGraphicsShapesOut;
	}

	if (world1 && cache.m_bulletMultiBody)
	{
		B3_PROFILE("Post process");
		btMultiBody* mb = cache.m_bulletMultiBody;

		mb->setHasSelfCollision((flags&CUF_USE_SELF_COLLISION)!=0);
		
		mb->finalizeMultiDof();

		btTransform localInertialFrameRoot = cache.m_urdfLinkLocalInertialFrames[urdfLinkIndex];

		if (flags & CUF_USE_MJCF)
		{
		} else
		{
			mb->setBaseWorldTransform(rootTransformInWorldSpace*localInertialFrameRoot);
		}
		btAlignedObjectArray<btQuaternion> scratch_q;
		btAlignedObjectArray<btVector3> scratch_m;
		mb->forwardKinematics(scratch_q,scratch_m);
		mb->updateCollisionObjectWorldTransforms(scratch_q,scratch_m);
		
		world1->addMultiBody(mb);
	}
}
开发者ID:benelot,项目名称:bullet3,代码行数:45,代码来源:URDF2Bullet.cpp


示例6: ConvertURDF2Bullet

void ConvertURDF2Bullet(const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, const btTransform& rootTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix)
{
    URDF2BulletCachedData cache;

    InitURDF2BulletCache(u2b,cache);
    int urdfLinkIndex = u2b.getRootLinkIndex();
    ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex,rootTransformInWorldSpace,world1,createMultiBody,pathPrefix);
    
	if (world1 && cache.m_bulletMultiBody)
	{
		btMultiBody* mb = cache.m_bulletMultiBody;
		mb->setHasSelfCollision(false);
		mb->finalizeMultiDof();

		mb->setBaseWorldTransform(rootTransformInWorldSpace);

		world1->addMultiBody(mb);
	}
}
开发者ID:Mwenjoy,项目名称:bullet3,代码行数:19,代码来源:URDF2Bullet.cpp


示例7: GetAllIndices

void GetAllIndices(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex, int parentIndex, btAlignedObjectArray<childParentIndex>& allIndices)
{
	childParentIndex cp;
	cp.m_index = urdfLinkIndex;
	int mbIndex = cache.getMbIndexFromUrdfIndex(urdfLinkIndex);
	cp.m_mbIndex = mbIndex;
	cp.m_parentIndex = parentIndex;
	int parentMbIndex = parentIndex>=0? cache.getMbIndexFromUrdfIndex(parentIndex) : -1;
	cp.m_parentMBIndex = parentMbIndex;

	allIndices.push_back(cp);
	btAlignedObjectArray<int> urdfChildIndices;
	u2b.getLinkChildIndices(urdfLinkIndex, urdfChildIndices);
	int numChildren = urdfChildIndices.size();
	for (int i = 0; i < numChildren; i++)
	{
		int urdfChildLinkIndex = urdfChildIndices[i];
		GetAllIndices(u2b, cache, urdfChildLinkIndex, urdfLinkIndex, allIndices);
	}
}
开发者ID:YunfeiBai,项目名称:bullet3,代码行数:20,代码来源:URDF2Bullet.cpp


示例8: InitURDF2BulletCache

void InitURDF2BulletCache(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache)
{
    //compute the number of links, and compute parent indices array (and possibly other cached data?)
    cache.m_totalNumJoints1 = 0;

    int rootLinkIndex = u2b.getRootLinkIndex();
    if (rootLinkIndex>=0)
    {
        ComputeTotalNumberOfJoints(u2b,cache,rootLinkIndex);
        int numTotalLinksIncludingBase = 1+cache.m_totalNumJoints1;

        cache.m_urdfLinkParentIndices.resize(numTotalLinksIncludingBase);
        cache.m_urdfLinkIndices2BulletLinkIndices.resize(numTotalLinksIncludingBase);
        cache.m_urdfLink2rigidBodies.resize(numTotalLinksIncludingBase);
		cache.m_urdfLinkLocalInertialFrames.resize(numTotalLinksIncludingBase);

        cache.m_currentMultiBodyLinkIndex = -1;//multi body base has 'link' index -1
        ComputeParentIndices(u2b,cache,rootLinkIndex,-2);
    }

}
开发者ID:RobotLocomotion,项目名称:bullet3,代码行数:21,代码来源:URDF2Bullet.cpp


示例9: InitURDF2BulletCache

void InitURDF2BulletCache(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int flags)
{
	//compute the number of links, and compute parent indices array (and possibly other cached data?)
	cache.m_totalNumJoints1 = 0;

	int rootLinkIndex = u2b.getRootLinkIndex();
	if (rootLinkIndex >= 0)
	{
		ComputeTotalNumberOfJoints(u2b, cache, rootLinkIndex);
		int numTotalLinksIncludingBase = 1 + cache.m_totalNumJoints1;

		cache.m_urdfLinkParentIndices.resize(numTotalLinksIncludingBase);
		cache.m_urdfLinkIndices2BulletLinkIndices.resize(numTotalLinksIncludingBase);
		cache.m_urdfLink2rigidBodies.resize(numTotalLinksIncludingBase);
		cache.m_urdfLinkLocalInertialFrames.resize(numTotalLinksIncludingBase);

		cache.m_currentMultiBodyLinkIndex = -1;  //multi body base has 'link' index -1
		
		bool maintainLinkOrder  = (flags & CUF_MAINTAIN_LINK_ORDER)!=0;
		if (maintainLinkOrder)
		{
			URDF2BulletCachedData cache2 = cache;

			ComputeParentIndices(u2b, cache2, rootLinkIndex, -2);

			for (int j=0;j<numTotalLinksIncludingBase;j++)
			{
				cache.m_urdfLinkParentIndices[j] = cache2.m_urdfLinkParentIndices[j];
				cache.m_urdfLinkIndices2BulletLinkIndices[j] = j - 1;
			}
		}else
		{
			ComputeParentIndices(u2b, cache, rootLinkIndex, -2);
		}

	}
}
开发者ID:YunfeiBai,项目名称:bullet3,代码行数:37,代码来源:URDF2Bullet.cpp


示例10: ConvertURDF2BulletInternal

void ConvertURDF2BulletInternal(
    const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation,
    URDF2BulletCachedData& cache, int urdfLinkIndex,
    const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,
    bool createMultiBody, const char* pathPrefix,
    int flags = 0)
{
    //b3Printf("start converting/extracting data from URDF interface\n");

    btTransform linkTransformInWorldSpace;
    linkTransformInWorldSpace.setIdentity();


    int mbLinkIndex =cache.getMbIndexFromUrdfIndex(urdfLinkIndex);

    int urdfParentIndex =   cache.getParentUrdfIndex(urdfLinkIndex);
    int mbParentIndex = cache.getMbIndexFromUrdfIndex(urdfParentIndex);
    btRigidBody* parentRigidBody = 0;

    //b3Printf("mb link index = %d\n",mbLinkIndex);

	btTransform parentLocalInertialFrame;
	parentLocalInertialFrame.setIdentity();
	btScalar parentMass(1);
	btVector3 parentLocalInertiaDiagonal(1,1,1);

    if (urdfParentIndex==-2)
    {
        //b3Printf("root link has no parent\n");
    } else
    {
        //b3Printf("urdf parent index = %d\n",urdfParentIndex);
        //b3Printf("mb parent index = %d\n",mbParentIndex);
        parentRigidBody = cache.getRigidBodyFromLink(urdfParentIndex);
		u2b.getMassAndInertia(urdfParentIndex, parentMass,parentLocalInertiaDiagonal,parentLocalInertialFrame);

    }

    btScalar mass = 0;
    btTransform localInertialFrame;
    localInertialFrame.setIdentity();
    btVector3 localInertiaDiagonal(0,0,0);
    u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,localInertialFrame);





    btTransform parent2joint;
    parent2joint.setIdentity();

    int jointType;
    btVector3 jointAxisInJointSpace;
    btScalar jointLowerLimit;
    btScalar jointUpperLimit;
    btScalar jointDamping;
    btScalar jointFriction;


    bool hasParentJoint = u2b.getJointInfo(urdfLinkIndex, parent2joint, linkTransformInWorldSpace, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit, jointDamping, jointFriction);
    if (flags & CUF_USE_SDF)
    {
        parent2joint =parentTransformInWorldSpace.inverse()*linkTransformInWorldSpace;
    }
    else
    {
        linkTransformInWorldSpace =parentTransformInWorldSpace*parent2joint;
    }
    
    

    btCompoundShape* compoundShape = u2b.convertLinkCollisionShapes(urdfLinkIndex,pathPrefix,localInertialFrame);
	
	int graphicsIndex = u2b.convertLinkVisualShapes(urdfLinkIndex,pathPrefix,localInertialFrame);
	
	


    if (compoundShape)
    {


        btVector3 color = selectColor2();
        /*
         if (visual->material.get())
         {
            color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a);
         }
         */
        if (mass)
        {
            if (!(flags & CUF_USE_URDF_INERTIA))
            {
                compoundShape->calculateLocalInertia(mass, localInertiaDiagonal);
            }
            URDFLinkContactInfo contactInfo;
            u2b.getLinkContactInfo(urdfLinkIndex,contactInfo);
            //temporary inertia scaling until we load inertia from URDF
            if (contactInfo.m_flags & URDF_CONTACT_HAS_INERTIA_SCALING)
            {
//.........这里部分代码省略.........
开发者ID:RobotLocomotion,项目名称:bullet3,代码行数:101,代码来源:URDF2Bullet.cpp


示例11: ConvertURDF2Bullet

void ConvertURDF2Bullet(
	const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation,
	const btTransform& rootTransformInWorldSpace,
	btMultiBodyDynamicsWorld* world1,
	bool createMultiBody, const char* pathPrefix, int flags, UrdfVisualShapeCache* cachedLinkGraphicsShapes)
{
	URDF2BulletCachedData cache;
	InitURDF2BulletCache(u2b, cache, flags);
	int urdfLinkIndex = u2b.getRootLinkIndex();
	int rootIndex = u2b.getRootLinkIndex();
	B3_PROFILE("ConvertURDF2Bullet");

	UrdfVisualShapeCache cachedLinkGraphicsShapesOut;


	bool recursive = (flags & CUF_MAINTAIN_LINK_ORDER)==0;
	if (recursive)
	{
		ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex, rootTransformInWorldSpace, world1, createMultiBody, pathPrefix, flags, cachedLinkGraphicsShapes, &cachedLinkGraphicsShapesOut, recursive);
	}
	else
	{
		
		btAlignedObjectArray<btTransform> parentTransforms;
		if (urdfLinkIndex >= parentTransforms.size())
		{
			parentTransforms.resize(urdfLinkIndex + 1);
		}
		parentTransforms[urdfLinkIndex] = rootTransformInWorldSpace;
		btAlignedObjectArray<childParentIndex> allIndices;
		
		GetAllIndices(u2b, cache, urdfLinkIndex, -1, allIndices);
		allIndices.quickSort(MyIntCompareFunc);

		for (int i = 0; i < allIndices.size(); i++)
		{
			int urdfLinkIndex = allIndices[i].m_index;
			int parentIndex = allIndices[i].m_parentIndex;
			btTransform parentTr = parentIndex >= 0 ? parentTransforms[parentIndex] : rootTransformInWorldSpace;
			btTransform tr = ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex, parentTr , world1, createMultiBody, pathPrefix, flags, cachedLinkGraphicsShapes, &cachedLinkGraphicsShapesOut, recursive);
			if ((urdfLinkIndex+1) >= parentTransforms.size())
			{
				parentTransforms.resize(urdfLinkIndex + 1);
			}
			parentTransforms[urdfLinkIndex] = tr;
		}
		


	}
	if (cachedLinkGraphicsShapes && cachedLinkGraphicsShapesOut.m_cachedUrdfLinkVisualShapeIndices.size() > cachedLinkGraphicsShapes->m_cachedUrdfLinkVisualShapeIndices.size())
	{
		*cachedLinkGraphicsShapes = cachedLinkGraphicsShapesOut;
	}

	if (world1 && cache.m_bulletMultiBody)
	{
		B3_PROFILE("Post process");
		btMultiBody* mb = cache.m_bulletMultiBody;

		mb->setHasSelfCollision((flags & CUF_USE_SELF_COLLISION) != 0);

		mb->finalizeMultiDof();

		btTransform localInertialFrameRoot = cache.m_urdfLinkLocalInertialFrames[urdfLinkIndex];

		if (flags & CUF_USE_MJCF)
		{
		}
		else
		{
			mb->setBaseWorldTransform(rootTransformInWorldSpace * localInertialFrameRoot);
		}
		btAlignedObjectArray<btQuaternion> scratch_q;
		btAlignedObjectArray<btVector3> scratch_m;
		mb->forwardKinematics(scratch_q, scratch_m);
		mb->updateCollisionObjectWorldTransforms(scratch_q, scratch_m);

		world1->addMultiBody(mb);
	}
}
开发者ID:YunfeiBai,项目名称:bullet3,代码行数:81,代码来源:URDF2Bullet.cpp


示例12: ConvertURDF2BulletInternal

btTransform ConvertURDF2BulletInternal(
	const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation,
	URDF2BulletCachedData& cache, int urdfLinkIndex,
	const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,
	bool createMultiBody, const char* pathPrefix,
	int flags, UrdfVisualShapeCache* cachedLinkGraphicsShapesIn, UrdfVisualShapeCache* cachedLinkGraphicsShapesOut, bool recursive)
{
	B3_PROFILE("ConvertURDF2BulletInternal2");
	//b3Printf("start converting/extracting data from URDF interface\n");

	btTransform linkTransformInWorldSpace;
	linkTransformInWorldSpace.setIdentity();

	int mbLinkIndex = cache.getMbIndexFromUrdfIndex(urdfLinkIndex);

	int urdfParentIndex = cache.getParentUrdfIndex(urdfLinkIndex);
	int mbParentIndex = cache.getMbIndexFromUrdfIndex(urdfParentIndex);
	btRigidBody* parentRigidBody = 0;

	//b3Printf("mb link index = %d\n",mbLinkIndex);

	btTransform parentLocalInertialFrame;
	parentLocalInertialFrame.setIdentity();
	btScalar parentMass(1);
	btVector3 parentLocalInertiaDiagonal(1, 1, 1);

	if (urdfParentIndex == -2)
	{
		//b3Printf("root link has no parent\n");
	}
	else
	{
		//b3Printf("urdf parent index = %d\n",urdfParentIndex);
		//b3Printf("mb parent index = %d\n",mbParentIndex);
		parentRigidBody = cache.getRigidBodyFromLink(urdfParentIndex);
		u2b.getMassAndInertia2(urdfParentIndex, parentMass, parentLocalInertiaDiagonal, parentLocalInertialFrame, flags);
	}

	btScalar mass = 0;
	btTransform localInertialFrame;
	localInertialFrame.setIdentity();
	btVector3 localInertiaDiagonal(0, 0, 0);
	u2b.getMassAndInertia2(urdfLinkIndex, mass, localInertiaDiagonal, localInertialFrame, flags);

	btTransform parent2joint;
	parent2joint.setIdentity();

	int jointType;
	btVector3 jointAxisInJointSpace;
	btScalar jointLowerLimit;
	btScalar jointUpperLimit;
	btScalar jointDamping;
	btScalar jointFriction;
	btScalar jointMaxForce;
	btScalar jointMaxVelocity;

	bool hasParentJoint = u2b.getJointInfo2(urdfLinkIndex, parent2joint, linkTransformInWorldSpace, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction, jointMaxForce, jointMaxVelocity);
	std::string linkName = u2b.getLinkName(urdfLinkIndex);

	if (flags & CUF_USE_SDF)
	{
		parent2joint = parentTransformInWorldSpace.inverse() * linkTransformInWorldSpace;
	}
	else
	{
		if (flags & CUF_USE_MJCF)
		{
			linkTransformInWorldSpace = parentTransformInWorldSpace * linkTransformInWorldSpace;
		}
		else
		{
			linkTransformInWorldSpace = parentTransformInWorldSpace * parent2joint;
		}
	}

	btCompoundShape* tmpShape = u2b.convertLinkCollisionShapes(urdfLinkIndex, pathPrefix, localInertialFrame);
	btCollisionShape* compoundShape = tmpShape;
	if (tmpShape->getNumChildShapes() == 1 && tmpShape->getChildTransform(0) == btTransform::getIdentity())
	{
		compoundShape = tmpShape->getChildShape(0);
	}

	int graphicsIndex;
	{
		B3_PROFILE("convertLinkVisualShapes");
		if (cachedLinkGraphicsShapesIn && cachedLinkGraphicsShapesIn->m_cachedUrdfLinkVisualShapeIndices.size() > (mbLinkIndex + 1))
		{
			graphicsIndex = cachedLinkGraphicsShapesIn->m_cachedUrdfLinkVisualShapeIndices[mbLinkIndex + 1];
			UrdfMaterialColor matColor = cachedLinkGraphicsShapesIn->m_cachedUrdfLinkColors[mbLinkIndex + 1];
			u2b.setLinkColor2(urdfLinkIndex, matColor);
		}
		else
		{
			graphicsIndex = u2b.convertLinkVisualShapes(urdfLinkIndex, pathPrefix, localInertialFrame);
			if (cachedLinkGraphicsShapesOut)
			{
				cachedLinkGraphicsShapesOut->m_cachedUrdfLinkVisualShapeIndices.push_back(graphicsIndex);
				UrdfMaterialColor matColor;
				u2b.getLinkColor2(urdfLinkIndex, matColor);
				cachedLinkGraphicsShapesOut->m_cachedUrdfLinkColors.push_back(matColor);
//.........这里部分代码省略.........
开发者ID:YunfeiBai,项目名称:bullet3,代码行数:101,代码来源:URDF2Bullet.cpp


示例13: ConvertURDF2BulletInternal

void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation, URDF2BulletCachedData& cache, int urdfLinkIndex, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,bool createMultiBody, const char* pathPrefix)
{
    printf("start converting/extracting data from URDF interface\n");
    
    btTransform linkTransformInWorldSpace;
    linkTransformInWorldSpace.setIdentity();
    

    int mbLinkIndex =cache.getMbIndexFromUrdfIndex(urdfLinkIndex);

    int urdfParentIndex =   cache.getParentUrdfIndex(urdfLinkIndex);
    int mbParentIndex = cache.getMbIndexFromUrdfIndex(urdfParentIndex);
    btRigidBody* parentRigidBody = 0;
    
    std::string name = u2b.getLinkName(urdfLinkIndex);
    printf("link name=%s urdf link index=%d\n",name.c_str(),urdfLinkIndex);
    printf("mb link index = %d\n",mbLinkIndex);

	btTransform parentLocalInertialFrame;
	parentLocalInertialFrame.setIdentity();
	btScalar parentMass(1);
	btVector3 parentLocalInertiaDiagonal(1,1,1);

    if (urdfParentIndex==-2)
    {
        printf("root link has no parent\n");
    } else
    {
        printf("urdf parent index = %d\n",urdfParentIndex);
        printf("mb parent index = %d\n",mbParentIndex);
        parentRigidBody = cache.getRigidBodyFromLink(urdfParentIndex);
		u2b.getMassAndInertia(urdfParentIndex, parentMass,parentLocalInertiaDiagonal,parentLocalInertialFrame);
        
    }
    
    btScalar mass = 0;
    btTransform localInertialFrame;
    localInertialFrame.setIdentity();
    btVector3 localInertiaDiagonal(0,0,0);
    u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,localInertialFrame);
    
	
	
    
    
    btTransform parent2joint;
    parent2joint.setIdentity();

    int jointType;
    btVector3 jointAxisInJointSpace;
    btScalar jointLowerLimit;
    btScalar jointUpperLimit;
    

    bool hasParentJoint = u2b.getJointInfo(urdfLinkIndex, parent2joint, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit);
    
    
    linkTransformInWorldSpace =parentTransformInWorldSpace*parent2joint;

    int graphicsIndex = u2b.convertLinkVisualShapes(urdfLinkIndex,pathPrefix,localInertialFrame);
    
    btCompoundShape* compoundShape = u2b.convertLinkCollisionShapes(urdfLinkIndex,pathPrefix,localInertialFrame);
    
    if (compoundShape)
    {
        
        
        btVector3 color = selectColor2();
        /* 
         if (visual->material.get())
         {
            color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a);
         }
         */
        //btVector3 localInertiaDiagonal(0, 0, 0);
        //if (mass)
        //{
        //	shape->calculateLocalInertia(mass, localInertiaDiagonal);
        //}
        
        btRigidBody* linkRigidBody = 0;
        btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace*localInertialFrame;
        
        if (!createMultiBody)
        {
            btRigidBody* body = creation.allocateRigidBody(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrameInWorldSpace, compoundShape);
            linkRigidBody = body;
            
            world1->addRigidBody(body, bodyCollisionFilterGroup, bodyCollisionFilterMask);
            
            compoundShape->setUserIndex(graphicsIndex);
            
            creation.createRigidBodyGraphicsInstance(urdfLinkIndex, body, color, graphicsIndex);
            cache.registerRigidBody(urdfLinkIndex, body, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
        } else
        {
            if (cache.m_bulletMultiBody==0)
            {
                bool multiDof = true;
                bool canSleep = false;
//.........这里部分代码省略.........
开发者ID:Mwenjoy,项目名称:bullet3,代码行数:101,代码来源:URDF2Bullet.cpp



注:本文中的URDFImporterInterface类示例由纯净天空整理自Github/MSDocs等源码及文档管理平台,相关代码片段筛选自各路编程大神贡献的开源项目,源码版权归原作者所有,传播和使用请参考对应项目的License;未经允许,请勿转载。


鲜花

握手

雷人

路过

鸡蛋
该文章已有0人参与评论

请发表评论

全部评论

专题导读
上一篇:
C++ URI类代码示例发布时间:2022-05-31
下一篇:
C++ UProperty类代码示例发布时间:2022-05-31
热门推荐
阅读排行榜

扫描微信二维码

查看手机版网站

随时了解更新最新资讯

139-2527-9053

在线客服(服务时间 9:00~18:00)

在线QQ客服
地址:深圳市南山区西丽大学城创智工业园
电邮:jeky_zhao#qq.com
移动电话:139-2527-9053

Powered by 互联科技 X3.4© 2001-2213 极客世界.|Sitemap