Пример #1
0
    Tutorial(GUIHelperInterface* guiHelper, int tutorialIndex)
    :m_app(guiHelper->getAppInterface()),
	m_guiHelper(guiHelper),
	m_tutorialIndex(tutorialIndex),
	m_stage(0),
	m_counter(0),
	m_timeSeriesCanvas0(0),
	m_timeSeriesCanvas1(0)
    {
		int numBodies = 1;
		
		m_app->setUpAxis(1);
		m_app->m_renderer->enableBlend(true);
		
		switch (m_tutorialIndex)
		{
			case TUT_VELOCITY:
			{
				numBodies=10;
				m_timeSeriesCanvas0 = new TimeSeriesCanvas(m_app->m_2dCanvasInterface,512,256,"Constant Velocity");
				
				m_timeSeriesCanvas0 ->setupTimeSeries(2,60, 0);
				m_timeSeriesCanvas0->addDataSource("X position (m)", 255,0,0);
				m_timeSeriesCanvas0->addDataSource("X velocity (m/s)", 0,0,255);
				m_timeSeriesCanvas0->addDataSource("dX/dt (m/s)", 0,0,0);
				break;
			}
			case TUT_ACCELERATION:
			{
				numBodies=10;
				m_timeSeriesCanvas1 = new TimeSeriesCanvas(m_app->m_2dCanvasInterface,256,512,"Constant Acceleration");
				
				m_timeSeriesCanvas1 ->setupTimeSeries(50,60, 0);
				m_timeSeriesCanvas1->addDataSource("Y position (m)", 255,0,0);
				m_timeSeriesCanvas1->addDataSource("Y velocity (m/s)", 0,0,255);
				m_timeSeriesCanvas1->addDataSource("dY/dt (m/s)", 0,0,0);
				break;
			}
			case TUT_COLLISION:
			{
				numBodies=2;
				m_timeSeriesCanvas1 = new TimeSeriesCanvas(m_app->m_2dCanvasInterface,512,200,"Distance");
				m_timeSeriesCanvas1 ->setupTimeSeries(1.5,60, 0);
				m_timeSeriesCanvas1->addDataSource("distance", 255,0,0);
				break;
			}
			
			case TUT_SOLVE_CONTACT_CONSTRAINT:
			{
				numBodies=2;
				m_timeSeriesCanvas1 = new TimeSeriesCanvas(m_app->m_2dCanvasInterface,512,200,"Collision Impulse");
				m_timeSeriesCanvas1 ->setupTimeSeries(1.5,60, 0);
				m_timeSeriesCanvas1->addDataSource("Distance", 0,0,255);
				m_timeSeriesCanvas1->addDataSource("Impulse magnutide", 255,0,0);
				
				{
					SliderParams slider("Restitution",&gRestitution);
					slider.m_minVal=0;
					slider.m_maxVal=1;
					m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
				}
				{
					SliderParams slider("Mass A",&gMassA);
					slider.m_minVal=0;
					slider.m_maxVal=100;
					m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
				}
				
				{
					SliderParams slider("Mass B",&gMassB);
					slider.m_minVal=0;
					slider.m_maxVal=100;
					m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
				}
				
				
				
				break;
			}
				
			default:
			{
				
				m_timeSeriesCanvas0 = new TimeSeriesCanvas(m_app->m_2dCanvasInterface,512,256,"Unknown");
				m_timeSeriesCanvas0 ->setupTimeSeries(1,60, 0);
				
			}
		};

		
		
		if (m_tutorialIndex==TUT_VELOCITY)
		{

		 int boxId = m_app->registerCubeShape(100,1,100);
            b3Vector3 pos = b3MakeVector3(0,-3.5,0);
            b3Quaternion orn(0,0,0,1);
            b3Vector4 color = b3MakeVector4(1,1,1,1);
            b3Vector3 scaling = b3MakeVector3(1,1,1);
            m_app->m_renderer->registerGraphicsInstance(boxId,pos,orn,color,scaling);
		}

		for (int i=0;i<numBodies;i++)
		{
			m_bodies.push_back(new LWRigidBody());
		}
		for (int i=0;i<m_bodies.size();i++)
		{
			m_bodies[i]->m_worldPose.m_position.setValue((i/4)*5,3,(i&3)*5);
		}
		{
			int textureIndex = -1;
			
			if (1)
			{
				int width,height,n;
				
				const char* filename = "data/cube.png";
				const unsigned char* image=0;
				
				const char* prefix[]={"./","../","../../","../../../","../../../../"};
				int numprefix = sizeof(prefix)/sizeof(const char*);
				
				for (int i=0;!image && i<numprefix;i++)
				{
					char relativeFileName[1024];
					sprintf(relativeFileName,"%s%s",prefix[i],filename);
					image = stbi_load(relativeFileName, &width, &height, &n, 0);
				}
				
				b3Assert(image);
				if (image)
				{
					textureIndex = m_app->m_renderer->registerTexture(image,width,height);
				}
			}
			
			//            int boxId = m_app->registerCubeShape(1,1,1,textureIndex);
			int boxId = m_app->registerGraphicsUnitSphereShape(SPHERE_LOD_HIGH, textureIndex);
			b3Vector4 color = b3MakeVector4(1,1,1,0.8);
			b3Vector3 scaling = b3MakeVector3(SPHERE_RADIUS,SPHERE_RADIUS,SPHERE_RADIUS);
			for (int i=0;i<m_bodies.size();i++)
			{
				m_bodies[i]->m_collisionShape.m_sphere.m_radius = SPHERE_RADIUS;
				m_bodies[i]->m_collisionShape.m_type = LW_SPHERE_TYPE;
				
				m_bodies[i]->m_graphicsIndex = m_app->m_renderer->registerGraphicsInstance(boxId,m_bodies[i]->m_worldPose.m_position, m_bodies[i]->m_worldPose.m_orientation,color,scaling);
				m_app->m_renderer->writeSingleInstanceTransformToCPU(m_bodies[i]->m_worldPose.m_position, m_bodies[i]->m_worldPose.m_orientation, m_bodies[i]->m_graphicsIndex);
			}
		}

		
		if (m_tutorialIndex == TUT_SOLVE_CONTACT_CONSTRAINT)
		{
			m_bodies[0]->m_invMass = gMassA? 1./gMassA : 0;
			m_bodies[0]->m_collisionShape.m_sphere.computeLocalInertia(gMassA,m_bodies[0]->m_localInertia);
			
			m_bodies[1]->m_invMass =gMassB? 1./gMassB : 0;
			m_bodies[1]->m_collisionShape.m_sphere.computeLocalInertia(gMassB,m_bodies[1]->m_localInertia);

			if (gMassA)
				m_bodies[0]->m_linearVelocity.setValue(0,0,1);
			if (gMassB)
				m_bodies[1]->m_linearVelocity.setValue(0,0,-1);

		}
		

			
		 m_app->m_renderer->writeTransforms();
    }
void InverseDynamicsExample::initPhysics()
{
    //roboticists like Z up
    int upAxis = 2;
    m_guiHelper->setUpAxis(upAxis);

    createEmptyDynamicsWorld();
    btVector3 gravity(0,0,0);
   // gravity[upAxis]=-9.8;
    m_dynamicsWorld->setGravity(gravity);

    m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);

    {
        SliderParams slider("Kp",&kp);
        slider.m_minVal=0;
        slider.m_maxVal=2000;
		if (m_guiHelper->getParameterInterface())
	        m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
    }
    {
        SliderParams slider("Kd",&kd);
        slider.m_minVal=0;
        slider.m_maxVal=50;
		if (m_guiHelper->getParameterInterface())
	        m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
    }

    if (m_option == BT_ID_PROGRAMMATICALLY)
    {
        ButtonParams button("toggle inverse model",0,true);
        button.m_callback = toggleUseInverseModel;
        m_guiHelper->getParameterInterface()->registerButtonParameter(button);
    }

   
    switch (m_option)
    {
    case BT_ID_LOAD_URDF:
        {


			
            BulletURDFImporter u2b(m_guiHelper,0,1);
			bool loadOk = u2b.loadURDF("kuka_iiwa/model.urdf");// lwr / kuka.urdf");
            if (loadOk)
            {
                    int rootLinkIndex = u2b.getRootLinkIndex();
                    b3Printf("urdf root link index = %d\n",rootLinkIndex);
                    MyMultiBodyCreator creation(m_guiHelper);
                    btTransform identityTrans;
                    identityTrans.setIdentity();
                    ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,true,u2b.getPathPrefix());
					for (int i = 0; i < u2b.getNumAllocatedCollisionShapes(); i++)
					{
						m_collisionShapes.push_back(u2b.getAllocatedCollisionShape(i));
					}
                    m_multiBody = creation.getBulletMultiBody();
                    if (m_multiBody)
                    {
                        //kuka without joint control/constraints will gain energy explode soon due to timestep/integrator
                        //temporarily set some extreme damping factors until we have some joint control or constraints
                        m_multiBody->setAngularDamping(0*0.99);
                        m_multiBody->setLinearDamping(0*0.99);
                        b3Printf("Root link name = %s",u2b.getLinkName(u2b.getRootLinkIndex()).c_str());
                    }
            }
            break;
        }
    case BT_ID_PROGRAMMATICALLY:
        {
            btTransform baseWorldTrans;
            baseWorldTrans.setIdentity();
            m_multiBody = createInvertedPendulumMultiBody(m_dynamicsWorld, m_guiHelper, baseWorldTrans, false);
            break;
        }
    default:
        {
            b3Error("Unknown option in InverseDynamicsExample::initPhysics");
            b3Assert(0);
        }
    };


    if(m_multiBody) {
        {
			if (m_guiHelper->getAppInterface() && m_guiHelper->getParameterInterface())
			{
				m_timeSeriesCanvas = new TimeSeriesCanvas(m_guiHelper->getAppInterface()->m_2dCanvasInterface,512,230, "Joint Space Trajectory");
				m_timeSeriesCanvas ->setupTimeSeries(3,100, 0);
			}           
        }
        
        // construct inverse model
        btInverseDynamics::btMultiBodyTreeCreator id_creator;
        if (-1 == id_creator.createFromBtMultiBody(m_multiBody, false)) {
            b3Error("error creating tree\n");
        } else {
            m_inverseModel = btInverseDynamics::CreateMultiBodyTree(id_creator);
        }
        // add joint target controls
        qd.resize(m_multiBody->getNumDofs());
		
        qd_name.resize(m_multiBody->getNumDofs());
       	q_name.resize(m_multiBody->getNumDofs()); 

		if (m_timeSeriesCanvas && m_guiHelper->getParameterInterface())
		{
			for(std::size_t dof=0;dof<qd.size();dof++) 
			{
				qd[dof] = 0;
				char tmp[25];
				sprintf(tmp,"q_desired[%lu]",dof);
				qd_name[dof] = tmp;
				SliderParams slider(qd_name[dof].c_str(),&qd[dof]);
				slider.m_minVal=-3.14;
				slider.m_maxVal=3.14;
				
				sprintf(tmp,"q[%lu]",dof); 
				q_name[dof] = tmp;   
				m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
				btVector4 color = sJointCurveColors[dof&7];
				m_timeSeriesCanvas->addDataSource(q_name[dof].c_str(), color[0]*255,color[1]*255,color[2]*255);
		
			}
		}
        
    }
    
    m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);

}