コード例 #1
0
void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
{
    
    switch (commandId)
    {
        case  CMD_LOAD_URDF:
        {
            
            b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model.urdf");
            
            //setting the initial position, orientation and other arguments are optional
            double startPosX = 0;
            static double startPosY = 0;
            double startPosZ = 0;
            b3LoadUrdfCommandSetStartPosition(commandHandle, startPosX,startPosY,startPosZ);
			startPosY += 2.f;
//            ret = b3LoadUrdfCommandSetUseFixedBase(commandHandle, 1);
            b3SubmitClientCommand(m_physicsClientHandle, commandHandle);

            break;
        }
        case CMD_REQUEST_CAMERA_IMAGE_DATA:
        {
            ///request an image from a simulated camera, using a software renderer.
            
            b3SharedMemoryCommandHandle commandHandle = b3InitRequestCameraImage(m_physicsClientHandle);
            
			float viewMatrix[16];
			float projectionMatrix[16];
			this->m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraProjectionMatrix(projectionMatrix);
			this->m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraViewMatrix(viewMatrix);
            b3RequestCameraImageSetCameraMatrices(commandHandle, viewMatrix,projectionMatrix);
            b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
            break;
        }
        case CMD_CREATE_BOX_COLLISION_SHAPE:
        {
            b3SharedMemoryCommandHandle commandHandle = b3CreateBoxShapeCommandInit(m_physicsClientHandle);
            b3CreateBoxCommandSetStartPosition(commandHandle,0,0,-3);
			b3CreateBoxCommandSetColorRGBA(commandHandle,0,0,1,1);
            b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
            break;
        }
		case CMD_CREATE_RIGID_BODY:
		{
			b3SharedMemoryCommandHandle commandHandle = b3CreateBoxShapeCommandInit(m_physicsClientHandle);
            b3CreateBoxCommandSetStartPosition(commandHandle,0,0,0);
			b3CreateBoxCommandSetMass(commandHandle,1);
			b3CreateBoxCommandSetCollisionShapeType(commandHandle,COLLISION_SHAPE_TYPE_CYLINDER_Y);
			b3CreateBoxCommandSetColorRGBA(commandHandle,1,1,0,1);
			double radius = 0.2;
			double halfHeight = 0.5;
			b3CreateBoxCommandSetHalfExtents(commandHandle,radius,halfHeight,radius);
            b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
			break;
		}

        case CMD_REQUEST_ACTUAL_STATE:
        {
			if (m_selectedBody>=0)
			{
				b3SharedMemoryCommandHandle commandHandle = b3RequestActualStateCommandInit(m_physicsClientHandle,m_selectedBody);
                b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_physicsClientHandle, commandHandle);
				b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
                
                int numJoints = b3GetNumJoints(m_physicsClientHandle, m_selectedBody);
                for (int i = 0; i < numJoints; ++i) {
                    struct b3JointSensorState sensorState;
                    b3GetJointState(m_physicsClientHandle, statusHandle, i, &sensorState);
                    b3Printf("Joint %d: %f", i, sensorState.m_jointMotorTorque);
                }
			}
            break;
        };

		case CMD_INIT_POSE:
		{
			
			if (m_selectedBody>=0)
			{
				b3SharedMemoryCommandHandle commandHandle = b3CreatePoseCommandInit(m_physicsClientHandle,m_selectedBody);
				static int toggle = 0;
				double pos[3] = {0,0,0};
				pos[toggle] = 2;
				toggle++; 
				if (toggle>2)
					toggle=0;

				btQuaternion orn;
				orn.setValue(0,0,0,1);

				switch (toggle)
				{
				case 0:
					orn = btQuaternion(btVector3(1,0,0),SIMD_HALF_PI);
					break;
				case 1:
					orn = btQuaternion(btVector3(0,1,0),SIMD_HALF_PI);
					break;
				case 2:
					orn = btQuaternion(btVector3(0,0,1),SIMD_HALF_PI);
					break;

				default:
					orn.setValue(0,0,0,1);
				};
				

				b3CreatePoseCommandSetBaseOrientation(commandHandle,orn[0],orn[1],orn[2],orn[3]);
				b3CreatePoseCommandSetBasePosition(commandHandle, pos[0],pos[1],pos[2]);
				int numJoints = b3GetNumJoints(m_physicsClientHandle,m_selectedBody);
				static double jointPos = SIMD_PI/2.f;
				
				for (int i=0;i<numJoints;i++)
				{
					b3JointInfo info;
					b3GetJointInfo(m_physicsClientHandle, m_selectedBody,i, &info);
					if ((info.m_jointType == 0) || (info.m_jointType == 1)) //revolute or prismatic
					{
						b3CreatePoseCommandSetJointPosition(m_physicsClientHandle,commandHandle,i,jointPos);
					}
				}
				jointPos += SIMD_PI/8.0;
				b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
			}
			break;
		}
        case CMD_STEP_FORWARD_SIMULATION:
        {
        
            b3SharedMemoryCommandHandle commandHandle = b3InitStepSimulationCommand(m_physicsClientHandle);
            b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
            break;
        }
            
        case CMD_REQUEST_DEBUG_LINES:
        {
            b3SharedMemoryCommandHandle commandHandle = b3InitRequestDebugLinesCommand(m_physicsClientHandle, btIDebugDraw::DBG_DrawWireframe);
            b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
            break;
        }
        case CMD_SEND_DESIRED_STATE:
        {
            // b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, CONTROL_MODE_VELOCITY);
            b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, CONTROL_MODE_POSITION_VELOCITY_PD);
            prepareControlCommand(command);
            b3SubmitClientCommand(m_physicsClientHandle, command);
            break;
        }
        case CMD_RESET_SIMULATION:
        {
            b3SharedMemoryCommandHandle commandHandle = b3InitResetSimulationCommand(m_physicsClientHandle);
            b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
            break;
        }
        case CMD_SEND_BULLET_DATA_STREAM:
        {
#if 0

            //this worked, but needs C-API and a streaming options, similar to debug lines
            command.m_type = buttonId;
            cl->enqueueCommand(command);
#endif

            break;
        }
        case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: {
            b3SharedMemoryCommandHandle commandHandle = b3InitPhysicsParamCommand(m_physicsClientHandle);
            b3PhysicsParamSetGravity(commandHandle, 0.0, 0.0, -9.8);
            b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
            break;
        }
        default:
        {
            b3Error("Unknown buttonId");
            btAssert(0);
        }
    };
}
コード例 #2
0
void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
{
	switch (commandId)
	{
		case CMD_LOAD_URDF:
		{
			b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model.urdf");
			//setting the initial position, orientation and other arguments are optional
			double startPosX = 0;
			static double startPosY = 0;
			double startPosZ = 0;
			b3LoadUrdfCommandSetStartPosition(commandHandle, startPosX, startPosY, startPosZ);
			startPosY += 2.f;
			//            ret = b3LoadUrdfCommandSetUseFixedBase(commandHandle, 1);
			b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
			break;
		}

		case CMD_LOAD_SDF:
		{
#ifdef BT_DEBUG
			b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "two_cubes.sdf");
#else
			b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "kitchens/1.sdf");  //two_cubes.sdf");//kitchens/1.sdf");//kuka_iiwa/model.sdf");
#endif
			b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
			break;
		}
		case CMD_REQUEST_CAMERA_IMAGE_DATA:
		{
			///request an image from a simulated camera, using a software renderer.
			b3SharedMemoryCommandHandle commandHandle = b3InitRequestCameraImage(m_physicsClientHandle);
			//b3RequestCameraImageSelectRenderer(commandHandle,ER_BULLET_HARDWARE_OPENGL);

			float viewMatrix[16];
			float projectionMatrix[16];
			m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraProjectionMatrix(projectionMatrix);
			m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraViewMatrix(viewMatrix);

			b3RequestCameraImageSetCameraMatrices(commandHandle, viewMatrix, projectionMatrix);
			b3RequestCameraImageSetPixelResolution(commandHandle, camVisualizerWidth, camVisualizerHeight);
			float lightPos[3];
			lightPos[0] = m_lightPos[0];
			lightPos[1] = m_lightPos[1];
			lightPos[2] = m_lightPos[2];
			b3RequestCameraImageSetLightDirection(commandHandle, lightPos);
			b3RequestCameraImageSetLightSpecularCoeff(commandHandle, m_specularCoeff);
			b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
			break;
		}
		case CMD_CREATE_BOX_COLLISION_SHAPE:
		{
			b3SharedMemoryCommandHandle commandHandle = b3CreateBoxShapeCommandInit(m_physicsClientHandle);
			b3CreateBoxCommandSetStartPosition(commandHandle, 0, 0, -1.5);
			b3CreateBoxCommandSetColorRGBA(commandHandle, 0, 0, 1, 1);
			b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
			break;
		}
		case CMD_CREATE_RIGID_BODY:
		{
			b3SharedMemoryCommandHandle commandHandle = b3CreateBoxShapeCommandInit(m_physicsClientHandle);
			b3CreateBoxCommandSetStartPosition(commandHandle, 0, 0, 0);
			b3CreateBoxCommandSetMass(commandHandle, 1);
			b3CreateBoxCommandSetCollisionShapeType(commandHandle, COLLISION_SHAPE_TYPE_CYLINDER_Y);
			b3CreateBoxCommandSetColorRGBA(commandHandle, 1, 1, 0, 1);
			double radius = 0.2;
			double halfHeight = 0.5;
			b3CreateBoxCommandSetHalfExtents(commandHandle, radius, halfHeight, radius);
			b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
			break;
		}

		case CMD_REQUEST_ACTUAL_STATE:
		{
			if (m_selectedBody >= 0)
			{
				b3SharedMemoryCommandHandle commandHandle = b3RequestActualStateCommandInit(m_physicsClientHandle, m_selectedBody);
				b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_physicsClientHandle, commandHandle);
				b3SubmitClientCommand(m_physicsClientHandle, commandHandle);

				int numJoints = b3GetNumJoints(m_physicsClientHandle, m_selectedBody);
				for (int i = 0; i < numJoints; ++i)
				{
					struct b3JointSensorState sensorState;
					b3GetJointState(m_physicsClientHandle, statusHandle, i, &sensorState);
					//b3Printf("Joint %d: %f", i, sensorState.m_jointMotorTorque);
				}
			}

			break;
		};

		case CMD_INIT_POSE:
		{
			if (m_selectedBody >= 0)
			{
				b3SharedMemoryCommandHandle commandHandle = b3CreatePoseCommandInit(m_physicsClientHandle, m_selectedBody);
				static int toggle = 0;
				double pos[3] = {0, 0, 0};
				pos[toggle] = 2;
				toggle++;
				if (toggle > 2)
					toggle = 0;

				btQuaternion orn;
				orn.setValue(0, 0, 0, 1);

				switch (toggle)
				{
					case 0:
						orn = btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI);
						break;
					case 1:
						orn = btQuaternion(btVector3(0, 1, 0), SIMD_HALF_PI);
						break;
					case 2:
						orn = btQuaternion(btVector3(0, 0, 1), SIMD_HALF_PI);
						break;

					default:
						orn.setValue(0, 0, 0, 1);
				};

				b3CreatePoseCommandSetBaseOrientation(commandHandle, orn[0], orn[1], orn[2], orn[3]);
				b3CreatePoseCommandSetBasePosition(commandHandle, pos[0], pos[1], pos[2]);
				int numJoints = b3GetNumJoints(m_physicsClientHandle, m_selectedBody);
				static double jointPos = SIMD_PI / 2.f;

				for (int i = 0; i < numJoints; i++)
				{
					b3JointInfo info;
					b3GetJointInfo(m_physicsClientHandle, m_selectedBody, i, &info);
					if ((info.m_jointType == 0) || (info.m_jointType == 1))  //revolute or prismatic
					{
						b3CreatePoseCommandSetJointPosition(m_physicsClientHandle, commandHandle, i, jointPos);
					}
				}
				jointPos += SIMD_PI / 8.0;
				b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
			}
			break;
		}
		case CMD_STEP_FORWARD_SIMULATION:
		{
			b3SharedMemoryCommandHandle commandHandle = b3InitStepSimulationCommand(m_physicsClientHandle);
			b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
			break;
		}

		case CMD_REQUEST_DEBUG_LINES:
		{
			b3SharedMemoryCommandHandle commandHandle = b3InitRequestDebugLinesCommand(m_physicsClientHandle, btIDebugDraw::DBG_DrawWireframe);
			b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
			break;
		}
		case CMD_SEND_DESIRED_STATE:
		{
			if (m_selectedBody >= 0)
			{
				// b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, m_selectedBody, CONTROL_MODE_VELOCITY);
				b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_physicsClientHandle, m_selectedBody, CONTROL_MODE_POSITION_VELOCITY_PD);
				//  b3Printf("prepare control command for body %d", m_selectedBody);

				prepareControlCommand(command);

				b3SubmitClientCommand(m_physicsClientHandle, command);
			}
			break;
		}
		case CMD_RESET_SIMULATION:
		{
			b3SharedMemoryCommandHandle commandHandle = b3InitResetSimulationCommand(m_physicsClientHandle);
			b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
			break;
		}
		case CMD_SEND_BULLET_DATA_STREAM:
		{
#if 0

            //this worked, but needs C-API and a streaming options, similar to debug lines
            command.m_type = buttonId;
            cl->enqueueCommand(command);
#endif

			break;
		}
		case CMD_CUSTOM_SET_GRAVITY:
		{
			b3SharedMemoryCommandHandle commandHandle = b3InitPhysicsParamCommand(m_physicsClientHandle);
			b3PhysicsParamSetGravity(commandHandle, 0.0, 0.0, -9.8);
			b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
			break;
		}

		case CMD_CUSTOM_SET_REALTIME_SIMULATION:
		{
			b3SharedMemoryCommandHandle commandHandle = b3InitPhysicsParamCommand(m_physicsClientHandle);
			b3PhysicsParamSetRealTimeSimulation(commandHandle, 1);
			b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
			break;
		}

		case CMD_CALCULATE_INVERSE_DYNAMICS:
		{
			if (m_selectedBody >= 0)
			{
				btAlignedObjectArray<double> jointPositionsQ;
				btAlignedObjectArray<double> jointVelocitiesQdot;
				btAlignedObjectArray<double> jointAccelerations;
				int numJoints = b3GetNumJoints(m_physicsClientHandle, m_selectedBody);
				if (numJoints)
				{
					b3Printf("Compute inverse dynamics for joint accelerations:");
					jointPositionsQ.resize(numJoints);
					jointVelocitiesQdot.resize(numJoints);
					jointAccelerations.resize(numJoints);
					for (int i = 0; i < numJoints; i++)
					{
						jointAccelerations[i] = 100;
						b3Printf("Desired joint acceleration[%d]=%f", i, jointAccelerations[i]);
					}
					b3SharedMemoryCommandHandle commandHandle = b3CalculateInverseDynamicsCommandInit(m_physicsClientHandle,
																									  m_selectedBody, &jointPositionsQ[0], &jointVelocitiesQdot[0], &jointAccelerations[0]);
					b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
				}
			}
			break;
		}
		case CMD_REQUEST_CONTACT_POINT_INFORMATION:
		{
			b3SharedMemoryCommandHandle commandHandle = b3InitRequestContactPointInformation(m_physicsClientHandle);
			b3SetContactFilterBodyA(commandHandle, 0);
			b3SetContactFilterBodyB(commandHandle, 1);
			b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
			break;
		}
		case CMD_SAVE_WORLD:
		{
			b3SharedMemoryCommandHandle commandHandle = b3SaveWorldCommandInit(m_physicsClientHandle, "saveWorld.py");
			b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
			break;
		}
		case CMD_REQUEST_VISUAL_SHAPE_INFO:
		{
			if (m_selectedBody >= 0)
			{
				//request visual shape information
				b3SharedMemoryCommandHandle commandHandle = b3InitRequestVisualShapeInformation(m_physicsClientHandle, m_selectedBody);
				b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
			}
			break;
		}
		case CMD_SET_SHADOW:
		{
			b3SharedMemoryCommandHandle commandHandle = b3InitRequestCameraImage(m_physicsClientHandle);
			float viewMatrix[16];
			float projectionMatrix[16];
			m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraProjectionMatrix(projectionMatrix);
			m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraViewMatrix(viewMatrix);

			b3RequestCameraImageSetCameraMatrices(commandHandle, viewMatrix, projectionMatrix);
			b3RequestCameraImageSetPixelResolution(commandHandle, camVisualizerWidth, camVisualizerHeight);
			bool hasShadow = true;
			b3RequestCameraImageSetShadow(commandHandle, hasShadow);
			b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
			break;
		}
		case CMD_UPDATE_VISUAL_SHAPE:
		{
			int objectUniqueId = 0;
			int linkIndex = -1;
			int shapeIndex = -1;
			int textureIndex = -2;
			double rgbaColor[4] = {0.0, 1.0, 0.0, 1.0};
			b3SharedMemoryCommandHandle commandHandle = b3InitUpdateVisualShape2(m_physicsClientHandle, objectUniqueId, linkIndex, shapeIndex);
			b3UpdateVisualShapeRGBAColor(commandHandle, rgbaColor);
			b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
			break;
		}

		default:
		{
			b3Error("Unknown buttonId");
			btAssert(0);
		}
	};
}