From c02e197e778074f069711751f2c09e92bd862891 Mon Sep 17 00:00:00 2001 From: Jorge Bernal Date: Sun, 17 May 2015 17:39:48 +0200 Subject: [PATCH 01/53] New btConeshape member functions to can set radius and height for the cone shape after creation --- src/BulletCollision/CollisionShapes/btConeShape.h | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/BulletCollision/CollisionShapes/btConeShape.h b/src/BulletCollision/CollisionShapes/btConeShape.h index 4a0df0d55..46d78d148 100644 --- a/src/BulletCollision/CollisionShapes/btConeShape.h +++ b/src/BulletCollision/CollisionShapes/btConeShape.h @@ -43,6 +43,15 @@ public: btScalar getRadius() const { return m_radius;} btScalar getHeight() const { return m_height;} + void setRadius(const btScalar radius) + { + m_radius = radius; + } + void setHeight(const btScalar height) + { + m_height = height; + } + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const { From 5bf125cab525778f5f73395eacfa61fc542486d1 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 2 Jun 2016 13:54:52 -0700 Subject: [PATCH 02/53] fix some compile issues using premake use textured cube/sphere model for test --- data/cube.mtl | 2 +- examples/RenderingExamples/TinyRendererSetup.cpp | 4 +++- examples/SharedMemory/PhysicsClientExample.cpp | 7 +++++-- .../main_sw_tinyrenderer_single_example.cpp | 3 ++- .../main_tinyrenderer_single_example.cpp | 3 ++- examples/TinyRenderer/TinyRenderer.cpp | 13 +++++++------ examples/TinyRenderer/model.cpp | 6 +++--- 7 files changed, 23 insertions(+), 15 deletions(-) diff --git a/data/cube.mtl b/data/cube.mtl index fca828974..26b4b44e4 100644 --- a/data/cube.mtl +++ b/data/cube.mtl @@ -10,5 +10,5 @@ newmtl cube Ks 0.0000 0.0000 0.0000 Ke 0.0000 0.0000 0.0000 map_Ka cube.tga - map_Kd floor_diffuse.tga + map_Kd cube.png diff --git a/examples/RenderingExamples/TinyRendererSetup.cpp b/examples/RenderingExamples/TinyRendererSetup.cpp index d7e9f05aa..11258779d 100644 --- a/examples/RenderingExamples/TinyRendererSetup.cpp +++ b/examples/RenderingExamples/TinyRendererSetup.cpp @@ -147,7 +147,9 @@ TinyRendererSetup::TinyRendererSetup(struct GUIHelperInterface* gui) m_internalData = new TinyRendererSetupInternalData(gui->getAppInterface()->m_window->getWidth(),gui->getAppInterface()->m_window->getHeight()); m_app->m_renderer->enableBlend(true); - const char* fileName = "teddy.obj";//cube.obj";//textured_sphere_smooth.obj";//cube.obj"; + + const char* fileName = "textured_sphere_smooth.obj"; + fileName = "cube.obj"; { diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index e249977bd..a57fde289 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -527,7 +527,6 @@ void PhysicsClientExample::initPhysics() m_selectedBody = -1; m_prevSelectedBody = -1; - if (m_options == eCLIENTEXAMPLE_SERVER) { m_canvas = m_guiHelper->get2dCanvasInterface(); if (m_canvas) @@ -557,9 +556,13 @@ void PhysicsClientExample::initPhysics() } - m_isOptionalServerConnected = m_physicsServer.connectSharedMemory( m_guiHelper); } + if (m_options == eCLIENTEXAMPLE_SERVER) + { + m_isOptionalServerConnected = m_physicsServer.connectSharedMemory( m_guiHelper); + } + m_physicsClientHandle = b3ConnectSharedMemory(m_sharedMemoryKey); //m_physicsClientHandle = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY); //m_physicsClientHandle = b3ConnectPhysicsDirect(); diff --git a/examples/StandaloneMain/main_sw_tinyrenderer_single_example.cpp b/examples/StandaloneMain/main_sw_tinyrenderer_single_example.cpp index 69c3b7db5..786e0eed1 100644 --- a/examples/StandaloneMain/main_sw_tinyrenderer_single_example.cpp +++ b/examples/StandaloneMain/main_sw_tinyrenderer_single_example.cpp @@ -111,7 +111,8 @@ public: if (shapeIndex>=0) { TinyRenderObjectData* swObj = new TinyRenderObjectData(m_swWidth,m_swHeight,m_rgbColorBuffer,m_depthBuffer); - swObj->registerMeshShape(vertices,numvertices,indices,numIndices); + float rgbaColor[4] = {1,1,1,1}; + swObj->registerMeshShape(vertices,numvertices,indices,numIndices,rgbaColor); //swObj->createCube(1,1,1);//MeshShape(vertices,numvertices,indices,numIndices); m_swRenderObjects.insert(shapeIndex,swObj); } diff --git a/examples/StandaloneMain/main_tinyrenderer_single_example.cpp b/examples/StandaloneMain/main_tinyrenderer_single_example.cpp index 77ab903ab..157ab233c 100644 --- a/examples/StandaloneMain/main_tinyrenderer_single_example.cpp +++ b/examples/StandaloneMain/main_tinyrenderer_single_example.cpp @@ -254,7 +254,8 @@ struct TinyRendererGUIHelper : public GUIHelperInterface int shapeIndex = m_swRenderObjects.size(); TinyRenderObjectData* swObj = new TinyRenderObjectData(m_swWidth,m_swHeight,m_rgbColorBuffer,m_depthBuffer); - swObj->registerMeshShape(vertices,numvertices,indices,numIndices); + float rgbaColor[4] = {1,1,1,1}; + swObj->registerMeshShape(vertices,numvertices,indices,numIndices,rgbaColor); //swObj->createCube(1,1,1);//MeshShape(vertices,numvertices,indices,numIndices); m_swRenderObjects.insert(shapeIndex,swObj); return shapeIndex; diff --git a/examples/TinyRenderer/TinyRenderer.cpp b/examples/TinyRenderer/TinyRenderer.cpp index 77c387176..e7dfb8f05 100644 --- a/examples/TinyRenderer/TinyRenderer.cpp +++ b/examples/TinyRenderer/TinyRenderer.cpp @@ -76,12 +76,13 @@ struct Shader : public IShader { float diff = ambient+b3Min(b3Max(0.f, bn*m_light_dir_local),(1-ambient)); //float diff = b3Max(0.f, n*m_light_dir_local); color = m_model->diffuse(uv)*diff; - //colors are store in BGRA? - color = TGAColor(color[0]*m_colorRGBA[2], - color[1]*m_colorRGBA[1], - color[2]*m_colorRGBA[0], - color[3]*m_colorRGBA[3]); - + + //warning: bgra color is swapped to rgba to upload texture + color.bgra[0] *= m_colorRGBA[0]; + color.bgra[1] *= m_colorRGBA[1]; + color.bgra[2] *= m_colorRGBA[2]; + color.bgra[3] *= m_colorRGBA[3]; + return false; } }; diff --git a/examples/TinyRenderer/model.cpp b/examples/TinyRenderer/model.cpp index 626cc2331..f7a0bea75 100644 --- a/examples/TinyRenderer/model.cpp +++ b/examples/TinyRenderer/model.cpp @@ -57,9 +57,9 @@ void Model::setDiffuseTextureFromData(unsigned char* textureImage,int textureWid for (int j=0;j Date: Thu, 2 Jun 2016 18:04:22 -0700 Subject: [PATCH 03/53] Support the field under in SDF to make the model immovable, similar to setting the mass to zero. Add joint velocity motors in ImportSDF example. --- .../ImportSDFDemo/ImportSDFSetup.cpp | 54 +++++++++++++++++++ .../ImportURDFDemo/BulletUrdfImporter.cpp | 17 ++++-- .../Importers/ImportURDFDemo/UrdfParser.cpp | 14 ++++- .../Importers/ImportURDFDemo/UrdfParser.h | 7 +++ 4 files changed, 88 insertions(+), 4 deletions(-) diff --git a/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp b/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp index 54883298b..f55cc9758 100644 --- a/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp +++ b/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp @@ -235,6 +235,60 @@ void ImportSDFSetup::initPhysics() ConvertURDF2Bullet(u2b,creation, rootTrans,m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix(),true); mb = creation.getBulletMultiBody(); + + if (m_useMultiBody && mb ) + { + std::string* name = new std::string(u2b.getLinkName(u2b.getRootLinkIndex())); + m_nameMemory.push_back(name); +#ifdef TEST_MULTIBODY_SERIALIZATION + s->registerNameForPointer(name->c_str(),name->c_str()); +#endif//TEST_MULTIBODY_SERIALIZATION + mb->setBaseName(name->c_str()); + //create motors for each btMultiBody joint + int numLinks = mb->getNumLinks(); + for (int i=0;iregisterNameForPointer(jointName->c_str(),jointName->c_str()); + s->registerNameForPointer(linkName->c_str(),linkName->c_str()); +#endif//TEST_MULTIBODY_SERIALIZATION + m_nameMemory.push_back(jointName); + m_nameMemory.push_back(linkName); + + mb->getLink(i).m_linkName = linkName->c_str(); + mb->getLink(i).m_jointName = jointName->c_str(); + + if (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute + ||mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::ePrismatic + ) + { + if (m_data->m_numMotorsc_str()); + btScalar* motorVel = &m_data->m_motorTargetVelocities[m_data->m_numMotors]; + *motorVel = 0.f; + SliderParams slider(motorName,motorVel); + slider.m_minVal=-4; + slider.m_maxVal=4; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + float maxMotorImpulse = 10.1f; + btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,0,0,maxMotorImpulse); + //motor->setMaxAppliedImpulse(0); + m_data->m_jointMotors[m_data->m_numMotors]=motor; + m_dynamicsWorld->addMultiBodyConstraint(motor); + m_data->m_numMotors++; + } + } + + } + } } diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 7e9624d64..4838079a0 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -267,14 +267,25 @@ void BulletURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVect //the link->m_inertia is NOT necessarily aligned with the inertial frame //so an additional transform might need to be computed UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex); + + btAssert(linkPtr); if (linkPtr) { UrdfLink* link = *linkPtr; - mass = link->m_inertia.m_mass; + if (link->m_parentJoint==0 && m_data->m_urdfParser.getModel().m_overrideFixedBase) + { + mass = 0.f; + localInertiaDiagonal.setValue(0,0,0); + } + else + { + mass = link->m_inertia.m_mass; + localInertiaDiagonal.setValue(link->m_inertia.m_ixx,link->m_inertia.m_iyy, + link->m_inertia.m_izz); + } inertialFrame = link->m_inertia.m_linkLocalFrame; - localInertiaDiagonal.setValue(link->m_inertia.m_ixx,link->m_inertia.m_iyy, - link->m_inertia.m_izz); + } else { diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index 60b40b512..430593b5e 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -529,6 +529,8 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi link.m_name = linkName; if (m_parseSDF) { + + TiXmlElement* pose = config->FirstChildElement("pose"); if (0==pose) { @@ -572,7 +574,7 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi logger->reportWarning(link.m_name.c_str()); } } - + // Multiple Visuals (optional) for (TiXmlElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual")) { @@ -1240,6 +1242,16 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger) UrdfModel* localModel = new UrdfModel; m_tmpModels.push_back(localModel); + TiXmlElement* stat = robot_xml->FirstChildElement("static"); + if (0!=stat) + { + int val = int(atof(stat->GetText())); + if (val==1) + { + localModel->m_overrideFixedBase = true; + } + } + // Get robot name const char *name = robot_xml->Attribute("name"); diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index 1a855f281..b83c0b3c8 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -134,6 +134,13 @@ struct UrdfModel btHashMap m_joints; btArray m_rootLinks; + bool m_overrideFixedBase; + + UrdfModel() + :m_overrideFixedBase(false) + { + m_rootTransformInWorld.setIdentity(); + } }; From 4b2c0f6d8990185691c482e28cbda0c1dd6c59f3 Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Fri, 3 Jun 2016 19:03:56 -0700 Subject: [PATCH 04/53] make one of the cubes in two_cubes.sdf static (immovable) using the tag in SDF add an example using 'direct' fix the send-desired-state commands, to add flags for arguments set, using default values. Start exposing SDF loading in shared memory api, not fully implemented yet. --- data/two_cubes.sdf | 1 + examples/ExampleBrowser/ExampleEntries.cpp | 5 +- examples/SharedMemory/PhysicsClientC_API.cpp | 30 +++++++ examples/SharedMemory/PhysicsClientC_API.h | 2 + .../SharedMemory/PhysicsClientExample.cpp | 22 +++-- examples/SharedMemory/PhysicsClientExample.h | 2 +- examples/SharedMemory/PhysicsDirect.cpp | 86 ++++++++++++++++++- examples/SharedMemory/PhysicsDirect.h | 2 + .../PhysicsServerCommandProcessor.cpp | 57 ++++++++---- examples/SharedMemory/SharedMemoryCommands.h | 21 +++++ examples/SharedMemory/SharedMemoryPublic.h | 6 +- 11 files changed, 202 insertions(+), 32 deletions(-) diff --git a/data/two_cubes.sdf b/data/two_cubes.sdf index 24c0854bd..10dce545e 100644 --- a/data/two_cubes.sdf +++ b/data/two_cubes.sdf @@ -86,6 +86,7 @@ 0 + 1 0.512455 -1.58317 0.5 0 -0 0 diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 75e16b8f7..b6004aeef 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -242,7 +242,10 @@ static ExampleEntry gDefaultExamples[]= ExampleEntry(1,"Physics Server (Replay Log)", "Create a physics server that replay a command log from disk.", PhysicsServerCreateFunc,PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG), - ExampleEntry(1, "Physics Client", "Create a physics client that can communicate with a physics server over shared memory", PhysicsClientCreateFunc), + ExampleEntry(1, "Physics Client (Shared Mem)", "Create a physics client that can communicate with a physics server over shared memory.", PhysicsClientCreateFunc), + ExampleEntry(1, "Physics Client (Direct)", "Create a physics client that can communicate with a physics server directly in-process.", PhysicsClientCreateFunc,eCLIENTEXAMPLE_DIRECT), + + #ifdef ENABLE_LUA diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 31b8df6b9..914c9d091 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -5,6 +5,29 @@ #include "SharedMemoryCommands.h" +b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type = CMD_LOAD_SDF; + int len = strlen(sdfFileName); + if (lenm_sdfArguments.m_sdfFileName,sdfFileName); + } else + { + command->m_sdfArguments.m_sdfFileName[0] = 0; + } + command->m_updateFlags = SDF_ARGS_FILE_NAME; + + return (b3SharedMemoryCommandHandle) command; +} + + b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName) { @@ -146,6 +169,8 @@ int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); command->m_sendDesiredStateCommandArgument.m_desiredStateQ[qIndex] = value; + command->m_updateFlags |= SIM_DESIRED_STATE_HAS_Q; + return 0; } @@ -154,6 +179,7 @@ int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); command->m_sendDesiredStateCommandArgument.m_Kp[dofIndex] = value; + command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KP; return 0; } @@ -162,6 +188,8 @@ int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); command->m_sendDesiredStateCommandArgument.m_Kd[dofIndex] = value; + command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KD; + return 0; } @@ -170,6 +198,7 @@ int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); command->m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex] = value; + command->m_updateFlags |= SIM_DESIRED_STATE_HAS_QDOT; return 0; } @@ -179,6 +208,7 @@ int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value; + command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE; return 0; } diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index b202daa35..37008aceb 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -88,6 +88,8 @@ int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHand int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody); int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase); +b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName); + ///Set joint control variables such as desired position/angle, desired velocity, ///applied joint forces, dependent on the control mode (CONTROL_MODE_VELOCITY or CONTROL_MODE_TORQUE) b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode); diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index a57fde289..15c90748a 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -156,16 +156,12 @@ protected: { for (int i=0;i=0)) + { + m_canvas->destroyCanvas(m_canvasIndex); + } b3Printf("~PhysicsClientExample\n"); } @@ -563,10 +564,15 @@ void PhysicsClientExample::initPhysics() m_isOptionalServerConnected = m_physicsServer.connectSharedMemory( m_guiHelper); } - m_physicsClientHandle = b3ConnectSharedMemory(m_sharedMemoryKey); - //m_physicsClientHandle = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY); - //m_physicsClientHandle = b3ConnectPhysicsDirect(); - + if (m_options == eCLIENTEXAMPLE_DIRECT) + { + m_physicsClientHandle = b3ConnectPhysicsDirect(); + } else + { + m_physicsClientHandle = b3ConnectSharedMemory(m_sharedMemoryKey); + //m_physicsClientHandle = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY); + } + if (!b3CanSubmitCommand(m_physicsClientHandle)) { b3Warning("Cannot connect to physics client"); diff --git a/examples/SharedMemory/PhysicsClientExample.h b/examples/SharedMemory/PhysicsClientExample.h index b564d01be..a36a37e75 100644 --- a/examples/SharedMemory/PhysicsClientExample.h +++ b/examples/SharedMemory/PhysicsClientExample.h @@ -4,7 +4,7 @@ enum ClientExampleOptions { eCLIENTEXAMPLE_LOOPBACK=1, - eCLIENTEAXMPLE_DIRECT=2, + eCLIENTEXAMPLE_DIRECT=2, eCLIENTEXAMPLE_SERVER=3, }; diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 72b5eb65e..37baeaa83 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -33,6 +33,12 @@ struct PhysicsDirectInternalData char m_bulletStreamDataServerToClient[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE]; + int m_cachedCameraPixelsWidth; + int m_cachedCameraPixelsHeight; + btAlignedObjectArray m_cachedCameraPixelsRGBA; + btAlignedObjectArray m_cachedCameraDepthBuffer; + + PhysicsServerCommandProcessor* m_commandProcessor; PhysicsDirectInternalData() @@ -167,6 +173,73 @@ bool PhysicsDirect::processDebugLines(const struct SharedMemoryCommand& orgComma return m_data->m_hasStatus; } +bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand) +{ + SharedMemoryCommand command = orgCommand; + + const SharedMemoryStatus& serverCmd = m_data->m_serverStatus; + + do + { + + bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + m_data->m_hasStatus = hasStatus; + if (hasStatus) + { + btAssert(m_data->m_serverStatus.m_type == CMD_CAMERA_IMAGE_COMPLETED); + + if (m_data->m_verboseOutput) + { + b3Printf("Camera image OK\n"); + } + + int numBytesPerPixel = 4;//RGBA + int numTotalPixels = serverCmd.m_sendPixelDataArguments.m_startingPixelIndex+ + serverCmd.m_sendPixelDataArguments.m_numPixelsCopied+ + serverCmd.m_sendPixelDataArguments.m_numRemainingPixels; + + m_data->m_cachedCameraPixelsWidth = 0; + m_data->m_cachedCameraPixelsHeight = 0; + + int numPixels = serverCmd.m_sendPixelDataArguments.m_imageWidth*serverCmd.m_sendPixelDataArguments.m_imageHeight; + + m_data->m_cachedCameraPixelsRGBA.reserve(numPixels*numBytesPerPixel); + m_data->m_cachedCameraDepthBuffer.resize(numTotalPixels); + m_data->m_cachedCameraPixelsRGBA.resize(numTotalPixels*numBytesPerPixel); + + + unsigned char* rgbaPixelsReceived = + (unsigned char*)&m_data->m_bulletStreamDataServerToClient[0]; + printf("pixel = %d\n", rgbaPixelsReceived[0]); + + for (int i=0;im_cachedCameraPixelsRGBA[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex*numBytesPerPixel] + = rgbaPixelsReceived[i]; + } + + if (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0) + { + + + // continue requesting remaining pixels + command.m_type = CMD_REQUEST_CAMERA_IMAGE_DATA; + command.m_requestPixelDataArguments.m_startPixelIndex = + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex + + serverCmd.m_sendPixelDataArguments.m_numPixelsCopied; + } else + { + m_data->m_cachedCameraPixelsWidth = serverCmd.m_sendPixelDataArguments.m_imageWidth; + m_data->m_cachedCameraPixelsHeight = serverCmd.m_sendPixelDataArguments.m_imageHeight; + } + } + } while (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0); + + return m_data->m_hasStatus; + + +} + bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& command) { if (command.m_type==CMD_REQUEST_DEBUG_LINES) @@ -174,6 +247,11 @@ bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& comman return processDebugLines(command); } + if (command.m_type==CMD_REQUEST_CAMERA_IMAGE_DATA) + { + return processCamera(command); + } + bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); m_data->m_hasStatus = hasStatus; if (hasStatus) @@ -333,9 +411,9 @@ void PhysicsDirect::getCachedCameraImage(b3CameraImageData* cameraData) { if (cameraData) { - cameraData->m_pixelHeight = 0; - cameraData->m_pixelWidth = 0; - cameraData->m_depthValues = 0; - cameraData->m_rgbColorData = 0; + cameraData->m_pixelWidth = m_data->m_cachedCameraPixelsWidth; + cameraData->m_pixelHeight = m_data->m_cachedCameraPixelsHeight; + cameraData->m_depthValues = m_data->m_cachedCameraDepthBuffer.size() ? &m_data->m_cachedCameraDepthBuffer[0] : 0; + cameraData->m_rgbColorData = m_data->m_cachedCameraPixelsRGBA.size() ? &m_data->m_cachedCameraPixelsRGBA[0] : 0; } } diff --git a/examples/SharedMemory/PhysicsDirect.h b/examples/SharedMemory/PhysicsDirect.h index fb1aeefa6..9dea925d3 100644 --- a/examples/SharedMemory/PhysicsDirect.h +++ b/examples/SharedMemory/PhysicsDirect.h @@ -19,6 +19,8 @@ protected: bool processDebugLines(const struct SharedMemoryCommand& orgCommand); + bool processCamera(const struct SharedMemoryCommand& orgCommand); + public: PhysicsDirect(); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 1fc4efdcc..b4317bcc6 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1192,12 +1192,18 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm mb->clearForcesAndTorques(); int torqueIndex = 0; - btVector3 f(clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[0], - clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[1], - clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[2]); - btVector3 t(clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[3], - clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[4], - clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[5]); + btVector3 f(0,0,0); + btVector3 t(0,0,0); + + if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) + { + f = btVector3 (clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[0], + clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[1], + clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[2]); + t = btVector3 (clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[3], + clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[4], + clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[5]); + } torqueIndex+=6; mb->addBaseForce(f); mb->addBaseTorque(t); @@ -1206,7 +1212,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm for (int dof=0;dofgetLink(link).m_dofCount;dof++) { - double torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[torqueIndex]; + double torque = 0.f; + if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) + torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[torqueIndex]; mb->addJointTorqueMultiDof(link,dof,torque); torqueIndex++; } @@ -1233,10 +1241,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm if (motorPtr) { btMultiBodyJointMotor* motor = *motorPtr; - btScalar desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex]; + btScalar desiredVelocity = 0.f; + if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_QDOT)!=0) + desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex]; motor->setVelocityTarget(desiredVelocity); - btScalar maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]*m_data->m_physicsDeltaTime; + btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime; + if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) + maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]*m_data->m_physicsDeltaTime; + motor->setMaxAppliedImpulse(maxImp); numMotors++; @@ -1247,6 +1260,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } break; } + case CONTROL_MODE_POSITION_VELOCITY_PD: { if (m_data->m_verboseOutput) @@ -1271,11 +1285,19 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { btMultiBodyJointMotor* motor = *motorPtr; - btScalar desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex]; - btScalar desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex]; - - btScalar kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex]; - btScalar kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex]; + btScalar desiredVelocity = 0.f; + if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_QDOT)!=0) + desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex]; + btScalar desiredPosition = 0.f; + if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_Q)!=0) + desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex]; + + btScalar kp = 0.f; + if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_KP)!=0) + kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex]; + btScalar kd = 0.f; + if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_KD)!=0) + kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex]; int dof1 = 0; btScalar currentPosition = mb->getJointPosMultiDof(link)[dof1]; @@ -1288,9 +1310,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm motor->setVelocityTarget(desiredVelocity); - btScalar maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex]*m_data->m_physicsDeltaTime; + btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime; + + if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) + maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex]*m_data->m_physicsDeltaTime; - motor->setMaxAppliedImpulse(1000);//maxImp); + motor->setMaxAppliedImpulse(maxImp); numMotors++; } diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 42436d2b0..8328b485a 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -30,6 +30,7 @@ #define MAX_DEGREE_OF_FREEDOM 64 #define MAX_NUM_SENSORS 256 #define MAX_URDF_FILENAME_LENGTH 1024 +#define MAX_SDF_FILENAME_LENGTH 1024 #define MAX_FILENAME_LENGTH MAX_URDF_FILENAME_LENGTH #define MAX_NUM_LINKS MAX_DEGREE_OF_FREEDOM @@ -54,6 +55,16 @@ TmpFloat3 CreateTmpFloat3(float x, float y, float z) return tmp; } +enum EnumSdfArgsUpdateFlags +{ + SDF_ARGS_FILE_NAME=1, +}; + +struct SdfArgs +{ + char m_sdfFileName[MAX_URDF_FILENAME_LENGTH]; +}; + enum EnumUrdfArgsUpdateFlags { URDF_ARGS_FILE_NAME=1, @@ -179,6 +190,15 @@ struct SendDesiredStateArgs }; +enum EnumSimDesiredStateUpdateFlags +{ + SIM_DESIRED_STATE_HAS_Q=1, + SIM_DESIRED_STATE_HAS_QDOT=2, + SIM_DESIRED_STATE_HAS_KD=4, + SIM_DESIRED_STATE_HAS_KP=8, + SIM_DESIRED_STATE_HAS_MAX_FORCE=16, +}; + enum EnumSimParamUpdateFlags { @@ -287,6 +307,7 @@ struct SharedMemoryCommand union { struct UrdfArgs m_urdfArguments; + struct SdfArgs m_sdfArguments; struct InitPoseArgs m_initPoseArgs; struct SendPhysicsSimulationParameters m_physSimParamArgs; struct BulletDataStreamArgs m_dataStreamArguments; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index f7d04f724..2f676e057 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -5,7 +5,8 @@ enum EnumSharedMemoryClientCommand { - CMD_LOAD_URDF, + CMD_LOAD_SDF, + CMD_LOAD_URDF, CMD_SEND_BULLET_DATA_STREAM, CMD_CREATE_BOX_COLLISION_SHAPE, // CMD_DELETE_BOX_COLLISION_SHAPE, @@ -35,7 +36,8 @@ enum EnumSharedMemoryServerStatus CMD_CLIENT_COMMAND_COMPLETED, //the server will skip unknown command and report a status 'CMD_UNKNOWN_COMMAND_FLUSHED' CMD_UNKNOWN_COMMAND_FLUSHED, - + CMD_SDF_LOADING_COMPLETED, + CMD_SDF_LOADING_FAILED, CMD_URDF_LOADING_COMPLETED, CMD_URDF_LOADING_FAILED, CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED, From 612e46614ad1e9b849916a085e69308f5b813e83 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 4 Jun 2016 13:16:06 -0700 Subject: [PATCH 05/53] fix (unused) SSE operator btMatrix3x3 ==, thanks to yyzone for the report/fix. fixes 552 --- src/LinearMath/btMatrix3x3.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/LinearMath/btMatrix3x3.h b/src/LinearMath/btMatrix3x3.h index 41dea6948..963c5db97 100644 --- a/src/LinearMath/btMatrix3x3.h +++ b/src/LinearMath/btMatrix3x3.h @@ -1329,7 +1329,9 @@ SIMD_FORCE_INLINE bool operator==(const btMatrix3x3& m1, const btMatrix3x3& m2) c0 = _mm_and_ps(c0, c1); c0 = _mm_and_ps(c0, c2); - return (0x7 == _mm_movemask_ps((__m128)c0)); + int m = _mm_movemask_ps((__m128)c0); + return (0x7 == (m & 0x7)); + #else return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] && From 8076d5235c52333c655f42adbefa9e054e111bf7 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 4 Jun 2016 21:51:21 -0700 Subject: [PATCH 06/53] fix window resize/width bookkeeping bug in MacOpenGLWindow.mm --- examples/OpenGLWindow/MacOpenGLWindow.mm | 28 ++++++++---------------- examples/SimpleOpenGL3/main.cpp | 6 ++--- 2 files changed, 12 insertions(+), 22 deletions(-) diff --git a/examples/OpenGLWindow/MacOpenGLWindow.mm b/examples/OpenGLWindow/MacOpenGLWindow.mm index 301608cca..d3b3ba31e 100644 --- a/examples/OpenGLWindow/MacOpenGLWindow.mm +++ b/examples/OpenGLWindow/MacOpenGLWindow.mm @@ -102,11 +102,11 @@ float loop; } -(void)drawRect:(NSRect)rect { + if (([self frame].size.width != m_lastWidth) || ([self frame].size.height != m_lastHeight)) { m_lastWidth = [self frame].size.width; m_lastHeight = [self frame].size.height; - // Only needed on resize: [m_context clearDrawable]; @@ -114,7 +114,6 @@ float loop; float width = [self frame].size.width; float height = [self frame].size.height; - // Get view dimensions in pixels // glViewport(0,0,10,10); @@ -209,16 +208,12 @@ struct MacOpenGLWindowInternalData m_myview = 0; m_pool = 0; m_window = 0; - m_width = -1; - m_height = -1; m_exitRequested = false; } NSApplication* m_myApp; TestView* m_myview; NSAutoreleasePool* m_pool; NSWindow* m_window; - int m_width; - int m_height; bool m_exitRequested; }; @@ -294,8 +289,6 @@ void MacOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci) if (m_internalData) closeWindow(); - int width = ci.m_width; - int height = ci.m_height; const char* windowTitle = ci.m_title; @@ -303,9 +296,7 @@ void MacOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci) NSAutoreleasePool *pool = [[NSAutoreleasePool alloc] init]; m_internalData = new MacOpenGLWindowInternalData; - m_internalData->m_width = width; - m_internalData->m_height = height; - + m_internalData->m_pool = [NSAutoreleasePool new]; m_internalData->m_myApp = [NSApplication sharedApplication]; //myApp = [MyApp sharedApplication]; @@ -373,7 +364,7 @@ void MacOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci) [newItem release]; */ - NSRect frame = NSMakeRect(0., 0., width, height); + NSRect frame = NSMakeRect(0., 0., ci.m_width, ci.m_height); m_internalData->m_window = [NSWindow alloc]; [m_internalData->m_window initWithContentRect:frame @@ -423,9 +414,7 @@ void MacOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci) [m_internalData->m_window makeKeyAndOrderFront: nil]; [m_internalData->m_myview MakeCurrent]; - m_internalData->m_width = m_internalData->m_myview.GetWindowWidth; - m_internalData->m_height = m_internalData->m_myview.GetWindowHeight; - + [NSApp activateIgnoringOtherApps:YES]; @@ -1035,13 +1024,13 @@ void MacOpenGLWindow::startRendering() float aspect; //b3Vector3 extents; - if (m_internalData->m_width > m_internalData->m_height) + if (getWidth() > getHeight()) { - aspect = (float)m_internalData->m_width / (float)m_internalData->m_height; + aspect = (float)getWidth() / (float)getHeight(); //extents.setValue(aspect * 1.0f, 1.0f,0); } else { - aspect = (float)m_internalData->m_height / (float)m_internalData->m_width; + aspect = (float)getHeight() / (float)getWidth(); //extents.setValue(1.0f, aspect*1.f,0); } @@ -1136,6 +1125,7 @@ int MacOpenGLWindow::getWidth() const { if (m_internalData && m_internalData->m_myview && m_internalData->m_myview.GetWindowWidth) return m_internalData->m_myview.GetWindowWidth; + return 0; } @@ -1152,7 +1142,7 @@ void MacOpenGLWindow::setResizeCallback(b3ResizeCallback resizeCallback) [m_internalData->m_myview setResizeCallback:resizeCallback]; if (resizeCallback) { - (resizeCallback)(m_internalData->m_width,m_internalData->m_height); + (resizeCallback)(getWidth(), getHeight()); } } diff --git a/examples/SimpleOpenGL3/main.cpp b/examples/SimpleOpenGL3/main.cpp index 015db129e..726abf693 100644 --- a/examples/SimpleOpenGL3/main.cpp +++ b/examples/SimpleOpenGL3/main.cpp @@ -14,8 +14,8 @@ static b3MouseButtonCallback sOldMouseButtonCB = 0; static b3KeyboardCallback sOldKeyboardCB = 0; //static b3RenderCallback sOldRenderCB = 0; -float gWidth = 0 ; -float gHeight = 0; +float gWidth = 1024; +float gHeight = 768; void MyWheelCallback(float deltax, float deltay) { @@ -61,7 +61,7 @@ int main(int argc, char* argv[]) b3CommandLineArgs myArgs(argc,argv); - SimpleOpenGL3App* app = new SimpleOpenGL3App("SimpleOpenGL3App",1024,768,true); + SimpleOpenGL3App* app = new SimpleOpenGL3App("SimpleOpenGL3App",gWidth,gHeight,true); app->m_instancingRenderer->getActiveCamera()->setCameraDistance(13); app->m_instancingRenderer->getActiveCamera()->setCameraPitch(0); From 1c7f87aff100b472c2ecc1cb383d9d12556ecd60 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 6 Jun 2016 18:54:05 -0700 Subject: [PATCH 07/53] implement first draft of pybullet.renderImage for synthetic camera remove a few debug printf from tinyrenderer --- .../SharedMemory/PhysicsClientExample.cpp | 14 +- .../PhysicsClientSharedMemory.cpp | 4 +- .../PhysicsServerCommandProcessor.cpp | 2 +- .../TinyRendererVisualShapeConverter.cpp | 2 +- examples/TinyRenderer/TinyRenderer.cpp | 2 +- examples/pybullet/CMakeLists.txt | 10 +- examples/pybullet/pybullet.c | 161 +++++++++++++++++- 7 files changed, 180 insertions(+), 15 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 15c90748a..30da039b3 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -612,9 +612,9 @@ void PhysicsClientExample::stepSimulation(float deltaTime) } if (statusType ==CMD_CAMERA_IMAGE_COMPLETED) { - static int counter=0; - char msg[1024]; - sprintf(msg,"Camera image %d OK\n",counter++); + // static int counter=0; + // char msg[1024]; + // sprintf(msg,"Camera image %d OK\n",counter++); b3CameraImageData imageData; b3GetCameraImageData(m_physicsClientHandle,&imageData); if (m_canvas && m_canvasIndex >=0) @@ -642,11 +642,11 @@ void PhysicsClientExample::stepSimulation(float deltaTime) m_canvas->refreshImageData(m_canvasIndex); } - b3Printf(msg); + // b3Printf(msg); } if (statusType == CMD_CAMERA_IMAGE_FAILED) { - b3Printf("Camera image FAILED\n"); + b3Warning("Camera image FAILED\n"); } @@ -716,8 +716,8 @@ void PhysicsClientExample::stepSimulation(float deltaTime) bool hasStatus = (status != 0); if (hasStatus) { - int statusType = b3GetStatusType(status); - b3Printf("Status after reset: %d",statusType); + //int statusType = b3GetStatusType(status); + //b3Printf("Status after reset: %d",statusType); } } } else diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index c0ecc0a19..82062b0c8 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -448,7 +448,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { unsigned char* rgbaPixelsReceived = (unsigned char*)&m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0]; - printf("pixel = %d\n", rgbaPixelsReceived[0]); + // printf("pixel = %d\n", rgbaPixelsReceived[0]); for (int i=0;im_swRenderInstances.size()); + // printf("num m_swRenderInstances = %d\n", m_data->m_swRenderInstances.size()); for (int i=0;im_swRenderInstances.size();i++) { TinyRendererObjectArray** visualArrayPtr = m_data->m_swRenderInstances.getAtIndex(i); diff --git a/examples/TinyRenderer/TinyRenderer.cpp b/examples/TinyRenderer/TinyRenderer.cpp index e7dfb8f05..6b9fdbb35 100644 --- a/examples/TinyRenderer/TinyRenderer.cpp +++ b/examples/TinyRenderer/TinyRenderer.cpp @@ -265,7 +265,7 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData) Vec3f localScaling(renderData.m_localScaling[0],renderData.m_localScaling[1],renderData.m_localScaling[2]); Shader shader(model, light_dir_local, modelViewMatrix, renderData.m_projectionMatrix,renderData.m_modelMatrix, localScaling, model->getColorRGBA()); - printf("Render %d triangles.\n",model->nfaces()); + //printf("Render %d triangles.\n",model->nfaces()); for (int i=0; infaces(); i++) { diff --git a/examples/pybullet/CMakeLists.txt b/examples/pybullet/CMakeLists.txt index 5fd6b106a..672187557 100644 --- a/examples/pybullet/CMakeLists.txt +++ b/examples/pybullet/CMakeLists.txt @@ -9,6 +9,15 @@ INCLUDE_DIRECTORIES( SET(pybullet_SRCS pybullet.c ../../examples/ExampleBrowser/InProcessExampleBrowser.cpp + ../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp + ../../examples/SharedMemory/TinyRendererVisualShapeConverter.h + ../../examples/OpenGLWindow/SimpleCamera.cpp + ../../examples/OpenGLWindow/SimpleCamera.h + ../../examples/TinyRenderer/geometry.cpp + ../../examples/TinyRenderer/model.cpp + ../../examples/TinyRenderer/tgaimage.cpp + ../../examples/TinyRenderer/our_gl.cpp + ../../examples/TinyRenderer/TinyRenderer.cpp ../../examples/SharedMemory/InProcessMemory.cpp ../../examples/SharedMemory/PhysicsClient.cpp ../../examples/SharedMemory/PhysicsClient.h @@ -48,7 +57,6 @@ SET(pybullet_SRCS ../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp ../../examples/Importers/ImportURDFDemo/UrdfParser.cpp ../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp - ../../examples/Importers/ImportURDFDemo/DefaultVisualShapeConverter.cpp ../../examples/MultiThreading/b3PosixThreadSupport.cpp ../../examples/MultiThreading/b3Win32ThreadSupport.cpp ../../examples/MultiThreading/b3ThreadSupportInterface.cpp diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 3286e5cf2..43ac64db9 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -134,7 +134,7 @@ pybullet_loadURDF(PyObject* self, PyObject* args) float startOrnY = 0; float startOrnZ = 0; float startOrnW = 1; - printf("size=%d\n", size); + //printf("size=%d\n", size); if (0==sm) { PyErr_SetString(SpamError, "Not connected to physics server."); @@ -164,7 +164,7 @@ pybullet_loadURDF(PyObject* self, PyObject* args) b3SharedMemoryStatusHandle statusHandle; int statusType; b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(sm, urdfFileName); - printf("urdf filename = %s\n", urdfFileName); + //printf("urdf filename = %s\n", urdfFileName); //setting the initial position, orientation and other arguments are optional b3LoadUrdfCommandSetStartPosition(command, startPosX,startPosY,startPosZ); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); @@ -359,6 +359,157 @@ pybullet_getNumJoints(PyObject* self, PyObject* args) } } +static PyObject* +pybullet_setJointPositions(PyObject* self, PyObject* args) +{ + + Py_INCREF(Py_None); + return Py_None; +} + + // const unsigned char* m_rgbColorData;//3*m_pixelWidth*m_pixelHeight bytes + // const float* m_depthValues;//m_pixelWidth*m_pixelHeight floats + +static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) +{ + + ///request an image from a simulated camera, using a software renderer. + struct b3CameraImageData imageData; + PyObject* objViewMat,* objProjMat; + + b3SharedMemoryCommandHandle command = b3InitRequestCameraImage(sm); + + if (PyArg_ParseTuple(args, "OO", &objViewMat, &objProjMat)) + { + PyObject* seq; + int i, len; + PyObject* item; + float viewMatrix[16]; + float projectionMatrix[16]; + int valid = 1; + { + seq = PySequence_Fast(objViewMat, "expected a sequence"); + len = PySequence_Size(objViewMat); + //printf("objViewMat size = %d\n", len); + if (len==16) + { + + if (PyList_Check(seq)) + { + for (i = 0; i < len; i++) + { + item = PyList_GET_ITEM(seq, i); + viewMatrix[i] = PyFloat_AsDouble(item); + float v = viewMatrix[i]; + //printf("view %d to %f\n", i,v); + + } + } + else + { + for (i = 0; i < len; i++) + { + item = PyTuple_GET_ITEM(seq,i); + viewMatrix[i] = PyFloat_AsDouble(item); + } + } + } else + { + valid = 0; + } + } + + { + seq = PySequence_Fast(objProjMat, "expected a sequence"); + len = PySequence_Size(objProjMat); + //printf("projMat len = %d\n", len); + if (len==16) + { + if (PyList_Check(seq)) + { + for (i = 0; i < len; i++) + { + item = PyList_GET_ITEM(seq, i); + projectionMatrix[i] = PyFloat_AsDouble(item); + } + } + else + { + for (i = 0; i < len; i++) + { + item = PyTuple_GET_ITEM(seq,i); + projectionMatrix[i] = PyFloat_AsDouble(item); + } + } + } else + { + valid = 0; + } + } + + + Py_DECREF(seq); + if (valid) + { + //printf("set b3RequestCameraImageSetCameraMatrices\n"); + b3RequestCameraImageSetCameraMatrices(command, viewMatrix, projectionMatrix); + } + } + + if (b3CanSubmitCommand(sm)) + { + b3SharedMemoryStatusHandle statusHandle; + int statusType; + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + if (statusType==CMD_CAMERA_IMAGE_COMPLETED) + { + PyObject *item2; + PyObject* pyResultList;//store 4 elements in this result: width, height, rgbData, depth + + b3GetCameraImageData(sm, &imageData); + //todo: error handling if image size is 0 + pyResultList = PyTuple_New(4); + PyTuple_SetItem(pyResultList, 0, PyInt_FromLong(imageData.m_pixelWidth)); + PyTuple_SetItem(pyResultList, 1, PyInt_FromLong(imageData.m_pixelHeight)); + + PyObject *pylistPos; + PyObject* pylistDep; + + //printf("image width = %d, height = %d\n", imageData.m_pixelWidth, imageData.m_pixelHeight); + { + + PyObject *item; + int bytesPerPixel = 3;//Red, Green, Blue, each 8 bit values + int num=bytesPerPixel*imageData.m_pixelWidth*imageData.m_pixelHeight; + pylistPos = PyTuple_New(num); + pylistDep = PyTuple_New(imageData.m_pixelWidth*imageData.m_pixelHeight); + + for (int i=0;i Date: Tue, 7 Jun 2016 12:57:46 -0700 Subject: [PATCH 08/53] add .stl file support in 'File/Open' of example browser. --- examples/ExampleBrowser/main.cpp | 3 ++ .../ImportSTLDemo/ImportSTLSetup.cpp | 31 +++++++++++++------ examples/OpenGLWindow/MacOpenGLWindow.mm | 2 +- 3 files changed, 25 insertions(+), 11 deletions(-) diff --git a/examples/ExampleBrowser/main.cpp b/examples/ExampleBrowser/main.cpp index 179f18036..b245cdecb 100644 --- a/examples/ExampleBrowser/main.cpp +++ b/examples/ExampleBrowser/main.cpp @@ -13,6 +13,8 @@ #include "../Importers/ImportSTLDemo/ImportSTLSetup.h" #include "../Importers/ImportURDFDemo/ImportURDFSetup.h" #include "../Importers/ImportSDFDemo/ImportSDFSetup.h" +#include "../Importers/ImportSTLDemo/ImportSTLSetup.h" + int main(int argc, char* argv[]) @@ -29,6 +31,7 @@ int main(int argc, char* argv[]) exampleBrowser->registerFileImporter(".urdf",ImportURDFCreateFunc); exampleBrowser->registerFileImporter(".sdf",ImportSDFCreateFunc); exampleBrowser->registerFileImporter(".obj",ImportObjCreateFunc); + exampleBrowser->registerFileImporter(".stl",ImportSTLCreateFunc); clock.reset(); if (init) diff --git a/examples/Importers/ImportSTLDemo/ImportSTLSetup.cpp b/examples/Importers/ImportSTLDemo/ImportSTLSetup.cpp index b7f25c3dc..c05138ef1 100644 --- a/examples/Importers/ImportSTLDemo/ImportSTLSetup.cpp +++ b/examples/Importers/ImportSTLDemo/ImportSTLSetup.cpp @@ -13,9 +13,12 @@ class ImportSTLSetup : public CommonRigidBodyBase { + + const char* m_fileName; + btVector3 m_scaling; public: - ImportSTLSetup(struct GUIHelperInterface* helper); + ImportSTLSetup(struct GUIHelperInterface* helper, const char* fileName); virtual ~ImportSTLSetup(); virtual void initPhysics(); @@ -31,10 +34,19 @@ public: }; -ImportSTLSetup::ImportSTLSetup(struct GUIHelperInterface* helper) -:CommonRigidBodyBase(helper) +ImportSTLSetup::ImportSTLSetup(struct GUIHelperInterface* helper, const char* fileName) +:CommonRigidBodyBase(helper), +m_scaling(btVector3(10,10,10)) { - + if (fileName) + { + m_fileName = fileName; + m_scaling = btVector3(0.01,0.01,0.01); + } else + { + m_fileName = "l_finger_tip.stl"; + + } } ImportSTLSetup::~ImportSTLSetup() @@ -51,17 +63,16 @@ void ImportSTLSetup::initPhysics() m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe); - const char* fileName = "l_finger_tip.stl"; + char relativeFileName[1024]; - if (!b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024)) + if (!b3ResourcePath::findResourcePath(m_fileName, relativeFileName, 1024)) { - b3Warning("Cannot find file %s\n", fileName); + b3Warning("Cannot find file %s\n", m_fileName); return; } btVector3 shift(0,0,0); - btVector3 scaling(10,10,10); // int index=10; { @@ -81,12 +92,12 @@ void ImportSTLSetup::initPhysics() - m_guiHelper->getRenderInterface()->registerGraphicsInstance(shapeId,position,orn,color,scaling); + m_guiHelper->getRenderInterface()->registerGraphicsInstance(shapeId,position,orn,color,m_scaling); } } class CommonExampleInterface* ImportSTLCreateFunc(struct CommonExampleOptions& options) { - return new ImportSTLSetup(options.m_guiHelper); + return new ImportSTLSetup(options.m_guiHelper, options.m_fileName); } diff --git a/examples/OpenGLWindow/MacOpenGLWindow.mm b/examples/OpenGLWindow/MacOpenGLWindow.mm index d3b3ba31e..f4f853889 100644 --- a/examples/OpenGLWindow/MacOpenGLWindow.mm +++ b/examples/OpenGLWindow/MacOpenGLWindow.mm @@ -1071,7 +1071,7 @@ int MacOpenGLWindow::fileOpenDialog(char* filename, int maxNameLength) NSOpenGLContext *foo = [NSOpenGLContext currentContext]; // get the url of a .txt file NSOpenPanel * zOpenPanel = [NSOpenPanel openPanel]; - NSArray * zAryOfExtensions = [NSArray arrayWithObjects:@"urdf",@"bullet",@"obj",@"sdf",nil]; + NSArray * zAryOfExtensions = [NSArray arrayWithObjects:@"urdf",@"bullet",@"obj",@"sdf",@"stl",nil]; [zOpenPanel setAllowedFileTypes:zAryOfExtensions]; NSInteger zIntResult = [zOpenPanel runModal]; From d2e50d045b6c1d236115cf9b5c13558991901df0 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 7 Jun 2016 16:11:58 -0700 Subject: [PATCH 09/53] fix issues related to camera width/height add width,height as arguments to pybullet.renderImage(x,y,[viewMat4x4],[projMat4x4]) --- .../RenderingExamples/TinyRendererSetup.cpp | 4 +- examples/SharedMemory/PhysicsClientC_API.cpp | 10 + examples/SharedMemory/PhysicsClientC_API.h | 1 + .../SharedMemory/PhysicsClientExample.cpp | 23 +- .../PhysicsClientSharedMemory.cpp | 7 + examples/SharedMemory/PhysicsDirect.cpp | 10 +- .../PhysicsServerCommandProcessor.cpp | 9 + examples/SharedMemory/SharedMemoryCommands.h | 4 + .../TinyRendererVisualShapeConverter.cpp | 19 +- .../TinyRendererVisualShapeConverter.h | 1 + examples/TinyRenderer/TinyRenderer.cpp | 22 +- examples/TinyRenderer/TinyRenderer.h | 7 +- examples/pybullet/pybullet.c | 227 ++++++++++-------- 13 files changed, 203 insertions(+), 141 deletions(-) diff --git a/examples/RenderingExamples/TinyRendererSetup.cpp b/examples/RenderingExamples/TinyRendererSetup.cpp index 11258779d..8df1a76ad 100644 --- a/examples/RenderingExamples/TinyRendererSetup.cpp +++ b/examples/RenderingExamples/TinyRendererSetup.cpp @@ -183,7 +183,7 @@ TinyRendererSetup::TinyRendererSetup(struct GUIHelperInterface* gui) m_guiHelper->getRenderInterface()->writeTransforms(); m_internalData->m_shapePtr.push_back(0); - TinyRenderObjectData* ob = new TinyRenderObjectData(m_internalData->m_width,m_internalData->m_height, + TinyRenderObjectData* ob = new TinyRenderObjectData( m_internalData->m_rgbColorBuffer, m_internalData->m_depthBuffer); //ob->loadModel("cube.obj"); @@ -363,7 +363,7 @@ void TinyRendererSetup::stepSimulation(float deltaTime) } } - TinyRenderer::renderObject(*m_internalData->m_renderObjects[o]); + TinyRenderer::renderObject(*m_internalData->m_renderObjects[o], m_internalData->m_width,m_internalData->m_height); } //m_app->drawText("hello",500,500); render->activateTexture(m_internalData->m_textureHandle); diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 914c9d091..c6468cb89 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -735,6 +735,16 @@ void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle commandHa command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES; } +void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle commandHandle, int width, int height ) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA); + command->m_requestPixelDataArguments.m_pixelWidth = width; + command->m_requestPixelDataArguments.m_pixelHeight = height; + command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT; +} + void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData) { PhysicsClient* cl = (PhysicsClient* ) physClient; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 37008aceb..b67f078ae 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -68,6 +68,7 @@ void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* l ///request an image from a simulated camera, using a software renderer. b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient); void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle command, float viewMatrix[16], float projectionMatrix[16]); +void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle command, int width, int height ); void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData); diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 30da039b3..e7e02ba51 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -21,8 +21,8 @@ struct MyMotorInfo2 int m_qIndex; }; -int camVisualizerWidth = 640;//1024/3; -int camVisualizerHeight = 480;//768/3; +int camVisualizerWidth = 320;//1024/3; +int camVisualizerHeight = 240;//768/3; #define MAX_NUM_MOTORS 128 @@ -77,10 +77,10 @@ protected: virtual void resetCamera() { - float dist = 5; + float dist = 1.1; float pitch = 50; float yaw = 35; - float targetPos[3]={0,0,0};//-3,2.8,-2.5}; + float targetPos[3]={0,0,0.5};//-3,2.8,-2.5}; m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]); } @@ -248,7 +248,8 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) this->m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraProjectionMatrix(projectionMatrix); this->m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraViewMatrix(viewMatrix); b3RequestCameraImageSetCameraMatrices(commandHandle, viewMatrix,projectionMatrix); - b3SubmitClientCommand(m_physicsClientHandle, commandHandle); + b3RequestCameraImageSetPixelResolution(commandHandle, camVisualizerWidth,camVisualizerHeight); + b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_CREATE_BOX_COLLISION_SHAPE: @@ -619,18 +620,18 @@ void PhysicsClientExample::stepSimulation(float deltaTime) b3GetCameraImageData(m_physicsClientHandle,&imageData); if (m_canvas && m_canvasIndex >=0) { - for (int i=0;isetPixel(m_canvasIndex,xIndex,camVisualizerHeight-1-yIndex, + int pixelIndex = (xIndex+yIndex*imageData.m_pixelWidth)*bytesPerPixel; + m_canvas->setPixel(m_canvasIndex,i,camVisualizerHeight-1-j, imageData.m_rgbColorData[pixelIndex], imageData.m_rgbColorData[pixelIndex+1], diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 82062b0c8..728358b93 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -450,6 +450,13 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { (unsigned char*)&m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0]; // printf("pixel = %d\n", rgbaPixelsReceived[0]); + float* depthBuffer = (float*)&(m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*4]); + + for (int i=0;im_cachedCameraDepthBuffer[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = depthBuffer[i]; + } + for (int i=0;im_cachedCameraPixelsRGBA[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex*numBytesPerPixel] diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 37baeaa83..478e31031 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -210,8 +210,15 @@ bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand) unsigned char* rgbaPixelsReceived = (unsigned char*)&m_data->m_bulletStreamDataServerToClient[0]; - printf("pixel = %d\n", rgbaPixelsReceived[0]); + + float* depthBuffer = (float*)&(m_data->m_bulletStreamDataServerToClient[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*4]); + + // printf("pixel = %d\n", rgbaPixelsReceived[0]); + for (int i=0;im_cachedCameraDepthBuffer[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = depthBuffer[i]; + } for (int i=0;im_cachedCameraPixelsRGBA[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex*numBytesPerPixel] @@ -227,6 +234,7 @@ bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand) command.m_requestPixelDataArguments.m_startPixelIndex = serverCmd.m_sendPixelDataArguments.m_startingPixelIndex + serverCmd.m_sendPixelDataArguments.m_numPixelsCopied; + } else { m_data->m_cachedCameraPixelsWidth = serverCmd.m_sendPixelDataArguments.m_imageWidth; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 2f662aa4a..89490caa6 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -972,6 +972,14 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm int width, height; int numPixelsCopied = 0; + if ( + (clientCmd.m_requestPixelDataArguments.m_startPixelIndex==0) && + (clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT)!=0) + { + m_data->m_visualConverter.setWidthAndHeight(clientCmd.m_requestPixelDataArguments.m_pixelWidth, + clientCmd.m_requestPixelDataArguments.m_pixelHeight); + } + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_USE_HARDWARE_OPENGL)!=0) { m_data->m_guiHelper->copyCameraImageData(0,0,0,0,0,&width,&height,0); @@ -1005,6 +1013,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { // printf("-------------------------------\nRendering\n"); + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES)!=0) { m_data->m_visualConverter.render( diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 8328b485a..fa67d7673 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -129,12 +129,16 @@ struct RequestPixelDataArgs float m_viewMatrix[16]; float m_projectionMatrix[16]; int m_startPixelIndex; + int m_pixelWidth; + int m_pixelHeight; }; enum EnumRequestPixelDataUpdateFlags { REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES=1, REQUEST_PIXEL_ARGS_USE_HARDWARE_OPENGL=2, + REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT=4, + }; diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index fed633c1b..7c3507b25 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -462,7 +462,7 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const if (vertices.size() && indices.size()) { - TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_swWidth,m_data->m_swHeight,m_data->m_rgbColorBuffer,m_data->m_depthBuffer); + TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer,m_data->m_depthBuffer); tinyObj->registerMeshShape(&vertices[0].xyzw[0],vertices.size(),&indices[0],indices.size(),rgbaColor); visuals->m_renderObjects.push_back(tinyObj); } @@ -576,7 +576,7 @@ void TinyRendererVisualShapeConverter::render(const float viewMat[16], const flo renderObj->m_lightDirWorld = lightDirWorld; } } - TinyRenderer::renderObject(*renderObj); + TinyRenderer::renderObject(*renderObj,m_data->m_swWidth,m_data->m_swHeight); } } //printf("write tga \n"); @@ -590,6 +590,17 @@ void TinyRendererVisualShapeConverter::getWidthAndHeight(int& width, int& height height = m_data->m_swHeight; } + +void TinyRendererVisualShapeConverter::setWidthAndHeight(int width, int height) +{ + m_data->m_swWidth = width; + m_data->m_swHeight = height; + + m_data->m_depthBuffer.resize(m_data->m_swWidth*m_data->m_swHeight); + m_data->m_rgbColorBuffer = TGAImage(width, height, TGAImage::RGB); + +} + void TinyRendererVisualShapeConverter::copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied) { int w = m_data->m_rgbColorBuffer.get_width(); @@ -612,6 +623,10 @@ void TinyRendererVisualShapeConverter::copyCameraImageData(unsigned char* pixels { for (int i=0;im_depthBuffer[i+startPixelIndex]; + } if (pixelsRGBA) { pixelsRGBA[i*numBytesPerPixel] = m_data->m_rgbColorBuffer.buffer()[(i+startPixelIndex)*3+0]; diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.h b/examples/SharedMemory/TinyRendererVisualShapeConverter.h index 3bce1add7..6baff2491 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.h +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.h @@ -24,6 +24,7 @@ struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter void resetAll(); void getWidthAndHeight(int& width, int& height); + void setWidthAndHeight(int width, int height); void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied); diff --git a/examples/TinyRenderer/TinyRenderer.cpp b/examples/TinyRenderer/TinyRenderer.cpp index 6b9fdbb35..e13f97077 100644 --- a/examples/TinyRenderer/TinyRenderer.cpp +++ b/examples/TinyRenderer/TinyRenderer.cpp @@ -88,10 +88,8 @@ struct Shader : public IShader { }; -TinyRenderObjectData::TinyRenderObjectData(int width, int height,TGAImage& rgbColorBuffer,b3AlignedObjectArray&depthBuffer) -:m_width(width), -m_height(height), -m_rgbColorBuffer(rgbColorBuffer), +TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray&depthBuffer) +:m_rgbColorBuffer(rgbColorBuffer), m_depthBuffer(depthBuffer), m_model(0), m_userData(0), @@ -103,11 +101,6 @@ m_userIndex(-1) m_lightDirWorld.setValue(0,0,0); m_localScaling.setValue(1,1,1); m_modelMatrix = Matrix::identity(); - m_viewMatrix = lookat(eye, center, up); - //m_viewportMatrix = viewport(width/8, height/8, width*3/4, height*3/4); - //m_viewportMatrix = viewport(width/8, height/8, width*3/4, height*3/4); - m_viewportMatrix = viewport(0,0,width,height); - m_projectionMatrix = projection(-1.f/(eye-center).norm()); } @@ -238,7 +231,7 @@ TinyRenderObjectData::~TinyRenderObjectData() delete m_model; } -void TinyRenderer::renderObject(TinyRenderObjectData& renderData) +void TinyRenderer::renderObject(TinyRenderObjectData& renderData, int width, int height) { Vec3f light_dir_local = Vec3f(renderData.m_lightDirWorld[0],renderData.m_lightDirWorld[1],renderData.m_lightDirWorld[2]); Model* model = renderData.m_model; @@ -247,13 +240,8 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData) - //renderData.m_viewMatrix = lookat(eye, center, up); - int width = renderData.m_width; - int height = renderData.m_height; - //renderData.m_viewportMatrix = viewport(width/8, height/8, width*3/4, height*3/4); - renderData.m_viewportMatrix = viewport(0,0,renderData.m_width,renderData.m_height); - //renderData.m_projectionMatrix = projection(-1.f/(eye-center).norm()); - + renderData.m_viewportMatrix = viewport(0,0,width, height); + b3AlignedObjectArray& zbuffer = renderData.m_depthBuffer; TGAImage& frame = renderData.m_rgbColorBuffer; diff --git a/examples/TinyRenderer/TinyRenderer.h b/examples/TinyRenderer/TinyRenderer.h index 0cb2ffca9..cfe857eca 100644 --- a/examples/TinyRenderer/TinyRenderer.h +++ b/examples/TinyRenderer/TinyRenderer.h @@ -25,12 +25,11 @@ struct TinyRenderObjectData //class IShader* m_shader; todo(erwincoumans) expose the shader, for now we use a default shader //Output - int m_width; - int m_height; + TGAImage& m_rgbColorBuffer; b3AlignedObjectArray& m_depthBuffer; - TinyRenderObjectData(int width, int height,TGAImage& rgbColorBuffer,b3AlignedObjectArray& depthBuffer); + TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray& depthBuffer); virtual ~TinyRenderObjectData(); void loadModel(const char* fileName); @@ -48,7 +47,7 @@ struct TinyRenderObjectData class TinyRenderer { public: - static void renderObject(TinyRenderObjectData& renderData); + static void renderObject(TinyRenderObjectData& renderData, int width, int height); }; #endif // TINY_RENDERER_Hbla diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 43ac64db9..0bac58313 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -362,6 +362,11 @@ pybullet_getNumJoints(PyObject* self, PyObject* args) static PyObject* pybullet_setJointPositions(PyObject* self, PyObject* args) { + if (0==sm) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } Py_INCREF(Py_None); return Py_None; @@ -372,17 +377,22 @@ pybullet_setJointPositions(PyObject* self, PyObject* args) static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) { - + if (0==sm) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + ///request an image from a simulated camera, using a software renderer. struct b3CameraImageData imageData; PyObject* objViewMat,* objProjMat; + int width, height; - b3SharedMemoryCommandHandle command = b3InitRequestCameraImage(sm); - - if (PyArg_ParseTuple(args, "OO", &objViewMat, &objProjMat)) + if (PyArg_ParseTuple(args, "iiOO", &width, &height, &objViewMat, &objProjMat)) { - PyObject* seq; - int i, len; + + PyObject* seq; + int i, len; PyObject* item; float viewMatrix[16]; float projectionMatrix[16]; @@ -391,124 +401,133 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) seq = PySequence_Fast(objViewMat, "expected a sequence"); len = PySequence_Size(objViewMat); //printf("objViewMat size = %d\n", len); - if (len==16) - { - - if (PyList_Check(seq)) - { - for (i = 0; i < len; i++) + if (len==16) { + + if (PyList_Check(seq)) + { + for (i = 0; i < len; i++) + { item = PyList_GET_ITEM(seq, i); - viewMatrix[i] = PyFloat_AsDouble(item); - float v = viewMatrix[i]; + viewMatrix[i] = PyFloat_AsDouble(item); + float v = viewMatrix[i]; //printf("view %d to %f\n", i,v); + } + } + else + { + for (i = 0; i < len; i++) + { + item = PyTuple_GET_ITEM(seq,i); + viewMatrix[i] = PyFloat_AsDouble(item); + } + } + } else + { + valid = 0; } } - else - { - for (i = 0; i < len; i++) - { - item = PyTuple_GET_ITEM(seq,i); - viewMatrix[i] = PyFloat_AsDouble(item); - } - } - } else - { - valid = 0; - } - } + { - seq = PySequence_Fast(objProjMat, "expected a sequence"); - len = PySequence_Size(objProjMat); - //printf("projMat len = %d\n", len); - if (len==16) - { - if (PyList_Check(seq)) - { - for (i = 0; i < len; i++) - { - item = PyList_GET_ITEM(seq, i); - projectionMatrix[i] = PyFloat_AsDouble(item); - } - } - else - { - for (i = 0; i < len; i++) - { - item = PyTuple_GET_ITEM(seq,i); - projectionMatrix[i] = PyFloat_AsDouble(item); - } - } - } else - { - valid = 0; - } - } - + seq = PySequence_Fast(objProjMat, "expected a sequence"); + len = PySequence_Size(objProjMat); + //printf("projMat len = %d\n", len); + if (len==16) + { + if (PyList_Check(seq)) + { + for (i = 0; i < len; i++) + { + item = PyList_GET_ITEM(seq, i); + projectionMatrix[i] = PyFloat_AsDouble(item); + } + } + else + { + for (i = 0; i < len; i++) + { + item = PyTuple_GET_ITEM(seq,i); + projectionMatrix[i] = PyFloat_AsDouble(item); + } + } + } else + { + valid = 0; + } + } Py_DECREF(seq); if (valid) { + b3SharedMemoryCommandHandle command; + + command = b3InitRequestCameraImage(sm); + //printf("set b3RequestCameraImageSetCameraMatrices\n"); b3RequestCameraImageSetCameraMatrices(command, viewMatrix, projectionMatrix); - } - } - - if (b3CanSubmitCommand(sm)) - { - b3SharedMemoryStatusHandle statusHandle; - int statusType; - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - statusType = b3GetStatusType(statusHandle); - if (statusType==CMD_CAMERA_IMAGE_COMPLETED) - { - PyObject *item2; - PyObject* pyResultList;//store 4 elements in this result: width, height, rgbData, depth - - b3GetCameraImageData(sm, &imageData); - //todo: error handling if image size is 0 - pyResultList = PyTuple_New(4); - PyTuple_SetItem(pyResultList, 0, PyInt_FromLong(imageData.m_pixelWidth)); - PyTuple_SetItem(pyResultList, 1, PyInt_FromLong(imageData.m_pixelHeight)); - - PyObject *pylistPos; - PyObject* pylistDep; - - //printf("image width = %d, height = %d\n", imageData.m_pixelWidth, imageData.m_pixelHeight); - { - - PyObject *item; - int bytesPerPixel = 3;//Red, Green, Blue, each 8 bit values - int num=bytesPerPixel*imageData.m_pixelWidth*imageData.m_pixelHeight; - pylistPos = PyTuple_New(num); - pylistDep = PyTuple_New(imageData.m_pixelWidth*imageData.m_pixelHeight); - - for (int i=0;i Date: Tue, 7 Jun 2016 17:02:47 -0700 Subject: [PATCH 10/53] don't pass width/height into renderObject --- examples/RenderingExamples/TinyRendererSetup.cpp | 2 +- examples/SharedMemory/TinyRendererVisualShapeConverter.cpp | 2 +- .../StandaloneMain/main_sw_tinyrenderer_single_example.cpp | 2 +- examples/StandaloneMain/main_tinyrenderer_single_example.cpp | 2 +- examples/TinyRenderer/TinyRenderer.cpp | 5 ++++- examples/TinyRenderer/TinyRenderer.h | 2 +- examples/TinyRenderer/main.cpp | 2 +- 7 files changed, 10 insertions(+), 7 deletions(-) diff --git a/examples/RenderingExamples/TinyRendererSetup.cpp b/examples/RenderingExamples/TinyRendererSetup.cpp index 8df1a76ad..fd3f80e41 100644 --- a/examples/RenderingExamples/TinyRendererSetup.cpp +++ b/examples/RenderingExamples/TinyRendererSetup.cpp @@ -363,7 +363,7 @@ void TinyRendererSetup::stepSimulation(float deltaTime) } } - TinyRenderer::renderObject(*m_internalData->m_renderObjects[o], m_internalData->m_width,m_internalData->m_height); + TinyRenderer::renderObject(*m_internalData->m_renderObjects[o]); } //m_app->drawText("hello",500,500); render->activateTexture(m_internalData->m_textureHandle); diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index 7c3507b25..3bb5e893e 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -576,7 +576,7 @@ void TinyRendererVisualShapeConverter::render(const float viewMat[16], const flo renderObj->m_lightDirWorld = lightDirWorld; } } - TinyRenderer::renderObject(*renderObj,m_data->m_swWidth,m_data->m_swHeight); + TinyRenderer::renderObject(*renderObj); } } //printf("write tga \n"); diff --git a/examples/StandaloneMain/main_sw_tinyrenderer_single_example.cpp b/examples/StandaloneMain/main_sw_tinyrenderer_single_example.cpp index 786e0eed1..1fa1b0ccd 100644 --- a/examples/StandaloneMain/main_sw_tinyrenderer_single_example.cpp +++ b/examples/StandaloneMain/main_sw_tinyrenderer_single_example.cpp @@ -110,7 +110,7 @@ public: int shapeIndex = OpenGLGuiHelper::registerGraphicsShape(vertices,numvertices,indices,numIndices); if (shapeIndex>=0) { - TinyRenderObjectData* swObj = new TinyRenderObjectData(m_swWidth,m_swHeight,m_rgbColorBuffer,m_depthBuffer); + TinyRenderObjectData* swObj = new TinyRenderObjectData(m_rgbColorBuffer,m_depthBuffer); float rgbaColor[4] = {1,1,1,1}; swObj->registerMeshShape(vertices,numvertices,indices,numIndices,rgbaColor); //swObj->createCube(1,1,1);//MeshShape(vertices,numvertices,indices,numIndices); diff --git a/examples/StandaloneMain/main_tinyrenderer_single_example.cpp b/examples/StandaloneMain/main_tinyrenderer_single_example.cpp index 157ab233c..565b771e7 100644 --- a/examples/StandaloneMain/main_tinyrenderer_single_example.cpp +++ b/examples/StandaloneMain/main_tinyrenderer_single_example.cpp @@ -253,7 +253,7 @@ struct TinyRendererGUIHelper : public GUIHelperInterface { int shapeIndex = m_swRenderObjects.size(); - TinyRenderObjectData* swObj = new TinyRenderObjectData(m_swWidth,m_swHeight,m_rgbColorBuffer,m_depthBuffer); + TinyRenderObjectData* swObj = new TinyRenderObjectData(m_rgbColorBuffer,m_depthBuffer); float rgbaColor[4] = {1,1,1,1}; swObj->registerMeshShape(vertices,numvertices,indices,numIndices,rgbaColor); //swObj->createCube(1,1,1);//MeshShape(vertices,numvertices,indices,numIndices); diff --git a/examples/TinyRenderer/TinyRenderer.cpp b/examples/TinyRenderer/TinyRenderer.cpp index e13f97077..aebf0440e 100644 --- a/examples/TinyRenderer/TinyRenderer.cpp +++ b/examples/TinyRenderer/TinyRenderer.cpp @@ -231,8 +231,11 @@ TinyRenderObjectData::~TinyRenderObjectData() delete m_model; } -void TinyRenderer::renderObject(TinyRenderObjectData& renderData, int width, int height) +void TinyRenderer::renderObject(TinyRenderObjectData& renderData) { + int width = renderData.m_rgbColorBuffer.get_width(); + int height = renderData.m_rgbColorBuffer.get_height(); + Vec3f light_dir_local = Vec3f(renderData.m_lightDirWorld[0],renderData.m_lightDirWorld[1],renderData.m_lightDirWorld[2]); Model* model = renderData.m_model; if (0==model) diff --git a/examples/TinyRenderer/TinyRenderer.h b/examples/TinyRenderer/TinyRenderer.h index cfe857eca..c6f4f9821 100644 --- a/examples/TinyRenderer/TinyRenderer.h +++ b/examples/TinyRenderer/TinyRenderer.h @@ -47,7 +47,7 @@ struct TinyRenderObjectData class TinyRenderer { public: - static void renderObject(TinyRenderObjectData& renderData, int width, int height); + static void renderObject(TinyRenderObjectData& renderData); }; #endif // TINY_RENDERER_Hbla diff --git a/examples/TinyRenderer/main.cpp b/examples/TinyRenderer/main.cpp index 9995b8f66..32f5e7edd 100644 --- a/examples/TinyRenderer/main.cpp +++ b/examples/TinyRenderer/main.cpp @@ -86,7 +86,7 @@ int main(int argc, char* argv[]) b3AlignedObjectArray depthBuffer; depthBuffer.resize(gWidth*gHeight); - TinyRenderObjectData renderData(textureWidth, textureHeight,rgbColorBuffer,depthBuffer);//, "african_head/african_head.obj");//floor.obj"); + TinyRenderObjectData renderData(rgbColorBuffer,depthBuffer);//, "african_head/african_head.obj");//floor.obj"); //renderData.loadModel("african_head/african_head.obj"); renderData.loadModel("floor.obj"); From 98d2f91f3cca1a0f9e77a3cde62a4a04aea2d942 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 9 Jun 2016 12:12:46 -0700 Subject: [PATCH 11/53] fix flipped tinyrenderer image --- examples/SharedMemory/PhysicsClientExample.cpp | 2 +- .../TinyRendererVisualShapeConverter.cpp | 4 +++- examples/pybullet/pybullet.c | 16 +++++++++------- 3 files changed, 13 insertions(+), 9 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index e7e02ba51..e7d797fa6 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -631,7 +631,7 @@ void PhysicsClientExample::stepSimulation(float deltaTime) int bytesPerPixel = 4; int pixelIndex = (xIndex+yIndex*imageData.m_pixelWidth)*bytesPerPixel; - m_canvas->setPixel(m_canvasIndex,i,camVisualizerHeight-1-j, + m_canvas->setPixel(m_canvasIndex,i,j, imageData.m_rgbColorData[pixelIndex], imageData.m_rgbColorData[pixelIndex+1], diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index 3bb5e893e..ebe11877a 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -580,7 +580,9 @@ void TinyRendererVisualShapeConverter::render(const float viewMat[16], const flo } } //printf("write tga \n"); - m_data->m_rgbColorBuffer.write_tga_file("camera.tga"); + //m_data->m_rgbColorBuffer.write_tga_file("camera.tga"); +// printf("flipped!\n"); + m_data->m_rgbColorBuffer.flip_vertically(); } diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 0bac58313..41625b8cf 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -459,15 +459,16 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) } Py_DECREF(seq); - if (valid) { b3SharedMemoryCommandHandle command; command = b3InitRequestCameraImage(sm); - + if (valid) + { //printf("set b3RequestCameraImageSetCameraMatrices\n"); b3RequestCameraImageSetCameraMatrices(command, viewMatrix, projectionMatrix); - + } + b3RequestCameraImageSetPixelResolution(command,width,height); if (b3CanSubmitCommand(sm)) @@ -489,24 +490,25 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) PyObject *pylistPos; PyObject* pylistDep; + int i,j,p; //printf("image width = %d, height = %d\n", imageData.m_pixelWidth, imageData.m_pixelHeight); { PyObject *item; - int bytesPerPixel = 3;//Red, Green, Blue, each 8 bit values + int bytesPerPixel = 4;//Red, Green, Blue, each 8 bit values int num=bytesPerPixel*imageData.m_pixelWidth*imageData.m_pixelHeight; pylistPos = PyTuple_New(num); pylistDep = PyTuple_New(imageData.m_pixelWidth*imageData.m_pixelHeight); - for (int i=0;i Date: Fri, 10 Jun 2016 15:07:41 -0700 Subject: [PATCH 12/53] render image with ability to set pixel resolution and initial camera position --- examples/pybullet/pybullet.c | 301 ++++++++++++++++------------------- 1 file changed, 138 insertions(+), 163 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 41625b8cf..8e916234b 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -19,7 +19,7 @@ enum eCONNECT_METHOD static PyObject *SpamError; static b3PhysicsClientHandle sm=0; - +// Step through one timestep of the simulation static PyObject * pybullet_stepSimulation(PyObject *self, PyObject *args) { @@ -118,7 +118,11 @@ pybullet_disconnectPhysicsServer(PyObject *self, PyObject *args) return Py_None; } - +// Load a URDF file indicating the links and joints of an object +// function can be called without arguments and will default +// to position (0,0,1) with orientation(0,0,0,1) +// else, loadURDF(x,y,z) or +// loadURDF(pos_x, pos_y, pos_z, orn_x, orn_y, orn_z, orn_w) static PyObject * pybullet_loadURDF(PyObject* self, PyObject* args) { @@ -128,13 +132,13 @@ pybullet_loadURDF(PyObject* self, PyObject* args) int bodyIndex = -1; const char* urdfFileName=""; float startPosX =0; - float startPosY =0; - float startPosZ = 1; + float startPosY =0; + float startPosZ = 1; float startOrnX = 0; float startOrnY = 0; float startOrnZ = 0; float startOrnW = 1; - //printf("size=%d\n", size); + if (0==sm) { PyErr_SetString(SpamError, "Not connected to physics server."); @@ -157,14 +161,13 @@ pybullet_loadURDF(PyObject* self, PyObject* args) &startPosX,&startPosY,&startPosZ, &startOrnX,&startOrnY,&startOrnZ, &startOrnW)) return NULL; - } { b3SharedMemoryStatusHandle statusHandle; int statusType; b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(sm, urdfFileName); - //printf("urdf filename = %s\n", urdfFileName); + //setting the initial position, orientation and other arguments are optional b3LoadUrdfCommandSetStartPosition(command, startPosX,startPosY,startPosZ); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); @@ -178,6 +181,8 @@ pybullet_loadURDF(PyObject* self, PyObject* args) } return PyLong_FromLong(bodyIndex); } + +// Reset the simulation to remove all loaded objects static PyObject * pybullet_resetSimulation(PyObject* self, PyObject* args) { @@ -195,6 +200,7 @@ pybullet_resetSimulation(PyObject* self, PyObject* args) return Py_None; } +// Set the gravity of the world with (x, y, z) arguments static PyObject * pybullet_setGravity(PyObject* self, PyObject* args) { @@ -229,6 +235,8 @@ pybullet_setGravity(PyObject* self, PyObject* args) } +// Internal function used to get the base position and orientation +// Orientation is returned in quaternions static void pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double basePosition[3],double baseOrientation[3]) { basePosition[0] = 0.; @@ -272,6 +280,10 @@ static void pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double } } +// Get the positions (x,y,z) and orientation (x,y,z,w) in quaternion +// values for the base link of your object +// Object is retrieved based on body index, which is the order +// the object was loaded into the simulation (0-based) static PyObject * pybullet_getBasePositionAndOrientation(PyObject* self, PyObject* args) { @@ -331,7 +343,9 @@ pybullet_getBasePositionAndOrientation(PyObject* self, PyObject* args) } - +// Return the number of joints in an object based on +// body index; body index is based on order of sequence +// the object is loaded into simulation static PyObject * pybullet_getNumJoints(PyObject* self, PyObject* args) { @@ -359,6 +373,7 @@ pybullet_getNumJoints(PyObject* self, PyObject* args) } } +// TODO(hellojas): set joint positions for a body static PyObject* pybullet_setJointPositions(PyObject* self, PyObject* args) { @@ -372,179 +387,139 @@ pybullet_setJointPositions(PyObject* self, PyObject* args) return Py_None; } - // const unsigned char* m_rgbColorData;//3*m_pixelWidth*m_pixelHeight bytes - // const float* m_depthValues;//m_pixelWidth*m_pixelHeight floats +// const unsigned char* m_rgbColorData;//3*m_pixelWidth*m_pixelHeight bytes +// const float* m_depthValues;//m_pixelWidth*m_pixelHeight floats + +// Render an image from the current timestep of the simulation +// +// Examples: +// renderImage() - default image resolution and camera position +// renderImage(w, h) - image resolution of (w,h), default camera +// renderImage(w, h, view[16], projection[16]) - set both resolution +// and initialize camera to the view and projection values +// +// Note if the (w,h) is too small, the objects may not appear based on +// where the camera has been set +// +// TODO(hellojas): fix image is cut off at head +// TODO(hellojas): should we add check to give minimum image resolution +// to see object based on camera position? static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) { - if (0==sm) + + if (0==sm) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + ///request an image from a simulated camera, using a software renderer. + struct b3CameraImageData imageData; + PyObject* objViewMat,* objProjMat; + int width, height; + int size= PySequence_Size(args); + float viewMatrix[16]; + float projectionMatrix[16]; + + // inialize cmd + b3SharedMemoryCommandHandle command; + command = b3InitRequestCameraImage(sm); + + if (size==2) // only set camera resolution + { + if (PyArg_ParseTuple(args, "ii", &width, &height)) + { + b3RequestCameraImageSetPixelResolution(command,width,height); + } + } + + if (size==4) // set camera resoluation and view and projection matrix + { + if (PyArg_ParseTuple(args, "iiOO", &width, &height, &objViewMat, &objProjMat)) + { + b3RequestCameraImageSetPixelResolution(command,width,height); + + // set camera matrices only if set matrix function succeeds + if (pybullet_internalSetMatrix(objViewMat, viewMatrix) && + (pybullet_internalSetMatrix(objProjMat, projectionMatrix))) + { + b3RequestCameraImageSetCameraMatrices(command, viewMatrix, projectionMatrix); + } + } + } + + if (b3CanSubmitCommand(sm)) { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - ///request an image from a simulated camera, using a software renderer. - struct b3CameraImageData imageData; - PyObject* objViewMat,* objProjMat; - int width, height; - - if (PyArg_ParseTuple(args, "iiOO", &width, &height, &objViewMat, &objProjMat)) - { - - PyObject* seq; - int i, len; - PyObject* item; - float viewMatrix[16]; - float projectionMatrix[16]; - int valid = 1; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + if (statusType==CMD_CAMERA_IMAGE_COMPLETED) { - seq = PySequence_Fast(objViewMat, "expected a sequence"); - len = PySequence_Size(objViewMat); - //printf("objViewMat size = %d\n", len); - if (len==16) - { - - if (PyList_Check(seq)) - { - for (i = 0; i < len; i++) - { - item = PyList_GET_ITEM(seq, i); - viewMatrix[i] = PyFloat_AsDouble(item); - float v = viewMatrix[i]; - //printf("view %d to %f\n", i,v); - - } - } - else - { - for (i = 0; i < len; i++) - { - item = PyTuple_GET_ITEM(seq,i); - viewMatrix[i] = PyFloat_AsDouble(item); - } - } - } else - { - valid = 0; - } - } - - - { - seq = PySequence_Fast(objProjMat, "expected a sequence"); - len = PySequence_Size(objProjMat); - //printf("projMat len = %d\n", len); - if (len==16) - { - if (PyList_Check(seq)) - { - for (i = 0; i < len; i++) - { - item = PyList_GET_ITEM(seq, i); - projectionMatrix[i] = PyFloat_AsDouble(item); - } - } - else - { - for (i = 0; i < len; i++) - { - item = PyTuple_GET_ITEM(seq,i); - projectionMatrix[i] = PyFloat_AsDouble(item); - } - } - } else - { - valid = 0; - } - } + PyObject *item2; + PyObject* pyResultList;//store 4 elements in this result: width, height, rgbData, depth - Py_DECREF(seq); - { - b3SharedMemoryCommandHandle command; - - command = b3InitRequestCameraImage(sm); - if (valid) - { - //printf("set b3RequestCameraImageSetCameraMatrices\n"); - b3RequestCameraImageSetCameraMatrices(command, viewMatrix, projectionMatrix); - } + b3GetCameraImageData(sm, &imageData); + //TODO(hellojas): error handling if image size is 0 + pyResultList = PyTuple_New(4); + PyTuple_SetItem(pyResultList, 0, PyInt_FromLong(imageData.m_pixelWidth)); + PyTuple_SetItem(pyResultList, 1, PyInt_FromLong(imageData.m_pixelHeight)); + + PyObject *pylistRGB; + PyObject* pylistDep; + int i, j, p; - b3RequestCameraImageSetPixelResolution(command,width,height); - - if (b3CanSubmitCommand(sm)) { - b3SharedMemoryStatusHandle statusHandle; - int statusType; - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - statusType = b3GetStatusType(statusHandle); - if (statusType==CMD_CAMERA_IMAGE_COMPLETED) + + PyObject *item; + int bytesPerPixel = 4;//Red, Green, Blue, and Alpha each 8 bit values + int num=bytesPerPixel*imageData.m_pixelWidth*imageData.m_pixelHeight; + pylistRGB = PyTuple_New(num); + pylistDep = PyTuple_New(imageData.m_pixelWidth*imageData.m_pixelHeight); + + for (i=0;i Date: Fri, 10 Jun 2016 15:07:52 -0700 Subject: [PATCH 13/53] render image with ability to set pixel resolution and initial camera position --- examples/SharedMemory/PhysicsClientExample.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index e7d797fa6..bcd84d321 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -628,7 +628,7 @@ void PhysicsClientExample::stepSimulation(float deltaTime) int yIndex = int(float(j)*(float(imageData.m_pixelHeight)/float(camVisualizerHeight))); btClamp(yIndex,0,imageData.m_pixelHeight); btClamp(xIndex,0,imageData.m_pixelWidth); - int bytesPerPixel = 4; + int bytesPerPixel = 4; //RGBA int pixelIndex = (xIndex+yIndex*imageData.m_pixelWidth)*bytesPerPixel; m_canvas->setPixel(m_canvasIndex,i,j, @@ -636,8 +636,7 @@ void PhysicsClientExample::stepSimulation(float deltaTime) imageData.m_rgbColorData[pixelIndex], imageData.m_rgbColorData[pixelIndex+1], imageData.m_rgbColorData[pixelIndex+2], - 255); -// imageData.m_rgbColorData[pixelIndex+3]); + 255); //alpha set to 255 } } m_canvas->refreshImageData(m_canvasIndex); From 9c5a7925f0e3f2b63ccc0a0a0b40b710b7a759a1 Mon Sep 17 00:00:00 2001 From: Jasmine Hsu Date: Fri, 10 Jun 2016 15:14:00 -0700 Subject: [PATCH 14/53] add internal set matrix function --- examples/pybullet/pybullet.c | 39 ++++++++++++++++++++++++++++++++++++ 1 file changed, 39 insertions(+) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 8e916234b..f056b5b57 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -390,6 +390,45 @@ pybullet_setJointPositions(PyObject* self, PyObject* args) // const unsigned char* m_rgbColorData;//3*m_pixelWidth*m_pixelHeight bytes // const float* m_depthValues;//m_pixelWidth*m_pixelHeight floats +// internal function to set a float matrix[16] +// used to initialize camera position with +// a view and projection matrix in renderImage() +static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16]) +{ + + int i, len; + PyObject* item; + PyObject* seq; + + seq = PySequence_Fast(objMat, "expected a sequence"); + len = PySequence_Size(objMat); + if (len==16) + { + + if (PyList_Check(seq)) + { + for (i = 0; i < len; i++) + { + item = PyList_GET_ITEM(seq, i); + matrix[i] = PyFloat_AsDouble(item); + } + } + else + { + for (i = 0; i < len; i++) + { + item = PyTuple_GET_ITEM(seq,i); + matrix[i] = PyFloat_AsDouble(item); + } + } + Py_DECREF(seq); + return 1; + } else + { + Py_DECREF(seq); + return 0; + } +} // Render an image from the current timestep of the simulation // From 6523df336e6925c0b7597fd77bcc5b3e08047e95 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Mon, 13 Jun 2016 10:11:28 -0700 Subject: [PATCH 15/53] Fix pybullet Windows build errors: C99 requires variables to be defined at the start of the function. Improve CMake Windows support to build PyBullet (BUILD_PYBULLET) Implement b3LoadSdfCommandInit in shared memory API Implement pybullet SDF loading binding, in loadSDF API TODO for SDF support is provide way to query object/link/joint information. --- CMakeLists.txt | 5 +- examples/SharedMemory/PhysicsClientC_API.cpp | 31 ++++ examples/SharedMemory/PhysicsClientC_API.h | 2 + .../SharedMemory/PhysicsClientExample.cpp | 70 +++++--- .../PhysicsClientSharedMemory.cpp | 38 ++++- .../PhysicsServerCommandProcessor.cpp | 156 +++++++++++++++++- .../PhysicsServerCommandProcessor.h | 4 + examples/SharedMemory/SharedMemoryCommands.h | 25 +++ examples/SharedMemory/SharedMemoryPublic.h | 1 + examples/pybullet/pybullet.c | 93 +++++++++-- 10 files changed, 377 insertions(+), 48 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b34c7f996..083835f6e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -191,14 +191,15 @@ IF (APPLE) ENDIF() OPTION(BUILD_BULLET3 "Set when you want to build Bullet 3" ON) + +FIND_PACKAGE(PythonLibs) OPTION(BUILD_PYBULLET "Set when you want to build pybullet (experimental Python bindings for Bullet)" OFF) + IF(BUILD_PYBULLET) IF(WIN32) - FIND_PACKAGE(PythonLibs 3.4 REQUIRED) SET(BUILD_SHARED_LIBS OFF CACHE BOOL "Shared Libs" FORCE) ELSE(WIN32) - FIND_PACKAGE(PythonLibs 2.7 REQUIRED) SET(BUILD_SHARED_LIBS ON CACHE BOOL "Shared Libs" FORCE) ENDIF(WIN32) ENDIF(BUILD_PYBULLET) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index c6468cb89..ae1a54782 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -519,6 +519,33 @@ int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle) return CMD_INVALID_STATUS; } +int b3GetStatusBodyIndices(b3SharedMemoryStatusHandle statusHandle, int* bodyIndicesOut, int bodyIndicesCapacity) +{ + int numBodies = 0; + const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; + b3Assert(status); + + if (status) + { + switch (status->m_type) + { + case CMD_SDF_LOADING_COMPLETED: + { + int i,maxBodies; + numBodies = status->m_sdfLoadedArgs.m_numBodies; + maxBodies = btMin(bodyIndicesCapacity, numBodies); + for (i=0;im_sdfLoadedArgs.m_bodyUniqueIds[i]; + } + break; + } + } + } + + return numBodies; +} + int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle) { const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; @@ -558,6 +585,10 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle, const double* jointReactionForces[]) { const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; const SendActualStateArgs &args = status->m_sendActualStateArgs; + btAssert(status->m_type == CMD_URDF_LOADING_COMPLETED); + if (status->m_type != CMD_URDF_LOADING_COMPLETED) + return false; + if (bodyUniqueId) { *bodyUniqueId = args.m_bodyUniqueId; } diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index b67f078ae..468347ef5 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -40,6 +40,8 @@ b3SharedMemoryStatusHandle b3ProcessServerStatus(b3PhysicsClientHandle physClien /// Get the physics server return status type. See EnumSharedMemoryServerStatus in SharedMemoryPublic.h for error codes. int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle); +int b3GetStatusBodyIndices(b3SharedMemoryStatusHandle statusHandle, int* bodyIndicesOut, int bodyIndicesCapacity); + int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle); int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle, diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index bcd84d321..acce1dfde 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -77,9 +77,9 @@ protected: virtual void resetCamera() { - float dist = 1.1; - float pitch = 50; - float yaw = 35; + float dist = 4; + float pitch = 193; + float yaw = 25; float targetPos[3]={0,0,0.5};//-3,2.8,-2.5}; m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]); @@ -223,9 +223,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int 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; @@ -234,7 +232,13 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) startPosY += 2.f; // ret = b3LoadUrdfCommandSetUseFixedBase(commandHandle, 1); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); - + break; + } + + case CMD_LOAD_SDF: + { + b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "two_cubes.sdf"); + b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_REQUEST_CAMERA_IMAGE_DATA: @@ -452,6 +456,7 @@ void PhysicsClientExample::createButtons() m_guiHelper->getParameterInterface()->removeAllParameters(); createButton("Load URDF",CMD_LOAD_URDF, isTrigger); + createButton("Load SDF",CMD_LOAD_SDF, isTrigger); createButton("Get Camera Image",CMD_REQUEST_CAMERA_IMAGE_DATA,isTrigger); createButton("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger); createButton("Send Bullet Stream",CMD_SEND_BULLET_DATA_STREAM, isTrigger); @@ -649,11 +654,40 @@ void PhysicsClientExample::stepSimulation(float deltaTime) b3Warning("Camera image FAILED\n"); } - - if (statusType == CMD_URDF_LOADING_COMPLETED) + if (statusType == CMD_SDF_LOADING_COMPLETED) { - int bodyIndex = b3GetStatusBodyIndex(status); - if (bodyIndex>=0) + //int bodyIndex = b3GetStatusBodyIndex(status); + /*if (bodyIndex>=0) + { + int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex); + + for (int i=0;igetParameterInterface()->registerComboBox(comboParams); + } + */ + } + + + if (statusType == CMD_URDF_LOADING_COMPLETED) + { + int bodyIndex = b3GetStatusBodyIndex(status); + if (bodyIndex>=0) { int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex); @@ -662,7 +696,6 @@ void PhysicsClientExample::stepSimulation(float deltaTime) b3JointInfo info; b3GetJointInfo(m_physicsClientHandle,bodyIndex,i,&info); b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex); - } ComboBoxParams comboParams; comboParams.m_comboboxId = bodyIndex; @@ -676,12 +709,8 @@ void PhysicsClientExample::stepSimulation(float deltaTime) comboParams.m_items=blarray;//{&bla}; m_guiHelper->getParameterInterface()->registerComboBox(comboParams); - - } - } - } } if (b3CanSubmitCommand(m_physicsClientHandle)) @@ -734,13 +763,12 @@ void PhysicsClientExample::stepSimulation(float deltaTime) if (m_numMotors) { enqueueCommand(CMD_SEND_DESIRED_STATE); - enqueueCommand(CMD_STEP_FORWARD_SIMULATION); - if (m_options != eCLIENTEXAMPLE_SERVER) - { - enqueueCommand(CMD_REQUEST_DEBUG_LINES); - } - //enqueueCommand(CMD_REQUEST_ACTUAL_STATE); } + enqueueCommand(CMD_STEP_FORWARD_SIMULATION); + if (m_options != eCLIENTEXAMPLE_SERVER) + { + enqueueCommand(CMD_REQUEST_DEBUG_LINES); + } } } diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 728358b93..a4c57574d 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -40,7 +40,7 @@ struct PhysicsClientSharedMemoryInternalData { SharedMemoryStatus m_lastServerStatus; int m_counter; - bool m_serverLoadUrdfOK; + bool m_isConnected; bool m_waitingForServer; bool m_hasLastServerStatus; @@ -54,7 +54,6 @@ struct PhysicsClientSharedMemoryInternalData { m_counter(0), m_cachedCameraPixelsWidth(0), m_cachedCameraPixelsHeight(0), - m_serverLoadUrdfOK(false), m_isConnected(false), m_waitingForServer(false), m_hasLastServerStatus(false), @@ -204,8 +203,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { } break; } + case CMD_SDF_LOADING_COMPLETED: { + + if (m_data->m_verboseOutput) { + b3Printf("Server loading the SDF OK\n"); + } + break; + } case CMD_URDF_LOADING_COMPLETED: { - m_data->m_serverLoadUrdfOK = true; + if (m_data->m_verboseOutput) { b3Printf("Server loading the URDF OK\n"); } @@ -265,7 +271,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { if (m_data->m_verboseOutput) { b3Printf("Server failed loading the URDF...\n"); } - m_data->m_serverLoadUrdfOK = false; + + break; + } + + case CMD_SDF_LOADING_FAILED: { + if (m_data->m_verboseOutput) { + b3Printf("Server failed loading the SDF...\n"); + } + break; } @@ -490,6 +504,22 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { m_data->m_waitingForServer = true; } + /*if (serverCmd.m_type == CMD_SDF_LOADING_COMPLETED) + { + int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies; + if (numBodies>0) + { + SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; + //now transfer the information of the individual objects etc. + command.m_type = CMD_REQUEST_SDF_INFO; + command.m_updateFlags = SDF_REQUEST_INFO_BODY; + command.m_sdfRequestInfoArgs.m_infoIndex = 0; + submitClientCommand(command); + return 0; + } + } + */ + if (serverCmd.m_type == CMD_CAMERA_IMAGE_COMPLETED) { SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 89490caa6..98994ea68 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -393,6 +393,7 @@ struct PhysicsServerCommandProcessorInternalData btMultiBodyDynamicsWorld* m_dynamicsWorld; SharedMemoryDebugDrawer* m_remoteDebugDrawer; + btAlignedObjectArray m_sdfRecentLoadedBodies; @@ -681,6 +682,128 @@ void PhysicsServerCommandProcessor::createJointMotors(btMultiBody* mb) } +bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes) +{ + btAssert(m_data->m_dynamicsWorld); + if (!m_data->m_dynamicsWorld) + { + b3Error("loadSdf: No valid m_dynamicsWorld"); + return false; + } + + m_data->m_sdfRecentLoadedBodies.clear(); + + BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter); + + bool useFixedBase = false; + bool loadOk = u2b.loadSDF(fileName, useFixedBase); + if (loadOk) + { + for (int i=0;im_collisionShapes.push_back(shape); + if (shape->isCompound()) + { + btCompoundShape* compound = (btCompoundShape*) shape; + for (int childIndex=0;childIndexgetNumChildShapes();childIndex++) + { + m_data->m_collisionShapes.push_back(compound->getChildShape(childIndex)); + } + } + + } + + btTransform rootTrans; + rootTrans.setIdentity(); + if (m_data->m_verboseOutput) + { + b3Printf("loaded %s OK!", fileName); + } + + for (int m =0; mallocHandle(); + + InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId); + + { + btScalar mass = 0; + bodyHandle->m_rootLocalInertialFrame.setIdentity(); + btVector3 localInertiaDiagonal(0,0,0); + int urdfLinkIndex = u2b.getRootLinkIndex(); + u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,bodyHandle->m_rootLocalInertialFrame); + } + + + + //todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user + int rootLinkIndex = u2b.getRootLinkIndex(); + b3Printf("urdf root link index = %d\n",rootLinkIndex); + MyMultiBodyCreator creation(m_data->m_guiHelper); + + u2b.getRootTransformInWorld(rootTrans); + bool useMultiBody = true; + ConvertURDF2Bullet(u2b,creation, rootTrans,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix(),true); + + + + mb = creation.getBulletMultiBody(); + if (mb) + { + bodyHandle->m_multiBody = mb; + + m_data->m_sdfRecentLoadedBodies.push_back(bodyUniqueId); + + createJointMotors(mb); + + //serialize the btMultiBody and send the data to the client. This is one way to get the link/joint names across the (shared memory) wire + + UrdfLinkNameMapUtil* util2 = new UrdfLinkNameMapUtil; + m_data->m_urdfLinkNameMapper.push_back(util2); + util2->m_mb = mb; + util2->m_memSerializer = 0; + //disable serialization of the collision objects (they are too big, and the client likely doesn't need them); + + bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks()); + for (int i=0;igetNumLinks();i++) + { + //disable serialization of the collision objects + + int urdfLinkIndex = creation.m_mb2urdfLink[i]; + btScalar mass; + btVector3 localInertiaDiagonal(0,0,0); + btTransform localInertialFrame; + u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,localInertialFrame); + bodyHandle->m_linkLocalInertialFrames.push_back(localInertialFrame); + + std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str()); + m_data->m_strings.push_back(linkName); + + mb->getLink(i).m_linkName = linkName->c_str(); + + std::string* jointName = new std::string(u2b.getJointName(urdfLinkIndex).c_str()); + m_data->m_strings.push_back(jointName); + + mb->getLink(i).m_jointName = jointName->c_str(); + } + std::string* baseName = new std::string(u2b.getLinkName(u2b.getRootLinkIndex())); + m_data->m_strings.push_back(baseName); + mb->setBaseName(baseName->c_str()); + } else + { + b3Warning("No multibody loaded from URDF. Could add btRigidBody+btTypedConstraint solution later."); + } + + } + } + return loadOk; +} bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn, bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes) @@ -731,8 +854,6 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto ConvertURDF2Bullet(u2b,creation, tr,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix()); - - ///todo(erwincoumans) refactor this memory allocation issue for (int i=0;im_multiBody = mb; + createJointMotors(mb); @@ -805,12 +928,13 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto const char* structType = mb->serialize(chunk->m_oldPtr, util->m_memSerializer); util->m_memSerializer->finalizeChunk(chunk,structType,BT_MULTIBODY_CODE,mb); - return true; + return true; } else { b3Warning("No multibody loaded from URDF. Could add btRigidBody+btTypedConstraint solution later."); return false; } + } else { btAssert(0); @@ -1047,6 +1171,28 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm break; } + case CMD_LOAD_SDF: + { + const SdfArgs& sdfArgs = clientCmd.m_sdfArguments; + if (m_data->m_verboseOutput) + { + b3Printf("Processed CMD_LOAD_SDF:%s", sdfArgs.m_sdfFileName); + } + + bool completedOk = loadSdf(sdfArgs.m_sdfFileName,bufferServerToClient, bufferSizeInBytes); + + //serverStatusOut.m_type = CMD_SDF_LOADING_FAILED; + serverStatusOut.m_sdfLoadedArgs.m_numBodies = m_data->m_sdfRecentLoadedBodies.size(); + int maxBodies = btMin(MAX_SDF_BODIES, serverStatusOut.m_sdfLoadedArgs.m_numBodies); + for (int i=0;im_sdfRecentLoadedBodies[i]; + } + + serverStatusOut.m_type = CMD_SDF_LOADING_COMPLETED; + hasStatus = true; + break; + } case CMD_LOAD_URDF: { diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index dca5f8930..242a41176 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -19,6 +19,10 @@ class PhysicsServerCommandProcessor protected: + + + bool loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes); + bool loadUrdf(const char* fileName, const class btVector3& pos, const class btQuaternion& orn, bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index fa67d7673..a4c93180e 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -33,6 +33,7 @@ #define MAX_SDF_FILENAME_LENGTH 1024 #define MAX_FILENAME_LENGTH MAX_URDF_FILENAME_LENGTH #define MAX_NUM_LINKS MAX_DEGREE_OF_FREEDOM +#define MAX_SDF_BODIES 1024 struct TmpFloat3 { @@ -298,6 +299,28 @@ struct CreateBoxShapeArgs double m_colorRGBA[4]; }; +struct SdfLoadedArgs +{ + int m_numBodies; + int m_bodyUniqueIds[MAX_SDF_BODIES]; + + ///@todo(erwincoumans) load cameras, lights etc + //int m_numCameras; + //int m_numLights; +}; + + +struct SdfRequestInfoArgs +{ + int m_infoIndex; +}; + +enum EnumSdfRequestInfoFlags +{ + SDF_REQUEST_INFO_BODY=1, + //SDF_REQUEST_INFO_CAMERA=2, +}; + struct SharedMemoryCommand { int m_type; @@ -312,6 +335,7 @@ struct SharedMemoryCommand { struct UrdfArgs m_urdfArguments; struct SdfArgs m_sdfArguments; + struct SdfRequestInfoArgs m_sdfRequestInfoArgs; struct InitPoseArgs m_initPoseArgs; struct SendPhysicsSimulationParameters m_physSimParamArgs; struct BulletDataStreamArgs m_dataStreamArguments; @@ -340,6 +364,7 @@ struct SharedMemoryStatus union { struct BulletDataStreamArgs m_dataStreamArguments; + struct SdfLoadedArgs m_sdfLoadedArgs; struct SendActualStateArgs m_sendActualStateArgs; struct SendDebugLinesArgs m_sendDebugLinesArgs; struct SendPixelDataArgs m_sendPixelDataArguments; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 2f676e057..a038ecf51 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -19,6 +19,7 @@ enum EnumSharedMemoryClientCommand CMD_SEND_DESIRED_STATE,//todo: reconsider naming, for example SET_JOINT_CONTROL_VARIABLE? CMD_REQUEST_ACTUAL_STATE, CMD_REQUEST_DEBUG_LINES, + CMD_REQUEST_SDF_INFO, CMD_STEP_FORWARD_SIMULATION, CMD_RESET_SIMULATION, CMD_PICK_BODY, diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index f056b5b57..b735caeb1 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -182,6 +182,61 @@ pybullet_loadURDF(PyObject* self, PyObject* args) return PyLong_FromLong(bodyIndex); } +#define MAX_SDF_BODIES 512 + +static PyObject* +pybullet_loadSDF(PyObject* self, PyObject* args) +{ + const char* sdfFileName=""; + int size= PySequence_Size(args); + int numBodies = 0; + int i; + int bodyIndicesOut[MAX_SDF_BODIES]; + PyObject* pylist=0; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle command; + + if (0==sm) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + if (size==1) + { + if (!PyArg_ParseTuple(args, "s", &sdfFileName)) + return NULL; + } + + command = b3LoadSdfCommandInit(sm, sdfFileName); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + if (statusType!=CMD_SDF_LOADING_COMPLETED) + { + PyErr_SetString(SpamError, "Cannot load SDF file."); + return NULL; + } + + numBodies = b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, MAX_SDF_BODIES); + if (numBodies > MAX_SDF_BODIES) + { + PyErr_SetString(SpamError, "SDF exceeds body capacity"); + return NULL; + } + + pylist = PyTuple_New(numBodies); + + if (numBodies >0 && numBodies <= MAX_SDF_BODIES) + { + for (i=0;i Date: Mon, 13 Jun 2016 18:33:18 -0700 Subject: [PATCH 16/53] remove unused findex from btTypedConstraint.h --- src/BulletDynamics/ConstraintSolver/btTypedConstraint.h | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h index b55db0c1b..8a2a2d1ae 100644 --- a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h @@ -145,11 +145,6 @@ public: // lo and hi limits for variables (set to -/+ infinity on entry). btScalar *m_lowerLimit,*m_upperLimit; - // findex vector for variables. see the LCP solver interface for a - // description of what this does. this is set to -1 on entry. - // note that the returned indexes are relative to the first index of - // the constraint. - int *findex; // number of solver iterations int m_numIterations; From 566ed87c93cc222cc451cedac68ca799ed24e154 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 13 Jun 2016 18:58:52 -0700 Subject: [PATCH 17/53] fix a regression in a test --- examples/SharedMemory/PhysicsClientC_API.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index ae1a54782..81a58a992 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -585,8 +585,8 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle, const double* jointReactionForces[]) { const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; const SendActualStateArgs &args = status->m_sendActualStateArgs; - btAssert(status->m_type == CMD_URDF_LOADING_COMPLETED); - if (status->m_type != CMD_URDF_LOADING_COMPLETED) + btAssert(status->m_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED); + if (status->m_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) return false; if (bodyUniqueId) { From 4f4eb3a9c6cfe5328a5410b0cce52036f5b11385 Mon Sep 17 00:00:00 2001 From: Giorgos Tzampanakis Date: Tue, 14 Jun 2016 13:40:38 +0000 Subject: [PATCH 18/53] Fixed: Error because of unparameterized btVectorX --- src/BulletInverseDynamics/details/IDLinearMathInterface.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp b/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp index c77ba3cba..d81647b68 100644 --- a/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp +++ b/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp @@ -48,9 +48,9 @@ inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; } class vecx : public btVectorX { public: - vecx(int size) : btVectorX(size) {} + vecx(int size) : btVectorX(size) {} const vecx& operator=(const btVectorX& rhs) { - *static_cast(this) = rhs; + *static_cast*>(this) = rhs; return *this; } From b06e1cb873546d940003834121e2f928190adddb Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Tue, 14 Jun 2016 07:53:15 -0700 Subject: [PATCH 19/53] Update CollisionShape2TriangleMesh.cpp fix memory leak, thanks Ilya Kostrikov for the report. --- examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp b/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp index 1105955dc..2bc8a0ea4 100644 --- a/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp +++ b/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp @@ -167,6 +167,7 @@ void CollisionShape2TriangleMesh(btCollisionShape* collisionShape, const btTrans } } } + delete hull; } } else { From eeaff0747b47c46b1eb86e239475475e684762d8 Mon Sep 17 00:00:00 2001 From: Jasmine Hsu Date: Tue, 14 Jun 2016 14:08:24 -0700 Subject: [PATCH 20/53] return joint state given a joint index and body index --- examples/pybullet/pybullet.c | 182 ++++++++++++++++++++++++++++++++++- 1 file changed, 179 insertions(+), 3 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index f056b5b57..e4ffeeedf 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -235,6 +235,7 @@ pybullet_setGravity(PyObject* self, PyObject* args) } + // Internal function used to get the base position and orientation // Orientation is returned in quaternions static void pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double basePosition[3],double baseOrientation[3]) @@ -260,12 +261,18 @@ static void pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double const int status_type = b3GetStatusType(status_handle); const double* actualStateQ; + // const double* jointReactionForces[]; + int i; b3GetStatusActualState(status_handle, 0/* body_unique_id */, 0/* num_degree_of_freedom_q */, 0/* num_degree_of_freedom_u */, 0 /*root_local_inertial_frame*/, &actualStateQ , 0 /* actual_state_q_dot */, 0 /* joint_reaction_forces */); + // printf("joint reaction forces="); + // for (i=0; i < (sizeof(jointReactionForces)/sizeof(double)); i++) { + // printf("%f ", jointReactionForces[i]); + // } //now, position x,y,z = actualStateQ[0],actualStateQ[1],actualStateQ[2] //and orientation x,y,z,w = actualStateQ[3],actualStateQ[4],actualStateQ[5],actualStateQ[6] basePosition[0] = actualStateQ[0]; @@ -300,6 +307,7 @@ pybullet_getBasePositionAndOrientation(PyObject* self, PyObject* args) return NULL; } + double basePosition[3]; double baseOrientation[4]; @@ -375,14 +383,173 @@ pybullet_getNumJoints(PyObject* self, PyObject* args) // TODO(hellojas): set joint positions for a body static PyObject* -pybullet_setJointPositions(PyObject* self, PyObject* args) +pybullet_initializeJointPositions(PyObject* self, PyObject* args) { if (0==sm) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - + + + Py_INCREF(Py_None); + return Py_None; +} + + +// TODO(hellojas): get joint positions for a body +static PyObject* +pybullet_getJointInfo(PyObject* self, PyObject* args) +{ + if (0==sm) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + Py_INCREF(Py_None); + return Py_None; +} + + +// Returns the state of a specific joint in a given bodyIndex +// The state of a joint includes the following: +// position, velocity, force torque (6 values), and motor torque +// The returned pylist is an array of [float, float, float[6], float] + +// TODO(hellojas): check accuracy of position and velocity +// TODO(hellojas): check force torque values + +static PyObject* +pybullet_getJointState(PyObject* self, PyObject* args) +{ + if (0==sm) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + + PyObject *pyListJointForceTorque; + PyObject *pyListJointState; + PyObject *item; + + struct b3JointInfo info; + struct b3JointSensorState sensorState; + + int bodyIndex = -1; + int jointIndex = -1; + int sensorStateSize = 4; // size of struct b3JointSensorState + int forceTorqueSize = 6; // size of force torque list from b3JointSensorState + int i, j; + + + int size= PySequence_Size(args); + + if (size==2) // get body index and joint index + { + if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex)) + { + + b3SharedMemoryCommandHandle cmd_handle = + b3RequestActualStateCommandInit(sm, bodyIndex); + b3SharedMemoryStatusHandle status_handle = + b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); + + pyListJointState = PyTuple_New(sensorStateSize); + pyListJointForceTorque = PyTuple_New(forceTorqueSize); + + + // double m_jointPosition; + // double m_jointVelocity; + // double m_jointForceTorque[6]; /* note to roboticists: this is NOT the motor torque/force, but the spatial reaction force vector at joint */ + // double m_jointMotorTorque; + b3GetJointState(sm, status_handle, jointIndex, &sensorState); + // printf("Joint%d: position=%f velocity=%f motortorque=%f\n", + // jointIndex, + // sensorState.m_jointPosition, + // sensorState.m_jointVelocity, + // sensorState.m_jointMotorTorque); + + PyTuple_SetItem(pyListJointState, 0, + PyFloat_FromDouble(sensorState.m_jointPosition)); + PyTuple_SetItem(pyListJointState, 1, + PyFloat_FromDouble(sensorState.m_jointVelocity)); + + // joint force torque is list of 6 + /* note to roboticists: this is NOT the motor torque/force, but the spatial reaction force vector at joint */ + // printf(" jointForceTorque = "); + for (j = 0; j < forceTorqueSize; j++) { + // printf("%f ", sensorState.m_jointForceTorque[j]); + PyTuple_SetItem(pyListJointForceTorque, j, + PyFloat_FromDouble(sensorState.m_jointForceTorque[j])); + } + + PyTuple_SetItem(pyListJointState, 2, + pyListJointForceTorque); + + PyTuple_SetItem(pyListJointState, 3, + PyFloat_FromDouble(sensorState.m_jointMotorTorque)); + + return pyListJointState; + } + } + + Py_INCREF(Py_None); + return Py_None; +} + + +// TODO(hellojas): get joint positions for a body +static PyObject* +pybullet_getJointPositionAndVelocity(PyObject* self, PyObject* args) +{ + if (0==sm) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + struct b3JointInfo info; + int bodyIndex = 0; + int forceTorqueSize = 6; + PyObject *pylistJointPos; + PyObject *item; + int i, j, numJoints; + struct b3JointSensorState sensorState; + + b3SharedMemoryCommandHandle cmd_handle = + b3RequestActualStateCommandInit(sm, bodyIndex); + b3SharedMemoryStatusHandle status_handle = + b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); + + numJoints = b3GetNumJoints(sm,bodyIndex); + + const char *pyListJointNames[numJoints]; + pylistJointPos = PyTuple_New(numJoints); + + +// double m_jointPosition; +// double m_jointVelocity; +// double m_jointForceTorque[6]; /* note to roboticists: this is NOT the motor torque/force, but the spatial reaction force vector at joint */ +// double m_jointMotorTorque; + for (i = 0; i < numJoints; ++i) { + b3GetJointState(sm, status_handle, i, &sensorState); + printf("Joint%d: position=%f velocity=%f motortorque=%f\n", + i, + sensorState.m_jointPosition, + sensorState.m_jointVelocity, + sensorState.m_jointMotorTorque); + + // item = PyFloat_FromDouble(basePosition[i]); + + printf(" jointForceTorque = "); + for (j = 0; j < forceTorqueSize; j++) { + printf("%f ", sensorState.m_jointPosition); + } + printf("\n"); +} + Py_INCREF(Py_None); return Py_None; } @@ -565,9 +732,18 @@ static PyMethodDef SpamMethods[] = { {"setGravity", pybullet_setGravity, METH_VARARGS, "Set the gravity acceleration (x,y,z)."}, - {"initializeJointPositions", pybullet_setJointPositions, METH_VARARGS, + {"initializeJointPositions", pybullet_initializeJointPositions, METH_VARARGS, "Initialize the joint positions for all joints. This method skips any physics simulation and teleports all joints to the new positions."}, + // {"getJointPositions", pybullet_getJointPositions, METH_VARARGS, + // "Get the joint positions for all joints."}, + // + {"getJointInfo", pybullet_getJointInfo, METH_VARARGS, + "Get the joint metadata info for a joint on a body. This includes joint index, name, type, q-index and u-index."}, + + {"getJointState", pybullet_getJointState, METH_VARARGS, + "Get the joint metadata info for a joint on a body."}, + {"renderImage", pybullet_renderImage, METH_VARARGS, "Render an image (given the pixel resolution width & height and camera view & projection matrices), and return the 8-8-8bit RGB pixel data and floating point depth values"}, From d53d6366deebfbd223e0075009e9d4d7dce4a911 Mon Sep 17 00:00:00 2001 From: Jasmine Hsu Date: Tue, 14 Jun 2016 14:35:45 -0700 Subject: [PATCH 21/53] get the state of a specific joint given a body index --- examples/pybullet/pybullet.c | 59 +++++++++++++++++++++++++++++++++++- 1 file changed, 58 insertions(+), 1 deletion(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 8cd657d81..afcdcb76d 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -306,7 +306,6 @@ static void pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double { - { b3SharedMemoryCommandHandle cmd_handle = b3RequestActualStateCommandInit(sm, bodyIndex); @@ -463,6 +462,64 @@ pybullet_getJointInfo(PyObject* self, PyObject* args) return NULL; } + PyObject *pyListJointInfo; + + struct b3JointInfo info; + + int bodyIndex = -1; + int jointIndex = -1; + int jointInfoSize = 8; //size of struct b3JointInfo + + int size= PySequence_Size(args); + + if (size==2) // get body index and joint index + { + if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex)) + { + + // printf("body index = %d, joint index =%d\n", bodyIndex, jointIndex); + + b3SharedMemoryCommandHandle cmd_handle = + b3RequestActualStateCommandInit(sm, bodyIndex); + b3SharedMemoryStatusHandle status_handle = + b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); + + pyListJointInfo = PyTuple_New(jointInfoSize); + + b3GetJointInfo(sm, bodyIndex, jointIndex, &info); + + // printf("Joint%d %s, type %d, at q-index %d and u-index %d\n", + // info.m_jointIndex, + // info.m_jointName, + // info.m_jointType, + // info.m_qIndex, + // info.m_uIndex); + // printf(" flags=%d jointDamping=%f jointFriction=%f\n", + // info.m_flags, + // info.m_jointDamping, + // info.m_jointFriction); + + PyTuple_SetItem(pyListJointInfo, 0, + PyInt_FromLong(info.m_jointIndex)); + PyTuple_SetItem(pyListJointInfo, 1, + PyString_FromString(info.m_jointName)); + PyTuple_SetItem(pyListJointInfo, 2, + PyInt_FromLong(info.m_jointType)); + PyTuple_SetItem(pyListJointInfo, 3, + PyInt_FromLong(info.m_qIndex)); + PyTuple_SetItem(pyListJointInfo, 4, + PyInt_FromLong(info.m_uIndex)); + PyTuple_SetItem(pyListJointInfo, 5, + PyInt_FromLong(info.m_flags)); + PyTuple_SetItem(pyListJointInfo, 6, + PyFloat_FromDouble(info.m_jointDamping)); + PyTuple_SetItem(pyListJointInfo, 7, + PyFloat_FromDouble(info.m_jointFriction)); + + return pyListJointInfo; + } + } + Py_INCREF(Py_None); return Py_None; } From d6ab1ab43416df3207d5d9f48231cd9548b5ae9f Mon Sep 17 00:00:00 2001 From: Jasmine Hsu Date: Tue, 14 Jun 2016 14:58:36 -0700 Subject: [PATCH 22/53] initialize a single joint position for a given body index --- examples/pybullet/pybullet.c | 119 +++++++++++++++++------------------ 1 file changed, 57 insertions(+), 62 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index afcdcb76d..b645045b3 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -436,7 +436,7 @@ pybullet_getNumJoints(PyObject* self, PyObject* args) } } -// TODO(hellojas): set joint positions for a body +// Initalize all joint positions given a list of values static PyObject* pybullet_initializeJointPositions(PyObject* self, PyObject* args) { @@ -445,13 +445,65 @@ pybullet_initializeJointPositions(PyObject* self, PyObject* args) PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - + // TODO(hellojas): initialize all joint positions given a pylist of values Py_INCREF(Py_None); return Py_None; } +// Initalize a single joint position for a specific body index +// +// This method skips any physics simulation and +// teleports all joints to the new positions. +// TODO(hellojas): test accuracy of joint position values +static PyObject* +pybullet_initializeJointPosition(PyObject* self, PyObject* args) +{ + if (0==sm) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + int i; + int bodyIndex = -1; + int jointIndex = -1; + double jointPos = 0.0; + + int size= PySequence_Size(args); + + if (size==3) // get body index, joint index, and joint position value + { + if (PyArg_ParseTuple(args, "iid", &bodyIndex, &jointIndex, &jointPos)) + { + b3SharedMemoryCommandHandle cmd_handle = b3CreatePoseCommandInit(sm, bodyIndex); + + printf("initializing joint %d at %f\n", jointIndex, jointPos); + b3CreatePoseCommandSetJointPosition(sm, cmd_handle, jointIndex, jointPos); + + b3SharedMemoryStatusHandle status_handle = + b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); + + const int status_type = b3GetStatusType(status_handle); + + } + } + + Py_INCREF(Py_None); + return Py_None; +} + + +// Get the a single joint info for a specific bodyIndex +// +// Joint information includes: +// index, name, type, q-index, u-index, +// flags, joint damping, joint friction +// +// The format of the returned list is +// [int, str, int, int, int, int, float, float] +// // TODO(hellojas): get joint positions for a body static PyObject* pybullet_getJointInfo(PyObject* self, PyObject* args) @@ -613,60 +665,6 @@ pybullet_getJointState(PyObject* self, PyObject* args) } -// TODO(hellojas): get joint positions for a body -static PyObject* -pybullet_getJointPositionAndVelocity(PyObject* self, PyObject* args) -{ - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - struct b3JointInfo info; - int bodyIndex = 0; - int forceTorqueSize = 6; - PyObject *pylistJointPos; - PyObject *item; - int i, j, numJoints; - struct b3JointSensorState sensorState; - - b3SharedMemoryCommandHandle cmd_handle = - b3RequestActualStateCommandInit(sm, bodyIndex); - b3SharedMemoryStatusHandle status_handle = - b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); - - numJoints = b3GetNumJoints(sm,bodyIndex); - - const char *pyListJointNames[numJoints]; - pylistJointPos = PyTuple_New(numJoints); - - -// double m_jointPosition; -// double m_jointVelocity; -// double m_jointForceTorque[6]; /* note to roboticists: this is NOT the motor torque/force, but the spatial reaction force vector at joint */ -// double m_jointMotorTorque; - for (i = 0; i < numJoints; ++i) { - b3GetJointState(sm, status_handle, i, &sensorState); - printf("Joint%d: position=%f velocity=%f motortorque=%f\n", - i, - sensorState.m_jointPosition, - sensorState.m_jointVelocity, - sensorState.m_jointMotorTorque); - - // item = PyFloat_FromDouble(basePosition[i]); - - printf(" jointForceTorque = "); - for (j = 0; j < forceTorqueSize; j++) { - printf("%f ", sensorState.m_jointPosition); - } - printf("\n"); -} - - Py_INCREF(Py_None); - return Py_None; -} - // const unsigned char* m_rgbColorData;//3*m_pixelWidth*m_pixelHeight bytes // const float* m_depthValues;//m_pixelWidth*m_pixelHeight floats @@ -852,9 +850,9 @@ static PyMethodDef SpamMethods[] = { {"initializeJointPositions", pybullet_initializeJointPositions, METH_VARARGS, "Initialize the joint positions for all joints. This method skips any physics simulation and teleports all joints to the new positions."}, - // {"getJointPositions", pybullet_getJointPositions, METH_VARARGS, - // "Get the joint positions for all joints."}, - // + {"initializeJointPosition", pybullet_initializeJointPosition, METH_VARARGS, + "Initialize the joint position for one joint. This method skips any physics simulation and teleports the joint to the new position."}, + {"getJointInfo", pybullet_getJointInfo, METH_VARARGS, "Get the joint metadata info for a joint on a body. This includes joint index, name, type, q-index and u-index."}, @@ -866,9 +864,6 @@ static PyMethodDef SpamMethods[] = { {"getBasePositionAndOrientation", pybullet_getBasePositionAndOrientation, METH_VARARGS, "Get the world position and orientation of the base of the object. (x,y,z) position vector and (x,y,z,w) quaternion orientation."}, - - {"getNumsetGravity", pybullet_setGravity, METH_VARARGS, - "Set the gravity acceleration (x,y,z)."}, {"getNumJoints", pybullet_getNumJoints, METH_VARARGS, "Get the number of joints for an object."}, From 934725554f61ee567e76207d13c6d8e7e6c9447a Mon Sep 17 00:00:00 2001 From: Jasmine Hsu Date: Tue, 14 Jun 2016 15:21:50 -0700 Subject: [PATCH 23/53] remove initializeJointPosition as setting one joint is not supported --- examples/pybullet/pybullet.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index b645045b3..b8bf684c2 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -479,7 +479,7 @@ pybullet_initializeJointPosition(PyObject* self, PyObject* args) { b3SharedMemoryCommandHandle cmd_handle = b3CreatePoseCommandInit(sm, bodyIndex); - printf("initializing joint %d at %f\n", jointIndex, jointPos); + // printf("initializing joint %d at %f\n", jointIndex, jointPos); b3CreatePoseCommandSetJointPosition(sm, cmd_handle, jointIndex, jointPos); b3SharedMemoryStatusHandle status_handle = @@ -850,9 +850,9 @@ static PyMethodDef SpamMethods[] = { {"initializeJointPositions", pybullet_initializeJointPositions, METH_VARARGS, "Initialize the joint positions for all joints. This method skips any physics simulation and teleports all joints to the new positions."}, - {"initializeJointPosition", pybullet_initializeJointPosition, METH_VARARGS, - "Initialize the joint position for one joint. This method skips any physics simulation and teleports the joint to the new position."}, - + // {"initializeJointPosition", pybullet_initializeJointPosition, METH_VARARGS, + // "Initialize the joint position for one joint. This method skips any physics simulation and teleports the joint to the new position."}, + // {"getJointInfo", pybullet_getJointInfo, METH_VARARGS, "Get the joint metadata info for a joint on a body. This includes joint index, name, type, q-index and u-index."}, From fd88d73e3c0d064ee0815640deb2297954bcf4f4 Mon Sep 17 00:00:00 2001 From: David Date: Wed, 15 Jun 2016 01:36:10 +0100 Subject: [PATCH 24/53] Typos in readme --- README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 711ebcf98..942500cb9 100644 --- a/README.md +++ b/README.md @@ -34,7 +34,7 @@ The entire collision detection and rigid body dynamics is executed on the GPU. A high-end desktop GPU, such as an AMD Radeon 7970 or NVIDIA GTX 680 or better. We succesfully tested the software under Windows, Linux and Mac OSX. The software currently doesn't work on OpenCL CPU devices. It might run -on a laptop GPU but performance is likely not very good. Note that +on a laptop GPU but performance will not likely be very good. Note that often an OpenCL drivers fails to compile a kernel. Some unit tests exist to track down the issue, but more work is required to cover all OpenCL kernels. @@ -88,7 +88,7 @@ You can just run it though a terminal/command prompt, or by clicking it. [--fixed_timestep= 0.0] Use either a real-time delta time (0.0) or a fixed step size (0.016666) ``` -You can use mouse picking to grab objects. When holding the ALT of CONTROL key, you have Maya style camera mouse controls. -Press F1 to create series of screenshot. Hit ESCAPE to exit the demo app. +You can use mouse picking to grab objects. When holding the ALT or CONTROL key, you have Maya style camera mouse controls. +Press F1 to create a series of screenshots. Hit ESCAPE to exit the demo app. See docs folder for further information. From 456c844a6b9d243f211971471c0a2970ef207935 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 14 Jun 2016 18:41:19 -0700 Subject: [PATCH 25/53] work-in-progress send object/joint information after loading SDF file --- .../SharedMemory/PhysicsClientExample.cpp | 28 ++++- examples/SharedMemory/PhysicsDirect.cpp | 102 +++++++++++------- examples/SharedMemory/PhysicsDirect.h | 2 + .../PhysicsServerCommandProcessor.cpp | 56 ++++++++-- .../PhysicsServerCommandProcessor.h | 2 + examples/SharedMemory/SharedMemoryCommands.h | 2 +- examples/SharedMemory/SharedMemoryPublic.h | 4 +- test/SharedMemory/test.c | 19 +++- 8 files changed, 169 insertions(+), 46 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index acce1dfde..691dbb9c6 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -237,7 +237,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) case CMD_LOAD_SDF: { - b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "two_cubes.sdf"); + b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model.sdf"); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } @@ -656,6 +656,32 @@ void PhysicsClientExample::stepSimulation(float deltaTime) if (statusType == CMD_SDF_LOADING_COMPLETED) { + int bodyIndicesOut[1024]; + int bodyCapacity = 1024; + int numBodies = b3GetStatusBodyIndices(status, bodyIndicesOut, bodyCapacity); + if (numBodies > bodyCapacity) + { + b3Warning("loadSDF number of bodies (%d) exceeds the internal body capacity (%d)",numBodies, bodyCapacity); + } else + { + /* + for (int i=0;i=0) { diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 478e31031..36212cc8c 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -248,6 +248,47 @@ bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand) } + +void PhysicsDirect::processBodyJointInfo(int bodyUniqueId, const SharedMemoryStatus& serverCmd) +{ + bParse::btBulletFile bf( + &m_data->m_bulletStreamDataServerToClient[0], + serverCmd.m_dataStreamArguments.m_streamChunkLength); + bf.setFileDNAisMemoryDNA(); + bf.parse(false); + + + BodyJointInfoCache2* bodyJoints = new BodyJointInfoCache2; + m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints); + + for (int i = 0; i < bf.m_multiBodies.size(); i++) + { + int flag = bf.getFlags(); + if ((flag & bParse::FD_DOUBLE_PRECISION) != 0) + { + Bullet::btMultiBodyDoubleData* mb = + (Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i]; + + addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput); + } else + { + Bullet::btMultiBodyFloatData* mb = + (Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i]; + + addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput); + } + } + if (bf.ok()) { + if (m_data->m_verboseOutput) + { + b3Printf("Received robot description ok!\n"); + } + } else + { + b3Warning("Robot description not received"); + } +} + bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& command) { if (command.m_type==CMD_REQUEST_DEBUG_LINES) @@ -296,46 +337,35 @@ bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& comman break; } + case CMD_SDF_LOADING_COMPLETED: + { + //we'll stream further info from the physics server + //so serverCmd will be invalid, make a copy + + + int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies; + for (int i=0;im_commandProcessor->processCommand(infoRequestCommand,infoStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + if (hasStatus) + { + processBodyJointInfo(bodyUniqueId, infoStatus); + } + } + break; + } case CMD_URDF_LOADING_COMPLETED: { + if (serverCmd.m_dataStreamArguments.m_streamChunkLength > 0) { - bParse::btBulletFile bf( - &m_data->m_bulletStreamDataServerToClient[0], - serverCmd.m_dataStreamArguments.m_streamChunkLength); - bf.setFileDNAisMemoryDNA(); - bf.parse(false); - int bodyIndex = serverCmd.m_dataStreamArguments.m_bodyUniqueId; - - BodyJointInfoCache2* bodyJoints = new BodyJointInfoCache2; - m_data->m_bodyJointMap.insert(bodyIndex,bodyJoints); - - for (int i = 0; i < bf.m_multiBodies.size(); i++) - { - int flag = bf.getFlags(); - if ((flag & bParse::FD_DOUBLE_PRECISION) != 0) - { - Bullet::btMultiBodyDoubleData* mb = - (Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i]; - - addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput); - } else - { - Bullet::btMultiBodyFloatData* mb = - (Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i]; - - addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput); - } - } - if (bf.ok()) { - if (m_data->m_verboseOutput) - { - b3Printf("Received robot description ok!\n"); - } - } else - { - b3Warning("Robot description not received"); - } + int bodyIndex = serverCmd.m_dataStreamArguments.m_bodyUniqueId; + processBodyJointInfo(bodyIndex,serverCmd); } break; } diff --git a/examples/SharedMemory/PhysicsDirect.h b/examples/SharedMemory/PhysicsDirect.h index 9dea925d3..1e9ebbcf3 100644 --- a/examples/SharedMemory/PhysicsDirect.h +++ b/examples/SharedMemory/PhysicsDirect.h @@ -21,6 +21,8 @@ protected: bool processCamera(const struct SharedMemoryCommand& orgCommand); + void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd); + public: PhysicsDirect(); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 98994ea68..e3a9b7f48 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -762,12 +762,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe createJointMotors(mb); - //serialize the btMultiBody and send the data to the client. This is one way to get the link/joint names across the (shared memory) wire - - UrdfLinkNameMapUtil* util2 = new UrdfLinkNameMapUtil; - m_data->m_urdfLinkNameMapper.push_back(util2); - util2->m_mb = mb; - util2->m_memSerializer = 0; + //disable serialization of the collision objects (they are too big, and the client likely doesn't need them); bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks()); @@ -963,7 +958,44 @@ void PhysicsServerCommandProcessor::replayLogCommand(char* bufferServerToClient, } } +int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes) +{ + int streamSizeInBytes = 0; + //serialize the btMultiBody and send the data to the client. This is one way to get the link/joint names across the (shared memory) wire + InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId); + btMultiBody* mb = bodyHandle->m_multiBody; + if (mb) + { + UrdfLinkNameMapUtil* util = new UrdfLinkNameMapUtil; + m_data->m_urdfLinkNameMapper.push_back(util); + util->m_mb = mb; + util->m_memSerializer = new btDefaultSerializer(bufferSizeInBytes ,(unsigned char*)bufferServerToClient); + //disable serialization of the collision objects (they are too big, and the client likely doesn't need them); + util->m_memSerializer->m_skipPointers.insert(mb->getBaseCollider(),0); + + bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks()); + for (int i=0;igetNumLinks();i++) + { + //disable serialization of the collision objects + util->m_memSerializer->m_skipPointers.insert(mb->getLink(i).m_collider,0); + util->m_memSerializer->registerNameForPointer(mb->getLink(i).m_linkName,mb->getLink(i).m_linkName); + util->m_memSerializer->registerNameForPointer(mb->getLink(i).m_jointName,mb->getLink(i).m_jointName); + } + + util->m_memSerializer->registerNameForPointer(mb->getBaseName(),mb->getBaseName()); + + util->m_memSerializer->insertHeader(); + + int len = mb->calculateSerializeBufferSize(); + btChunk* chunk = util->m_memSerializer->allocate(len,1); + const char* structType = mb->serialize(chunk->m_oldPtr, util->m_memSerializer); + util->m_memSerializer->finalizeChunk(chunk,structType,BT_MULTIBODY_CODE,mb); + streamSizeInBytes = util->m_memSerializer->getCurrentBufferSize(); + + } + return streamSizeInBytes; +} bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes ) { @@ -1171,6 +1203,18 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm break; } + case CMD_REQUEST_BODY_INFO: + { + const SdfRequestInfoArgs& sdfInfoArgs = clientCmd.m_sdfRequestInfoArgs; + //stream info into memory + int streamSizeInBytes = createBodyInfoStream(sdfInfoArgs.m_bodyUniqueId, bufferServerToClient, bufferSizeInBytes); + + serverStatusOut.m_type = CMD_BODY_INFO_COMPLETED; + serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = sdfInfoArgs.m_bodyUniqueId; + serverStatusOut.m_dataStreamArguments.m_streamChunkLength = streamSizeInBytes; + hasStatus = true; + break; + } case CMD_LOAD_SDF: { const SdfArgs& sdfArgs = clientCmd.m_sdfArguments; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index 242a41176..bc74a0a27 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -27,6 +27,8 @@ protected: bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes); bool supportsJointMotor(class btMultiBody* body, int linkIndex); + + int createBodyInfoStream(int bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes); public: PhysicsServerCommandProcessor(); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index a4c93180e..83fd91c34 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -312,7 +312,7 @@ struct SdfLoadedArgs struct SdfRequestInfoArgs { - int m_infoIndex; + int m_bodyUniqueId; }; enum EnumSdfRequestInfoFlags diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index a038ecf51..5e6da5c50 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -19,7 +19,7 @@ enum EnumSharedMemoryClientCommand CMD_SEND_DESIRED_STATE,//todo: reconsider naming, for example SET_JOINT_CONTROL_VARIABLE? CMD_REQUEST_ACTUAL_STATE, CMD_REQUEST_DEBUG_LINES, - CMD_REQUEST_SDF_INFO, + CMD_REQUEST_BODY_INFO, CMD_STEP_FORWARD_SIMULATION, CMD_RESET_SIMULATION, CMD_PICK_BODY, @@ -55,6 +55,8 @@ enum EnumSharedMemoryServerStatus CMD_RESET_SIMULATION_COMPLETED, CMD_CAMERA_IMAGE_COMPLETED, CMD_CAMERA_IMAGE_FAILED, + CMD_BODY_INFO_COMPLETED, + CMD_BODY_INFO_FAILED, CMD_INVALID_STATUS, CMD_MAX_SERVER_COMMANDS }; diff --git a/test/SharedMemory/test.c b/test/SharedMemory/test.c index c6417b802..f6b95f37e 100644 --- a/test/SharedMemory/test.c +++ b/test/SharedMemory/test.c @@ -35,6 +35,7 @@ void testSharedMemory(b3PhysicsClientHandle sm) int sensorJointIndexLeft=-1; int sensorJointIndexRight=-1; const char* urdfFileName = "r2d2.urdf"; + const char* sdfFileName = "kuka_iiwa/model.sdf"; double gravx=0, gravy=0, gravz=-9.8; double timeStep = 1./60.; double startPosX, startPosY,startPosZ; @@ -52,7 +53,23 @@ void testSharedMemory(b3PhysicsClientHandle sm) ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); } - + { + b3SharedMemoryStatusHandle statusHandle; + int statusType; + int bodyIndicesOut[10];//MAX_SDF_BODIES = 10 + int numJoints, numBodies; + b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(sm, sdfFileName); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + ASSERT_EQ(statusType, CMD_SDF_LOADING_COMPLETED); + + numBodies = b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, 10); + ASSERT_EQ(numBodies,1); + numJoints = b3GetNumJoints(sm,bodyIndicesOut[0]); + printf("numJoints: %d\n", numJoints); + //ASSERT_EQ(numBodies ==1); + } + { b3SharedMemoryStatusHandle statusHandle; int statusType; From 03fded3dc7b6d730c7fcaad083115689387f9837 Mon Sep 17 00:00:00 2001 From: Jasmine Hsu Date: Wed, 15 Jun 2016 14:21:04 -0700 Subject: [PATCH 26/53] getJointPositions returns a list of all joint positions for a given bodyIndex --- examples/pybullet/pybullet.c | 180 +++++++++++++++++++++++++++-------- 1 file changed, 142 insertions(+), 38 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index b8bf684c2..17c092b2d 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -121,7 +121,7 @@ pybullet_disconnectPhysicsServer(PyObject *self, PyObject *args) // Load a URDF file indicating the links and joints of an object // function can be called without arguments and will default // to position (0,0,1) with orientation(0,0,0,1) -// else, loadURDF(x,y,z) or +// els(x,y,z) or // loadURDF(pos_x, pos_y, pos_z, orn_x, orn_y, orn_z, orn_w) static PyObject * pybullet_loadURDF(PyObject* self, PyObject* args) @@ -337,6 +337,7 @@ static void pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double baseOrientation[1] = actualStateQ[4]; baseOrientation[2] = actualStateQ[5]; baseOrientation[3] = actualStateQ[6]; + } } } @@ -451,52 +452,146 @@ pybullet_initializeJointPositions(PyObject* self, PyObject* args) return Py_None; } + +// CURRENTLY NOT SUPPORTED // Initalize a single joint position for a specific body index // // This method skips any physics simulation and // teleports all joints to the new positions. -// TODO(hellojas): test accuracy of joint position values -static PyObject* -pybullet_initializeJointPosition(PyObject* self, PyObject* args) -{ - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } +// TODO(hellojas): initializing one joint currently not supported +// static PyObject* +// pybullet_initializeJointPosition(PyObject* self, PyObject* args) +// { +// if (0==sm) +// { +// PyErr_SetString(SpamError, "Not connected to physics server."); +// return NULL; +// } +// +// int i; +// int bodyIndex = -1; +// int jointIndex = -1; +// double jointPos = 0.0; +// +// int size= PySequence_Size(args); +// +// if (size==3) // get body index, joint index, and joint position value +// { +// if (PyArg_ParseTuple(args, "iid", &bodyIndex, &jointIndex, &jointPos)) +// { +// b3SharedMemoryCommandHandle cmd_handle = b3CreatePoseCommandInit(sm, bodyIndex); +// +// // printf("initializing joint %d at %f\n", jointIndex, jointPos); +// b3CreatePoseCommandSetJointPosition(sm, cmd_handle, jointIndex, jointPos); +// +// b3SharedMemoryStatusHandle status_handle = +// b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); +// +// const int status_type = b3GetStatusType(status_handle); +// +// } +// } +// +// Py_INCREF(Py_None); +// return Py_None; +// } + +static void pybullet_internalGetJointPositions(int bodyIndex, int numJoints, double jointPositions[]) { + int i, j; + int numDegreeQ; + int numDegreeU; + int arrSizeOfPosAndOrn = 7; - int i; - int bodyIndex = -1; - int jointIndex = -1; - double jointPos = 0.0; - - int size= PySequence_Size(args); - - if (size==3) // get body index, joint index, and joint position value - { - if (PyArg_ParseTuple(args, "iid", &bodyIndex, &jointIndex, &jointPos)) - { - b3SharedMemoryCommandHandle cmd_handle = b3CreatePoseCommandInit(sm, bodyIndex); - - // printf("initializing joint %d at %f\n", jointIndex, jointPos); - b3CreatePoseCommandSetJointPosition(sm, cmd_handle, jointIndex, jointPos); - - b3SharedMemoryStatusHandle status_handle = - b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); - - const int status_type = b3GetStatusType(status_handle); - - } + for (i =0;i Date: Wed, 15 Jun 2016 18:01:41 -0700 Subject: [PATCH 27/53] implement getJointInfo for objects loaded through SDF --- examples/SharedMemory/PhysicsClient.h | 2 +- examples/SharedMemory/PhysicsClientC_API.cpp | 4 +- examples/SharedMemory/PhysicsClientC_API.h | 2 +- .../PhysicsClientSharedMemory.cpp | 99 +++++++++++++++++-- .../SharedMemory/PhysicsClientSharedMemory.h | 4 +- examples/SharedMemory/PhysicsDirect.cpp | 9 +- examples/SharedMemory/PhysicsDirect.h | 2 +- examples/SharedMemory/PhysicsLoopBack.cpp | 4 +- examples/SharedMemory/PhysicsLoopBack.h | 2 +- test/SharedMemory/test.c | 14 ++- 10 files changed, 122 insertions(+), 20 deletions(-) diff --git a/examples/SharedMemory/PhysicsClient.h b/examples/SharedMemory/PhysicsClient.h index 59a1a4e90..07b10aa13 100644 --- a/examples/SharedMemory/PhysicsClient.h +++ b/examples/SharedMemory/PhysicsClient.h @@ -26,7 +26,7 @@ public: virtual int getNumJoints(int bodyIndex) const = 0; - virtual void getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const = 0; + virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const = 0; virtual void setSharedMemoryKey(int key) = 0; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 81a58a992..a627e08af 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -653,10 +653,10 @@ int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyId) } -void b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info) +int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info) { PhysicsClient* cl = (PhysicsClient* ) physClient; - cl->getJointInfo(bodyIndex, linkIndex,*info); + return cl->getJointInfo(bodyIndex, linkIndex,*info); } b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX, diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 468347ef5..4ae1a5235 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -57,7 +57,7 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle, int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex); ///given a body and link index, return the joint information. See b3JointInfo in SharedMemoryPublic.h -void b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info); +int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info); ///Request debug lines for debug visualization. The flags in debugMode are the same as used in Bullet ///See btIDebugDraw::DebugDrawModes in Bullet/src/LinearMath/btIDebugDraw.h diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index a4c57574d..3ca2aa5c5 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -37,6 +37,9 @@ struct PhysicsClientSharedMemoryInternalData { btAlignedObjectArray m_cachedCameraPixelsRGBA; btAlignedObjectArray m_cachedCameraDepthBuffer; + btAlignedObjectArray m_bodyIdsRequestInfo; + SharedMemoryStatus m_tempBackupServerStatus; + SharedMemoryStatus m_lastServerStatus; int m_counter; @@ -82,15 +85,16 @@ int PhysicsClientSharedMemory::getNumJoints(int bodyIndex) const } -void PhysicsClientSharedMemory::getJointInfo(int bodyIndex, int jointIndex, b3JointInfo& info) const +bool PhysicsClientSharedMemory::getJointInfo(int bodyIndex, int jointIndex, b3JointInfo& info) const { BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex]; if (bodyJointsPtr && *bodyJointsPtr) { BodyJointInfoCache* bodyJoints = *bodyJointsPtr; info = bodyJoints->m_jointInfo[jointIndex]; + return true; } - + return false; } PhysicsClientSharedMemory::PhysicsClientSharedMemory() @@ -167,6 +171,48 @@ bool PhysicsClientSharedMemory::connect() { return true; } + +///todo(erwincoumans) refactor this: merge with PhysicsDirect::processBodyJointInfo +void PhysicsClientSharedMemory::processBodyJointInfo(int bodyUniqueId, const SharedMemoryStatus& serverCmd) +{ + bParse::btBulletFile bf( + &this->m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0], + serverCmd.m_dataStreamArguments.m_streamChunkLength); + bf.setFileDNAisMemoryDNA(); + bf.parse(false); + + + BodyJointInfoCache* bodyJoints = new BodyJointInfoCache; + m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints); + + for (int i = 0; i < bf.m_multiBodies.size(); i++) + { + int flag = bf.getFlags(); + if ((flag & bParse::FD_DOUBLE_PRECISION) != 0) + { + Bullet::btMultiBodyDoubleData* mb = + (Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i]; + + addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput); + } else + { + Bullet::btMultiBodyFloatData* mb = + (Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i]; + + addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput); + } + } + if (bf.ok()) { + if (m_data->m_verboseOutput) + { + b3Printf("Received robot description ok!\n"); + } + } else + { + b3Warning("Robot description not received"); + } +} + const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { SharedMemoryStatus* stat = 0; @@ -208,6 +254,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { if (m_data->m_verboseOutput) { b3Printf("Server loading the SDF OK\n"); } + break; } case CMD_URDF_LOADING_COMPLETED: { @@ -275,6 +322,16 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { break; } + case CMD_BODY_INFO_COMPLETED: + { + if (m_data->m_verboseOutput) { + b3Printf("Received body info\n"); + } + int bodyUniqueId = serverCmd.m_dataStreamArguments.m_bodyUniqueId; + processBodyJointInfo(bodyUniqueId, serverCmd); + + break; + } case CMD_SDF_LOADING_FAILED: { if (m_data->m_verboseOutput) { b3Printf("Server failed loading the SDF...\n"); @@ -504,21 +561,49 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { m_data->m_waitingForServer = true; } - /*if (serverCmd.m_type == CMD_SDF_LOADING_COMPLETED) + if (serverCmd.m_type == CMD_SDF_LOADING_COMPLETED) { int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies; if (numBodies>0) { + m_data->m_tempBackupServerStatus = m_data->m_lastServerStatus; + + for (int i=0;im_bodyIdsRequestInfo.push_back(serverCmd.m_sdfLoadedArgs.m_bodyUniqueIds[i]); + } + + int bodyId = m_data->m_bodyIdsRequestInfo[m_data->m_bodyIdsRequestInfo.size()-1]; + m_data->m_bodyIdsRequestInfo.pop_back(); + SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; //now transfer the information of the individual objects etc. - command.m_type = CMD_REQUEST_SDF_INFO; - command.m_updateFlags = SDF_REQUEST_INFO_BODY; - command.m_sdfRequestInfoArgs.m_infoIndex = 0; + command.m_type = CMD_REQUEST_BODY_INFO; + command.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyId; submitClientCommand(command); return 0; } } - */ + + if (serverCmd.m_type == CMD_BODY_INFO_COMPLETED) + { + //are there any bodies left to be processed? + if (m_data->m_bodyIdsRequestInfo.size()) + { + int bodyId = m_data->m_bodyIdsRequestInfo[m_data->m_bodyIdsRequestInfo.size()-1]; + m_data->m_bodyIdsRequestInfo.pop_back(); + + SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; + //now transfer the information of the individual objects etc. + command.m_type = CMD_REQUEST_BODY_INFO; + command.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyId; + submitClientCommand(command); + return 0; + } else + { + m_data->m_lastServerStatus = m_data->m_tempBackupServerStatus; + } + } if (serverCmd.m_type == CMD_CAMERA_IMAGE_COMPLETED) { diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.h b/examples/SharedMemory/PhysicsClientSharedMemory.h index 1081771e5..a47fc551a 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.h +++ b/examples/SharedMemory/PhysicsClientSharedMemory.h @@ -11,7 +11,9 @@ class PhysicsClientSharedMemory : public PhysicsClient { protected: virtual void setSharedMemoryInterface(class SharedMemoryInterface* sharedMem); + void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd); + public: PhysicsClientSharedMemory(); virtual ~PhysicsClientSharedMemory(); @@ -34,7 +36,7 @@ public: virtual int getNumJoints(int bodyIndex) const; - virtual void getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const; + virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const; virtual void setSharedMemoryKey(int key); diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 36212cc8c..511290778 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -393,14 +393,19 @@ int PhysicsDirect::getNumJoints(int bodyIndex) const return 0; } -void PhysicsDirect::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const +bool PhysicsDirect::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const { BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex]; if (bodyJointsPtr && *bodyJointsPtr) { BodyJointInfoCache2* bodyJoints = *bodyJointsPtr; - info = bodyJoints->m_jointInfo[jointIndex]; + if (jointIndex < bodyJoints->m_jointInfo.size()) + { + info = bodyJoints->m_jointInfo[jointIndex]; + return true; + } } + return false; } ///todo: move this out of the diff --git a/examples/SharedMemory/PhysicsDirect.h b/examples/SharedMemory/PhysicsDirect.h index 1e9ebbcf3..aaa2d8639 100644 --- a/examples/SharedMemory/PhysicsDirect.h +++ b/examples/SharedMemory/PhysicsDirect.h @@ -48,7 +48,7 @@ public: virtual int getNumJoints(int bodyIndex) const; - virtual void getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const; + virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const; ///todo: move this out of the virtual void setSharedMemoryKey(int key); diff --git a/examples/SharedMemory/PhysicsLoopBack.cpp b/examples/SharedMemory/PhysicsLoopBack.cpp index 985352113..ada37ce88 100644 --- a/examples/SharedMemory/PhysicsLoopBack.cpp +++ b/examples/SharedMemory/PhysicsLoopBack.cpp @@ -79,9 +79,9 @@ int PhysicsLoopBack::getNumJoints(int bodyIndex) const return m_data->m_physicsClient->getNumJoints(bodyIndex); } -void PhysicsLoopBack::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const +bool PhysicsLoopBack::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const { - m_data->m_physicsClient->getJointInfo(bodyIndex,jointIndex,info); + return m_data->m_physicsClient->getJointInfo(bodyIndex,jointIndex,info); } ///todo: move this out of the diff --git a/examples/SharedMemory/PhysicsLoopBack.h b/examples/SharedMemory/PhysicsLoopBack.h index edc71ceb9..8c0e187b6 100644 --- a/examples/SharedMemory/PhysicsLoopBack.h +++ b/examples/SharedMemory/PhysicsLoopBack.h @@ -40,7 +40,7 @@ public: virtual int getNumJoints(int bodyIndex) const; - virtual void getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const; + virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const; ///todo: move this out of the virtual void setSharedMemoryKey(int key); diff --git a/test/SharedMemory/test.c b/test/SharedMemory/test.c index f6b95f37e..01d0a16a8 100644 --- a/test/SharedMemory/test.c +++ b/test/SharedMemory/test.c @@ -65,8 +65,18 @@ void testSharedMemory(b3PhysicsClientHandle sm) numBodies = b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, 10); ASSERT_EQ(numBodies,1); - numJoints = b3GetNumJoints(sm,bodyIndicesOut[0]); - printf("numJoints: %d\n", numJoints); + int bodyUniqueId = bodyIndicesOut[0]; + + numJoints = b3GetNumJoints(sm,bodyUniqueId); + b3Printf("numJoints: %d\n", numJoints); + for (i=0;i Date: Thu, 16 Jun 2016 11:48:37 -0700 Subject: [PATCH 28/53] Set view and projection matrices from camera and frustum parameters. --- examples/SharedMemory/PhysicsClientC_API.cpp | 79 ++++++++++++++++++++ examples/SharedMemory/PhysicsClientC_API.h | 2 + 2 files changed, 81 insertions(+) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index a627e08af..ae001176d 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1,6 +1,7 @@ #include "PhysicsClientC_API.h" #include "PhysicsClientSharedMemory.h" #include "Bullet3Common/b3Scalar.h" +#include "Bullet3Common/b3Vector3.h" #include #include "SharedMemoryCommands.h" @@ -766,6 +767,84 @@ void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle commandHa command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES; } +void b3RequestCameraImageSetViewMatrix(b3SharedMemoryCommandHandle commandHandle, const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3]) +{ + b3Vector3 eye = b3MakeVector3(cameraPosition[0], cameraPosition[1], cameraPosition[2]); + b3Vector3 center = b3MakeVector3(cameraTargetPosition[0], cameraTargetPosition[1], cameraTargetPosition[2]); + b3Vector3 up = b3MakeVector3(cameraUp[0], cameraUp[1], cameraUp[2]); + b3Vector3 f = (center - eye).normalized(); + b3Vector3 u = up.normalized(); + b3Vector3 s = (f.cross(u)).normalized(); + u = s.cross(f); + + float viewMatrix[16]; + + viewMatrix[0*4+0] = s.x; + viewMatrix[1*4+0] = s.y; + viewMatrix[2*4+0] = s.z; + + viewMatrix[0*4+1] = u.x; + viewMatrix[1*4+1] = u.y; + viewMatrix[2*4+1] = u.z; + + viewMatrix[0*4+2] =-f.x; + viewMatrix[1*4+2] =-f.y; + viewMatrix[2*4+2] =-f.z; + + viewMatrix[0*4+3] = 0.f; + viewMatrix[1*4+3] = 0.f; + viewMatrix[2*4+3] = 0.f; + + viewMatrix[3*4+0] = -s.dot(eye); + viewMatrix[3*4+1] = -u.dot(eye); + viewMatrix[3*4+2] = f.dot(eye); + viewMatrix[3*4+3] = 1.f; + + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA); + for (int i=0;i<16;i++) + { + command->m_requestPixelDataArguments.m_viewMatrix[i] = viewMatrix[i]; + } + command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES; + +} + +void b3RequestCameraImageSetProjectionMatrix(b3SharedMemoryCommandHandle commandHandle, float left, float right, float bottom, float top, float nearVal, float farVal) +{ + float frustum[16]; + + frustum[0*4+0] = (float(2) * nearVal) / (right - left); + frustum[0*4+1] = float(0); + frustum[0*4+2] = float(0); + frustum[0*4+3] = float(0); + + frustum[1*4+0] = float(0); + frustum[1*4+1] = (float(2) * nearVal) / (top - bottom); + frustum[1*4+2] = float(0); + frustum[1*4+3] = float(0); + + frustum[2*4+0] = (right + left) / (right - left); + frustum[2*4+1] = (top + bottom) / (top - bottom); + frustum[2*4+2] = -(farVal + nearVal) / (farVal - nearVal); + frustum[2*4+3] = float(-1); + + frustum[3*4+0] = float(0); + frustum[3*4+1] = float(0); + frustum[3*4+2] = -(float(2) * farVal * nearVal) / (farVal - nearVal); + frustum[3*4+3] = float(0); + + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA); + for (int i=0;i<16;i++) + { + command->m_requestPixelDataArguments.m_projectionMatrix[i] = frustum[i]; + } + command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES; +} + void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle commandHandle, int width, int height ) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 4ae1a5235..1bff499c5 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -70,6 +70,8 @@ void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* l ///request an image from a simulated camera, using a software renderer. b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient); void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle command, float viewMatrix[16], float projectionMatrix[16]); +void b3RequestCameraImageSetViewMatrix(b3SharedMemoryCommandHandle command, const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3]); +void b3RequestCameraImageSetProjectionMatrix(b3SharedMemoryCommandHandle command, float left, float right, float bottom, float top, float nearVal, float farVal); void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle command, int width, int height ); void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData); From 53a0772257e2b1bb144df2f3129a784502652ab4 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 16 Jun 2016 18:46:34 -0700 Subject: [PATCH 29/53] fix some issues related to controlling a robot/multibody beyond body index 0 (most testing happened with a single robot/multibody so far) preliminary pybullet.setJointControl implementation --- examples/SharedMemory/PhysicsClientC_API.cpp | 8 +- examples/SharedMemory/PhysicsClientC_API.h | 4 +- .../SharedMemory/PhysicsClientExample.cpp | 82 ++++++-- .../PhysicsServerCommandProcessor.cpp | 24 +-- examples/pybullet/pybullet.c | 195 +++++++++++++++--- .../Featherstone/btMultiBodyLink.h | 3 + test/SharedMemory/test.c | 2 +- 7 files changed, 248 insertions(+), 70 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index a627e08af..77e6060bf 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -150,7 +150,7 @@ b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHand } -b3SharedMemoryCommandHandle b3JointControlCommandInit( b3PhysicsClientHandle physClient, int controlMode) +b3SharedMemoryCommandHandle b3JointControlCommandInit( b3PhysicsClientHandle physClient, int bodyUniqueId, int controlMode) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -159,7 +159,7 @@ b3SharedMemoryCommandHandle b3JointControlCommandInit( b3PhysicsClientHandle phy b3Assert(command); command->m_type = CMD_SEND_DESIRED_STATE; command->m_sendDesiredStateCommandArgument.m_controlMode = controlMode; - command->m_sendDesiredStateCommandArgument.m_bodyUniqueId = 0; + command->m_sendDesiredStateCommandArgument.m_bodyUniqueId = bodyUniqueId; command->m_updateFlags = 0; return (b3SharedMemoryCommandHandle) command; } @@ -433,7 +433,7 @@ int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3Shar -b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient) +b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -444,7 +444,7 @@ b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle phys command->m_type = CMD_CREATE_SENSOR; command->m_updateFlags = 0; command->m_createSensorArguments.m_numJointSensorChanges = 0; - command->m_createSensorArguments.m_bodyUniqueId = 0; + command->m_createSensorArguments.m_bodyUniqueId = bodyUniqueId; return (b3SharedMemoryCommandHandle) command; } diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 4ae1a5235..3813fab88 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -95,7 +95,7 @@ b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClien ///Set joint control variables such as desired position/angle, desired velocity, ///applied joint forces, dependent on the control mode (CONTROL_MODE_VELOCITY or CONTROL_MODE_TORQUE) -b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode); +b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int controlMode); ///Only use when controlMode is CONTROL_MODE_POSITION_VELOCITY_PD int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int qIndex, double value); int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); @@ -132,7 +132,7 @@ int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3Shar ///We are currently not reading the sensor information from the URDF file, and programmatically assign sensors. ///This is rather inconsistent, to mix programmatical creation with loading from file. -b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient); +b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId); int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle commandHandle, int jointIndex, int enable); ///b3CreateSensorEnableIMUForLink is not implemented yet. ///For now, if the IMU is located in the root link, use the root world transform to mimic an IMU. diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 691dbb9c6..e0338153a 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -37,6 +37,9 @@ protected: bool m_wantsTermination; btAlignedObjectArray m_userCommandRequests; + btAlignedObjectArray m_bodyUniqueIds; + + int m_sharedMemoryKey; int m_selectedBody; int m_prevSelectedBody; @@ -66,7 +69,8 @@ protected: { if (m_guiHelper && m_guiHelper->getParameterInterface()) { - int bodyIndex = comboIndex; + int itemIndex = int(atoi(name)); + int bodyIndex = m_bodyUniqueIds[itemIndex]; if (m_selectedBody != bodyIndex) { m_selectedBody = bodyIndex; @@ -154,14 +158,20 @@ protected: void prepareControlCommand(b3SharedMemoryCommandHandle commandHandle) { + for (int i=0;i=0) + { + // b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, m_selectedBody, CONTROL_MODE_VELOCITY); + b3SharedMemoryCommandHandle command = b3JointControlCommandInit( 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; + } + break; } case CMD_RESET_SIMULATION: { @@ -471,9 +488,36 @@ void PhysicsClientExample::createButtons() createButton("Initialize Pose",CMD_INIT_POSE, isTrigger); createButton("Set gravity", CMD_SEND_PHYSICS_SIMULATION_PARAMETERS, isTrigger); + if (m_bodyUniqueIds.size()) + { + if (m_selectedBody<0) + m_selectedBody = 0; + + ComboBoxParams comboParams; + comboParams.m_comboboxId = 0; + comboParams.m_numItems = m_bodyUniqueIds.size(); + comboParams.m_startItem = m_selectedBody; + comboParams.m_callback = MyComboBoxCallback; + comboParams.m_userPointer = this; + //todo: get the real object name + + const char** blarray = new const char*[m_bodyUniqueIds.size()]; + + for (int i=0;igetParameterInterface()->registerComboBox(comboParams); + } + if (m_physicsClientHandle && m_selectedBody>=0) { + m_numMotors = 0; + int numJoints = b3GetNumJoints(m_physicsClientHandle,m_selectedBody); for (int i=0;i0) + { + m_selectedBody = bodyUniqueId; + } +/* int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyUniqueId); b3Printf("numJoints = %d", numJoints); for (int i=0;i=0) { + m_bodyUniqueIds.push_back(bodyIndex); + m_selectedBody = bodyIndex; int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex); for (int i=0;igetParameterInterface()->registerComboBox(comboParams); + } } } @@ -759,6 +800,7 @@ void PhysicsClientExample::stepSimulation(float deltaTime) { m_selectedBody = -1; m_numMotors=0; + m_bodyUniqueIds.clear(); createButtons(); b3SharedMemoryCommandHandle commandHandle = b3InitResetSimulationCommand(m_physicsClientHandle); if (m_options == eCLIENTEXAMPLE_SERVER) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index e3a9b7f48..33d7d3153 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -382,7 +382,6 @@ struct PhysicsServerCommandProcessorInternalData btAlignedObjectArray m_worldImporters; btAlignedObjectArray m_urdfLinkNameMapper; - btHashMap m_multiBodyJointMotorMap; btAlignedObjectArray m_strings; btAlignedObjectArray m_collisionShapes; @@ -560,7 +559,6 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld() } m_data->m_urdfLinkNameMapper.clear(); - m_data->m_multiBodyJointMotorMap.clear(); for (int i=0;im_strings.size();i++) { @@ -672,7 +670,7 @@ void PhysicsServerCommandProcessor::createJointMotors(btMultiBody* mb) btScalar desiredVelocity = 0.f; btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,dof,desiredVelocity,maxMotorImpulse); //motor->setMaxAppliedImpulse(0); - m_data->m_multiBodyJointMotorMap.insert(mbLinkIndex,motor); + mb->getLink(mbLinkIndex).m_userPtr = motor; m_data->m_dynamicsWorld->addMultiBodyConstraint(motor); motor->finalizeMultiDof(); @@ -1436,10 +1434,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm if (supportsJointMotor(mb,link)) { - btMultiBodyJointMotor** motorPtr = m_data->m_multiBodyJointMotorMap[link]; - if (motorPtr) + btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr; + + + if (motor) { - btMultiBodyJointMotor* motor = *motorPtr; btScalar desiredVelocity = 0.f; if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_QDOT)!=0) desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex]; @@ -1479,10 +1478,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm if (supportsJointMotor(mb,link)) { - btMultiBodyJointMotor** motorPtr = m_data->m_multiBodyJointMotorMap[link]; - if (motorPtr) + + btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr; + + if (motor) { - btMultiBodyJointMotor* motor = *motorPtr; btScalar desiredVelocity = 0.f; if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_QDOT)!=0) @@ -1644,10 +1644,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm if (supportsJointMotor(mb,l)) { - btMultiBodyJointMotor** motorPtr = m_data->m_multiBodyJointMotorMap[l]; - if (motorPtr && m_data->m_physicsDeltaTime>btScalar(0)) + btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)body->m_multiBody->getLink(l).m_userPtr; + + if (motor && m_data->m_physicsDeltaTime>btScalar(0)) { - btMultiBodyJointMotor* motor = *motorPtr; btScalar force =motor->getAppliedImpulse(0)/m_data->m_physicsDeltaTime; serverCmd.m_sendActualStateArgs.m_jointMotorForce[l] = force; diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 17c092b2d..fbd070ff9 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1,6 +1,6 @@ -#include "../SharedMemory/SharedMemoryInProcessPhysicsC_API.h" #include "../SharedMemory/PhysicsClientC_API.h" #include "../SharedMemory/PhysicsDirectC_API.h" +#include "../SharedMemory/SharedMemoryInProcessPhysicsC_API.h" #ifdef __APPLE__ @@ -16,6 +16,8 @@ enum eCONNECT_METHOD eCONNECT_SHARED_MEMORY=3, }; + + static PyObject *SpamError; static b3PhysicsClientHandle sm=0; @@ -182,6 +184,25 @@ pybullet_loadURDF(PyObject* self, PyObject* args) return PyLong_FromLong(bodyIndex); } +static float pybullet_internalGetFloatFromSequence(PyObject* seq, int index) +{ + float v = 0.f; + PyObject* item; + + if (PyList_Check(seq)) + { + item = PyList_GET_ITEM(seq, index); + v = PyFloat_AsDouble(item); + } + else + { + item = PyTuple_GET_ITEM(seq,index); + v = PyFloat_AsDouble(item); + } + return v; +} + + #define MAX_SDF_BODIES 512 static PyObject* @@ -255,6 +276,109 @@ pybullet_resetSimulation(PyObject* self, PyObject* args) return Py_None; } +static int pybullet_setJointControl(PyObject* self, PyObject* args) +{ + //todo(erwincoumans): set max forces, kp, kd + + int size; + + if (0==sm) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + size= PySequence_Size(args); + + if (size==3) + { + int bodyIndex, controlMode; + PyObject* targetValues; + if (PyArg_ParseTuple(args, "iiO", &bodyIndex, &controlMode, &targetValues)) + { + PyObject* seq; + int numJoints, len; + seq = PySequence_Fast(targetValues, "expected a sequence"); + len = PySequence_Size(targetValues); + numJoints = b3GetNumJoints(sm,bodyIndex); + b3SharedMemoryCommandHandle commandHandle; + + if (len!=numJoints) + { + PyErr_SetString(SpamError, "Number of control target values doesn't match the number of joints."); + Py_DECREF(seq); + return NULL; + } + + if ((controlMode != CONTROL_MODE_VELOCITY) && + (controlMode != CONTROL_MODE_TORQUE) && + (controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)) + { + PyErr_SetString(SpamError, "Illegral control mode."); + Py_DECREF(seq); + return NULL; + } + + commandHandle = b3JointControlCommandInit(sm, bodyIndex,controlMode); + + for (int qIndex=0;qIndex=0) || (sensorJointIndexRight>=0)) { - b3SharedMemoryCommandHandle command = b3CreateSensorCommandInit(sm); + b3SharedMemoryCommandHandle command = b3CreateSensorCommandInit(sm, bodyIndex); b3SharedMemoryStatusHandle statusHandle; if (imuLinkIndex>=0) { From 26f6618f00b640c676b8bbe842c5b44f964ad910 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 17 Jun 2016 12:07:38 -0700 Subject: [PATCH 30/53] colors for sdf --- data/kuka_iiwa/model.sdf | 8 ++- .../Importers/ImportURDFDemo/UrdfParser.cpp | 60 ++++++++++++------- 2 files changed, 45 insertions(+), 23 deletions(-) diff --git a/data/kuka_iiwa/model.sdf b/data/kuka_iiwa/model.sdf index 1b6f8320d..0df20cff7 100644 --- a/data/kuka_iiwa/model.sdf +++ b/data/kuka_iiwa/model.sdf @@ -32,6 +32,12 @@ meshes/link_0.stl + + 1 0 0 1 + 0 0 1 1 + 0.1 0.1 0.1 1 + 0 0 0 0 + @@ -407,4 +413,4 @@ - \ No newline at end of file + diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index 430593b5e..17f6ef93f 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -490,29 +490,45 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement* // Material TiXmlElement *mat = config->FirstChildElement("material"); //todo(erwincoumans) skip materials in SDF for now (due to complexity) - if (mat && !m_parseSDF) + if (mat) { - // get material name - if (!mat->Attribute("name")) - { - logger->reportError("Visual material must contain a name attribute"); - return false; - } - visual.m_materialName = mat->Attribute("name"); - - // try to parse material element in place - - TiXmlElement *t = mat->FirstChildElement("texture"); - TiXmlElement *c = mat->FirstChildElement("color"); - if (t||c) - { - if (parseMaterial(visual.m_localMaterial, mat,logger)) - { - UrdfMaterial* matPtr = new UrdfMaterial(visual.m_localMaterial); - model.m_materials.insert(matPtr->m_name.c_str(),matPtr); - visual.m_hasLocalMaterial = true; - } - } + if (m_parseSDF) + { + UrdfMaterial* matPtr = new UrdfMaterial; + matPtr->m_name = "mat"; + std::string diffuseText = mat->FirstChildElement("diffuse")->GetText(); + btVector4 rgba(1,0,0,1); + parseVector4(rgba,diffuseText); + matPtr->m_rgbaColor = rgba; + matPtr->m_textureFilename = "textureTest.png"; + model.m_materials.insert(matPtr->m_name.c_str(),matPtr); + visual.m_materialName = "mat"; + visual.m_hasLocalMaterial = true; + } + else + { + // get material name + if (!mat->Attribute("name")) + { + logger->reportError("Visual material must contain a name attribute"); + return false; + } + visual.m_materialName = mat->Attribute("name"); + + // try to parse material element in place + + TiXmlElement *t = mat->FirstChildElement("texture"); + TiXmlElement *c = mat->FirstChildElement("color"); + if (t||c) + { + if (parseMaterial(visual.m_localMaterial, mat,logger)) + { + UrdfMaterial* matPtr = new UrdfMaterial(visual.m_localMaterial); + model.m_materials.insert(matPtr->m_name.c_str(),matPtr); + visual.m_hasLocalMaterial = true; + } + } + } } return true; From d51320be2ce7632ea0b006088c252387d53e37ae Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Fri, 17 Jun 2016 15:47:11 -0700 Subject: [PATCH 31/53] Check diffuse field in sdf. --- .../Importers/ImportURDFDemo/UrdfParser.cpp | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index 17f6ef93f..0e720a7c2 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -496,14 +496,16 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement* { UrdfMaterial* matPtr = new UrdfMaterial; matPtr->m_name = "mat"; - std::string diffuseText = mat->FirstChildElement("diffuse")->GetText(); - btVector4 rgba(1,0,0,1); - parseVector4(rgba,diffuseText); - matPtr->m_rgbaColor = rgba; - matPtr->m_textureFilename = "textureTest.png"; - model.m_materials.insert(matPtr->m_name.c_str(),matPtr); - visual.m_materialName = "mat"; - visual.m_hasLocalMaterial = true; + TiXmlElement *diffuse = mat->FirstChildElement("diffuse"); + if (diffuse) { + std::string diffuseText = diffuse->GetText(); + btVector4 rgba(1,0,0,1); + parseVector4(rgba,diffuseText); + matPtr->m_rgbaColor = rgba; + model.m_materials.insert(matPtr->m_name.c_str(),matPtr); + visual.m_materialName = "mat"; + visual.m_hasLocalMaterial = true; + } } else { From e3b2b1a9693ee743e5da9df9ebd74f518681c224 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 18 Jun 2016 18:02:20 -0700 Subject: [PATCH 32/53] add texture support in SDF, URDF for both OpenGL and software renderer (TinyRenderer) --- data/sphere2.urdf | 2 +- data/two_cubes.sdf | 26 ++++---- .../ImportMeshUtility/b3ImportMeshUtility.cpp | 29 +-------- .../ImportMeshUtility/b3ImportMeshUtility.h | 1 - .../ImportObjDemo/ImportObjExample.cpp | 28 ++++++++- .../ImportURDFDemo/BulletUrdfImporter.cpp | 57 ++++++++++++++++-- .../ImportURDFDemo/ImportURDFSetup.cpp | 2 +- .../TinyRendererVisualShapeConverter.cpp | 60 +++++++++++++++---- 8 files changed, 143 insertions(+), 62 deletions(-) diff --git a/data/sphere2.urdf b/data/sphere2.urdf index 891e018cb..fb0108b3d 100644 --- a/data/sphere2.urdf +++ b/data/sphere2.urdf @@ -9,7 +9,7 @@ - + diff --git a/data/two_cubes.sdf b/data/two_cubes.sdf index 10dce545e..e19b73b43 100644 --- a/data/two_cubes.sdf +++ b/data/two_cubes.sdf @@ -122,11 +122,13 @@ - - 1 1 1 - - + + 1 1 1 + cube.obj + + + 1 1 1 1 + 1 1 1 1 0 diff --git a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp index 3f6553b81..85aeb7e21 100644 --- a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp +++ b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp @@ -1,11 +1,8 @@ #include "b3ImportMeshUtility.h" #include -#include "../../OpenGLWindow/GLInstancingRenderer.h" #include"Wavefront/tiny_obj_loader.h" -#include "../../OpenGLWindow/GLInstanceGraphicsShape.h" -#include "btBulletDynamicsCommon.h" -#include "../../OpenGLWindow/SimpleOpenGL3App.h" +#include "LinearMath/btVector3.h" #include "../ImportObjDemo/Wavefront2GLInstanceGraphicsShape.h" #include "../../Utils/b3ResourcePath.h" #include "Bullet3Common/b3FileUtils.h" @@ -88,28 +85,4 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& return false; } -int b3ImportMeshUtility::loadAndRegisterMeshFromFile(const std::string& fileName, CommonRenderInterface* renderer) -{ - int shapeId = -1; - b3ImportMeshData meshData; - if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fileName, meshData)) - { - int textureIndex = -1; - - if (meshData.m_textureImage) - { - textureIndex = renderer->registerTexture(meshData.m_textureImage,meshData.m_textureWidth,meshData.m_textureHeight); - } - - shapeId = renderer->registerShape(&meshData.m_gfxShape->m_vertices->at(0).xyzw[0], - meshData.m_gfxShape->m_numvertices, - &meshData.m_gfxShape->m_indices->at(0), - meshData.m_gfxShape->m_numIndices, - B3_GL_TRIANGLES, - textureIndex); - delete meshData.m_gfxShape; - delete meshData.m_textureImage; - } - return shapeId; -} diff --git a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.h b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.h index 96c061eaf..7bf6af841 100644 --- a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.h +++ b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.h @@ -15,7 +15,6 @@ struct b3ImportMeshData class b3ImportMeshUtility { public: -static int loadAndRegisterMeshFromFile(const std::string& fileName, class CommonRenderInterface* renderer); static bool loadAndRegisterMeshFromFileInternal(const std::string& fileName, b3ImportMeshData& meshData); diff --git a/examples/Importers/ImportObjDemo/ImportObjExample.cpp b/examples/Importers/ImportObjDemo/ImportObjExample.cpp index a8995c148..2fc11afa3 100644 --- a/examples/Importers/ImportObjDemo/ImportObjExample.cpp +++ b/examples/Importers/ImportObjDemo/ImportObjExample.cpp @@ -56,7 +56,31 @@ ImportObjSetup::~ImportObjSetup() - +int loadAndRegisterMeshFromFile2(const std::string& fileName, CommonRenderInterface* renderer) +{ + int shapeId = -1; + + b3ImportMeshData meshData; + if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fileName, meshData)) + { + int textureIndex = -1; + + if (meshData.m_textureImage) + { + textureIndex = renderer->registerTexture(meshData.m_textureImage,meshData.m_textureWidth,meshData.m_textureHeight); + } + + shapeId = renderer->registerShape(&meshData.m_gfxShape->m_vertices->at(0).xyzw[0], + meshData.m_gfxShape->m_numvertices, + &meshData.m_gfxShape->m_indices->at(0), + meshData.m_gfxShape->m_numIndices, + B3_GL_TRIANGLES, + textureIndex); + delete meshData.m_gfxShape; + delete meshData.m_textureImage; + } + return shapeId; +} @@ -77,7 +101,7 @@ void ImportObjSetup::initPhysics() btVector3 scaling(1,1,1); btVector3 color(1,1,1); - int shapeId = b3ImportMeshUtility::loadAndRegisterMeshFromFile(m_fileName, m_guiHelper->getRenderInterface()); + int shapeId = loadAndRegisterMeshFromFile2(m_fileName, m_guiHelper->getRenderInterface()); if (shapeId>=0) { //int id = diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 4838079a0..9b6eb8ae1 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -13,7 +13,7 @@ subject to the following restrictions: #include "BulletUrdfImporter.h" - +#include "../../CommonInterfaces/CommonRenderInterface.h" #include "URDFImporterInterface.h" #include "btBulletCollisionCommon.h" @@ -26,12 +26,20 @@ subject to the following restrictions: #include #include "../../Utils/b3ResourcePath.h" +#include "../ImportMeshUtility/b3ImportMeshUtility.h" #include #include #include "UrdfParser.h" +struct MyTexture +{ + int m_width; + int m_height; + unsigned char* textureData; +}; + struct BulletURDFInternalData { UrdfParser m_urdfParser; @@ -589,7 +597,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co } -static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut) +static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut, btAlignedObjectArray& texturesOut) { @@ -693,7 +701,23 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha { case FILE_OBJ: { - glmesh = LoadMeshFromObj(fullPath,visualPathPrefix); +// glmesh = LoadMeshFromObj(fullPath,visualPathPrefix); + + b3ImportMeshData meshData; + if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fullPath, meshData)) + { + + if (meshData.m_textureImage) + { + MyTexture texData; + texData.m_width = meshData.m_textureWidth; + texData.m_height = meshData.m_textureHeight; + texData.textureData = meshData.m_textureImage; + texturesOut.push_back(texData); + } + glmesh = meshData.m_gfxShape; + } + break; } @@ -914,7 +938,8 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP btAlignedObjectArray vertices; btAlignedObjectArray indices; btTransform startTrans; startTrans.setIdentity(); - + btAlignedObjectArray textures; + const UrdfModel& model = m_data->m_urdfParser.getModel(); UrdfLink* const* linkPtr = model.m_links.getAtIndex(linkIndex); if (linkPtr) @@ -934,14 +959,34 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP //printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]); m_data->m_linkColors.insert(linkIndex,mat->m_rgbaColor); } - convertURDFToVisualShapeInternal(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices); + convertURDFToVisualShapeInternal(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures); } } if (vertices.size() && indices.size()) { - graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size()); +// graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size()); + //graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size()); + + CommonRenderInterface* renderer = m_data->m_guiHelper->getRenderInterface(); + + if (renderer) + { + int textureIndex = -1; + if (textures.size()) + { + textureIndex = renderer->registerTexture(textures[0].textureData,textures[0].m_width,textures[0].m_height); + } + graphicsIndex = renderer->registerShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(),B3_GL_TRIANGLES,textureIndex); + + } + } + + //delete textures + for (int i=0;i #include #include "../Importers/ImportURDFDemo/UrdfParser.h" @@ -41,6 +41,13 @@ enum MyFileType MY_FILE_OBJ=3, }; +struct MyTexture2 +{ + unsigned char* textureData; + int m_width; + int m_height; +}; + struct TinyRendererObjectArray { btAlignedObjectArray< TinyRenderObjectData*> m_renderObjects; @@ -95,7 +102,7 @@ TinyRendererVisualShapeConverter::~TinyRendererVisualShapeConverter() -void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut) +void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut, btAlignedObjectArray& texturesOut) { @@ -201,7 +208,23 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref { case MY_FILE_OBJ: { - glmesh = LoadMeshFromObj(fullPath,visualPathPrefix); + //glmesh = LoadMeshFromObj(fullPath,visualPathPrefix); + b3ImportMeshData meshData; + if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fullPath, meshData)) + { + + if (meshData.m_textureImage) + { + MyTexture2 texData; + texData.m_width = meshData.m_textureWidth; + texData.m_height = meshData.m_textureHeight; + texData.textureData = meshData.m_textureImage; + texturesOut.push_back(texData); + } + glmesh = meshData.m_gfxShape; + } + + break; } @@ -420,11 +443,7 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colObj) { - btAlignedObjectArray vertices; - btAlignedObjectArray indices; - btTransform startTrans; startTrans.setIdentity(); - int graphicsIndex = -1; - + UrdfLink* const* linkPtr = model.m_links.getAtIndex(linkIndex); if (linkPtr) { @@ -433,6 +452,12 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const for (int v = 0; v < link->m_visualArray.size();v++) { + btAlignedObjectArray textures; + btAlignedObjectArray vertices; + btAlignedObjectArray indices; + btTransform startTrans; startTrans.setIdentity(); + int graphicsIndex = -1; + const UrdfVisual& vis = link->m_visualArray[v]; btTransform childTrans = vis.m_linkLocalFrame; btHashString matName(vis.m_materialName.c_str()); @@ -458,14 +483,29 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const btAssert(visualsPtr); TinyRendererObjectArray* visuals = *visualsPtr; - convertURDFToVisualShape(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices); + convertURDFToVisualShape(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures); if (vertices.size() && indices.size()) { TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer,m_data->m_depthBuffer); - tinyObj->registerMeshShape(&vertices[0].xyzw[0],vertices.size(),&indices[0],indices.size(),rgbaColor); + unsigned char* textureImage=0; + int textureWidth=0; + int textureHeight=0; + if (textures.size()) + { + textureImage = textures[0].textureData; + textureWidth = textures[0].m_width; + textureHeight = textures[0].m_height; + } + + tinyObj->registerMeshShape(&vertices[0].xyzw[0],vertices.size(),&indices[0],indices.size(),rgbaColor, + textureImage,textureWidth,textureHeight); visuals->m_renderObjects.push_back(tinyObj); } + for (int i=0;i Date: Sun, 19 Jun 2016 17:35:25 -0700 Subject: [PATCH 33/53] fix cmake and premake build systems, after adding texture support in SDF, in a nutshell, add the following two files: examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp examples/ThirdPartyLibs/stb_image/stb_image.cpp --- examples/ExtendedTutorials/premake4.lua | 12 ++++++++---- .../ImportMeshUtility/b3ImportMeshUtility.cpp | 4 ++-- examples/InverseDynamics/premake4.lua | 12 ++++++++---- examples/SharedMemory/premake4.lua | 3 ++- examples/pybullet/CMakeLists.txt | 4 +++- test/InverseDynamics/premake4.lua | 2 ++ test/SharedMemory/CMakeLists.txt | 3 ++- test/SharedMemory/premake4.lua | 12 +++++++++--- 8 files changed, 36 insertions(+), 16 deletions(-) diff --git a/examples/ExtendedTutorials/premake4.lua b/examples/ExtendedTutorials/premake4.lua index ea1fc507e..92ac12d45 100644 --- a/examples/ExtendedTutorials/premake4.lua +++ b/examples/ExtendedTutorials/premake4.lua @@ -38,7 +38,8 @@ files { "../Importers/ImportURDFDemo/URDF2Bullet.cpp", "../Importers/ImportURDFDemo/UrdfParser.cpp", "../Importers/ImportURDFDemo/urdfStringSplit.cpp", - + "../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", + "../ThirdPartyLibs/stb_image/stb_image.cpp", } @@ -89,7 +90,8 @@ files { "../Importers/ImportURDFDemo/URDF2Bullet.cpp", "../Importers/ImportURDFDemo/UrdfParser.cpp", "../Importers/ImportURDFDemo/urdfStringSplit.cpp", - + "../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", + "../ThirdPartyLibs/stb_image/stb_image.cpp", } if os.is("Linux") then initX11() end @@ -153,7 +155,8 @@ files { "../Importers/ImportURDFDemo/URDF2Bullet.cpp", "../Importers/ImportURDFDemo/UrdfParser.cpp", "../Importers/ImportURDFDemo/urdfStringSplit.cpp", - + "../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", + "../ThirdPartyLibs/stb_image/stb_image.cpp", } if os.is("Linux") then initX11() end @@ -213,6 +216,7 @@ files { "../Importers/ImportURDFDemo/URDF2Bullet.cpp", "../Importers/ImportURDFDemo/UrdfParser.cpp", "../Importers/ImportURDFDemo/urdfStringSplit.cpp", - + "../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", + "../ThirdPartyLibs/stb_image/stb_image.cpp", } diff --git a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp index 85aeb7e21..67b64fd26 100644 --- a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp +++ b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp @@ -1,12 +1,12 @@ #include "b3ImportMeshUtility.h" #include -#include"Wavefront/tiny_obj_loader.h" +#include"../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h" #include "LinearMath/btVector3.h" #include "../ImportObjDemo/Wavefront2GLInstanceGraphicsShape.h" #include "../../Utils/b3ResourcePath.h" #include "Bullet3Common/b3FileUtils.h" -#include "stb_image/stb_image.h" +#include "../../ThirdPartyLibs/stb_image/stb_image.h" bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& fileName, b3ImportMeshData& meshData) diff --git a/examples/InverseDynamics/premake4.lua b/examples/InverseDynamics/premake4.lua index 3871526ce..53c4fa2ce 100644 --- a/examples/InverseDynamics/premake4.lua +++ b/examples/InverseDynamics/premake4.lua @@ -38,7 +38,8 @@ files { "../Importers/ImportURDFDemo/URDF2Bullet.cpp", "../Importers/ImportURDFDemo/UrdfParser.cpp", "../Importers/ImportURDFDemo/urdfStringSplit.cpp", - + "../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", + "../ThirdPartyLibs/stb_image/stb_image.cpp", } @@ -89,7 +90,8 @@ files { "../Importers/ImportURDFDemo/URDF2Bullet.cpp", "../Importers/ImportURDFDemo/UrdfParser.cpp", "../Importers/ImportURDFDemo/urdfStringSplit.cpp", - + "../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", + "../ThirdPartyLibs/stb_image/stb_image.cpp", } if os.is("Linux") then initX11() end @@ -153,7 +155,8 @@ files { "../Importers/ImportURDFDemo/URDF2Bullet.cpp", "../Importers/ImportURDFDemo/UrdfParser.cpp", "../Importers/ImportURDFDemo/urdfStringSplit.cpp", - + "../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", + "../ThirdPartyLibs/stb_image/stb_image.cpp", } if os.is("Linux") then initX11() end @@ -211,6 +214,7 @@ files { "../Importers/ImportURDFDemo/URDF2Bullet.cpp", "../Importers/ImportURDFDemo/UrdfParser.cpp", "../Importers/ImportURDFDemo/urdfStringSplit.cpp", - + "../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", + "../ThirdPartyLibs/stb_image/stb_image.cpp", } diff --git a/examples/SharedMemory/premake4.lua b/examples/SharedMemory/premake4.lua index 0616a8865..e923b412f 100644 --- a/examples/SharedMemory/premake4.lua +++ b/examples/SharedMemory/premake4.lua @@ -78,6 +78,7 @@ files { "../ThirdPartyLibs/tinyxml/tinyxml.cpp", "../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp", "../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp", - +"../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", + "../ThirdPartyLibs/stb_image/stb_image.cpp", } diff --git a/examples/pybullet/CMakeLists.txt b/examples/pybullet/CMakeLists.txt index 672187557..5540d10de 100644 --- a/examples/pybullet/CMakeLists.txt +++ b/examples/pybullet/CMakeLists.txt @@ -49,6 +49,7 @@ SET(pybullet_SRCS ../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp ../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp ../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h + ../../examples/ThirdPartyLibs/stb_image/stb_image.cpp ../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp ../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp ../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp @@ -57,7 +58,8 @@ SET(pybullet_SRCS ../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp ../../examples/Importers/ImportURDFDemo/UrdfParser.cpp ../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp - ../../examples/MultiThreading/b3PosixThreadSupport.cpp + ../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp + ../../examples/MultiThreading/b3PosixThreadSupport.cpp ../../examples/MultiThreading/b3Win32ThreadSupport.cpp ../../examples/MultiThreading/b3ThreadSupportInterface.cpp diff --git a/test/InverseDynamics/premake4.lua b/test/InverseDynamics/premake4.lua index 498cf94d8..9d6770347 100644 --- a/test/InverseDynamics/premake4.lua +++ b/test/InverseDynamics/premake4.lua @@ -78,6 +78,8 @@ "../../examples/Importers/ImportURDFDemo/UrdfParser.h", "../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp", "../../examples/Importers/ImportURDFDemo/URDF2Bullet.h", + "../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", + "../../examples/ThirdPartyLibs/stb_image/stb_image.cpp", "../../examples/Utils/b3Clock.cpp", "../../Extras/Serialize/BulletWorldImporter/*", "../../Extras/Serialize/BulletFileLoader/*", diff --git a/test/SharedMemory/CMakeLists.txt b/test/SharedMemory/CMakeLists.txt index 868841237..d39f36df4 100644 --- a/test/SharedMemory/CMakeLists.txt +++ b/test/SharedMemory/CMakeLists.txt @@ -71,7 +71,8 @@ ENDIF() ../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp ../../examples/Importers/ImportURDFDemo/UrdfParser.cpp ../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp - + ../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp + ../../examples/ThirdPartyLibs/stb_image/stb_image.cpp ) diff --git a/test/SharedMemory/premake4.lua b/test/SharedMemory/premake4.lua index ef70b0139..952b4c98a 100644 --- a/test/SharedMemory/premake4.lua +++ b/test/SharedMemory/premake4.lua @@ -91,7 +91,9 @@ project ("Test_PhysicsServerLoopBack") "../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp", "../../examples/Importers/ImportURDFDemo/UrdfParser.cpp", "../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp", - } + "../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", + "../../examples/ThirdPartyLibs/stb_image/stb_image.cpp", + } project ("Test_PhysicsServerDirect") @@ -157,6 +159,8 @@ project ("Test_PhysicsServerLoopBack") "../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp", "../../examples/Importers/ImportURDFDemo/UrdfParser.cpp", "../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp", + "../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", + "../../examples/ThirdPartyLibs/stb_image/stb_image.cpp", } @@ -255,8 +259,10 @@ project ("Test_PhysicsServerInProcessExampleBrowser") "../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp", "../../examples/MultiThreading/b3PosixThreadSupport.cpp", "../../examples/MultiThreading/b3Win32ThreadSupport.cpp", - "../../examples/MultiThreading/b3ThreadSupportInterface.cpp", - } + "../../examples/MultiThreading/b3ThreadSupportInterface.cpp", + "../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", + "../../examples/ThirdPartyLibs/stb_image/stb_image.cpp", + } if os.is("Linux") then initX11() end From 251fab0d75cd403ce5907b0f6b50006b8a2facc5 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 20 Jun 2016 12:40:08 -0700 Subject: [PATCH 34/53] expand b3ResourcePath automagic resource localization capability. --- examples/Utils/b3ResourcePath.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/examples/Utils/b3ResourcePath.cpp b/examples/Utils/b3ResourcePath.cpp index d27b61e99..fdd546d5e 100644 --- a/examples/Utils/b3ResourcePath.cpp +++ b/examples/Utils/b3ResourcePath.cpp @@ -77,6 +77,12 @@ int b3ResourcePath::findResourcePath(const char* resourceName, char* resourcePat { return strlen(resourcePath); } + sprintf(resourcePath,"%s.runfiles/google3/third_party/bullet/data/%s",exePath,resourceName); + //printf("try resource at %s\n", resourcePath); + if (b3FileUtils::findFile(resourcePath, resourcePath, resourcePathMaxNumBytes)) + { + return strlen(resourcePath); + } } } From c7c01e6b325bf2c9a20c96da3925b861309af737 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 20 Jun 2016 14:09:37 -0700 Subject: [PATCH 35/53] add one more test, related to b3CreatePoseCommandInit --- test/SharedMemory/test.c | 22 +++++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) diff --git a/test/SharedMemory/test.c b/test/SharedMemory/test.c index 9d80eed63..a74cb34a6 100644 --- a/test/SharedMemory/test.c +++ b/test/SharedMemory/test.c @@ -68,6 +68,9 @@ void testSharedMemory(b3PhysicsClientHandle sm) int bodyUniqueId = bodyIndicesOut[0]; numJoints = b3GetNumJoints(sm,bodyUniqueId); + ASSERT_EQ(numJoints,7); + +#if 0 b3Printf("numJoints: %d\n", numJoints); for (i=0;i Date: Mon, 20 Jun 2016 14:58:56 -0700 Subject: [PATCH 36/53] fix C99 issue, use malloc, not variable sized array. . --- examples/pybullet/pybullet.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index fbd070ff9..eb1e88f6d 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -707,7 +707,7 @@ pybullet_getJointPositions(PyObject* self, PyObject* args) int i; int numJoints = b3GetNumJoints(sm,bodyIndex); - double jointPositions[numJoints]; + double* jointPositions = malloc(numJoints*sizeof(double)); pyListJointPos = PyTuple_New(numJoints); @@ -718,7 +718,7 @@ pybullet_getJointPositions(PyObject* self, PyObject* args) item = PyFloat_FromDouble(jointPositions[i]); PyTuple_SetItem(pyListJointPos, i, item); } - + free(jointPositions); return pyListJointPos; } From 01cad7c2a5e5a3a14e78bede0e8108b50cd6c99b Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Mon, 20 Jun 2016 15:00:35 -0700 Subject: [PATCH 37/53] fix return type in pybullet . --- examples/pybullet/pybullet.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index eb1e88f6d..3731f4c4d 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -276,7 +276,7 @@ pybullet_resetSimulation(PyObject* self, PyObject* args) return Py_None; } -static int pybullet_setJointControl(PyObject* self, PyObject* args) +static PyObject* pybullet_setJointControl(PyObject* self, PyObject* args) { //todo(erwincoumans): set max forces, kp, kd From 51da3863452f6bfe967ab9a24afe3104f2e8fffe Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Mon, 20 Jun 2016 15:05:14 -0700 Subject: [PATCH 38/53] also compile pybullet in one of the tests --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index bb6dcde2e..8d309af7d 100644 --- a/.travis.yml +++ b/.travis.yml @@ -8,7 +8,7 @@ compiler: script: - echo "CXX="$CXX - echo "CC="$CC - - cmake . -G "Unix Makefiles" #-DCMAKE_CXX_FLAGS=-Werror + - cmake . -G -DBUILD_PYBULLET=ON "Unix Makefiles" #-DCMAKE_CXX_FLAGS=-Werror - make -j8 - ctest -j8 --output-on-failure # Build again with double precision From 07ee9673efe9886d87c6b39131af355d39795de1 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Tue, 21 Jun 2016 08:43:04 -0700 Subject: [PATCH 39/53] try to get python dependency in travis (pybullet) --- .travis.yml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.travis.yml b/.travis.yml index 8d309af7d..df480e393 100644 --- a/.travis.yml +++ b/.travis.yml @@ -5,6 +5,9 @@ os: compiler: - gcc - clang +python: + - "2.7" + - "3.4" script: - echo "CXX="$CXX - echo "CC="$CC From 08b88c445a2c03dcd1db5e7f547488cfafa93d07 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Tue, 21 Jun 2016 08:51:43 -0700 Subject: [PATCH 40/53] more travis/python testing fun --- .travis.yml | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/.travis.yml b/.travis.yml index df480e393..25053a90d 100644 --- a/.travis.yml +++ b/.travis.yml @@ -5,13 +5,14 @@ os: compiler: - gcc - clang -python: - - "2.7" - - "3.4" +addons: + apt: + packages: + - python3 script: - echo "CXX="$CXX - echo "CC="$CC - - cmake . -G -DBUILD_PYBULLET=ON "Unix Makefiles" #-DCMAKE_CXX_FLAGS=-Werror + - cmake . -DBUILD_PYBULLET=ON -G"Unix Makefiles" #-DCMAKE_CXX_FLAGS=-Werror - make -j8 - ctest -j8 --output-on-failure # Build again with double precision From 0b249361c243e27430838ee188dd08d9ff0e4cf4 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Tue, 21 Jun 2016 09:01:27 -0700 Subject: [PATCH 41/53] fix a c99 issue in pybullet --- examples/pybullet/pybullet.c | 21 ++------------------- 1 file changed, 2 insertions(+), 19 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 3731f4c4d..9235f3e5a 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -302,6 +302,7 @@ static PyObject* pybullet_setJointControl(PyObject* self, PyObject* args) len = PySequence_Size(targetValues); numJoints = b3GetNumJoints(sm,bodyIndex); b3SharedMemoryCommandHandle commandHandle; + int qIndex; if (len!=numJoints) { @@ -321,7 +322,7 @@ static PyObject* pybullet_setJointControl(PyObject* self, PyObject* args) commandHandle = b3JointControlCommandInit(sm, bodyIndex,controlMode); - for (int qIndex=0;qIndex Date: Wed, 22 Jun 2016 09:59:25 -0700 Subject: [PATCH 42/53] support single-scalar scale values in URDF, even though "officially" it should be a vector3. This will load quadcopter.urdf in Drake: https://github.com/RobotLocomotion/drake/blob/master/drake/examples/Quadrotor/quadrotor.urdf --- .../Importers/ImportURDFDemo/UrdfParser.cpp | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index 0e720a7c2..a6ec0a5c4 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -393,13 +393,22 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger* } geom.m_meshFileName = shape->Attribute("filename"); - - if (shape->Attribute("scale")) + geom.m_meshScale.setValue(1,1,1); + + if (shape->Attribute("scale")) { - parseVector3(geom.m_meshScale,shape->Attribute("scale"),logger); + if (!parseVector3(geom.m_meshScale,shape->Attribute("scale"),logger)) + { + logger->reportWarning("scale should be a vector3, not single scalar. Workaround activated.\n"); + std::string scalar_str = shape->Attribute("scale"); + double scaleFactor = urdfLexicalCast(scalar_str.c_str()); + if (scaleFactor) + { + geom.m_meshScale.setValue(scaleFactor,scaleFactor,scaleFactor); + } + } } else { - geom.m_meshScale.setValue(1,1,1); } } } From f5ffb11bc55bc041312e88a3c46d5350906080be Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 23 Jun 2016 05:10:00 +0000 Subject: [PATCH 43/53] fix bus error on Raspberry Pi, unaligned float access when loading STL files fix pybullet Python 3 issue (PyString_FromString -> PyBytes_FromString and PyInt_FromLong -> PyLong_FromLong) --- examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h | 11 ++++++----- examples/pybullet/pybullet.c | 8 ++++++-- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h b/examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h index f670dfeec..65bdd330b 100644 --- a/examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h +++ b/examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h @@ -66,17 +66,18 @@ static GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName) for (int i=0;ivertex0[v]; - v1.xyzw[v] = tri->vertex1[v]; - v2.xyzw[v] = tri->vertex2[v]; - v0.normal[v] = v1.normal[v] = v2.normal[v] = tri->normal[v]; + v0.xyzw[v] = tmp.vertex0[v]; + v1.xyzw[v] = tmp.vertex1[v]; + v2.xyzw[v] = tmp.vertex2[v]; + v0.normal[v] = v1.normal[v] = v2.normal[v] = tmp.normal[v]; } v0.xyzw[3] = v1.xyzw[3] = v2.xyzw[3] = 0.f; diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 9235f3e5a..2a3545bb3 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -9,6 +9,12 @@ #include #endif + +#if PY_MAJOR_VERSION >= 3 +#define PyInt_FromLong PyLong_FromLong +#define PyString_FromString PyBytes_FromString +#endif + enum eCONNECT_METHOD { eCONNECT_GUI=1, @@ -769,7 +775,6 @@ pybullet_getJointInfo(PyObject* self, PyObject* args) // info.m_flags, // info.m_jointDamping, // info.m_jointFriction); - PyTuple_SetItem(pyListJointInfo, 0, PyInt_FromLong(info.m_jointIndex)); PyTuple_SetItem(pyListJointInfo, 1, @@ -786,7 +791,6 @@ pybullet_getJointInfo(PyObject* self, PyObject* args) PyFloat_FromDouble(info.m_jointDamping)); PyTuple_SetItem(pyListJointInfo, 7, PyFloat_FromDouble(info.m_jointFriction)); - return pyListJointInfo; } } From 8b96e2de3cb53f02c492853d44709850d92123fa Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 22 Jun 2016 23:21:47 -0700 Subject: [PATCH 44/53] a few pybullet tweaks to set desired joint motor targets (pos/vel/torque) --- examples/SharedMemory/PhysicsClientC_API.cpp | 1 + .../PhysicsServerCommandProcessor.cpp | 6 +- examples/pybullet/pybullet.c | 90 +++++++++++++------ 3 files changed, 67 insertions(+), 30 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index f5a339fc1..a952111c2 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -218,6 +218,7 @@ int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandl struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value; + command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE; return 0; } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 33d7d3153..b2c3c5c05 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1389,6 +1389,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm mb->clearForcesAndTorques(); int torqueIndex = 0; + #if 0 + //it is inconsistent to allow application of base force/torque, there is no 'joint' motor. Use a separate API for this. + btVector3 f(0,0,0); btVector3 t(0,0,0); @@ -1404,7 +1407,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm torqueIndex+=6; mb->addBaseForce(f); mb->addBaseTorque(t); - for (int link=0;linkgetNumLinks();link++) + #endif + for (int link=0;linkgetNumLinks();link++) { for (int dof=0;dofgetLink(link).m_dofCount;dof++) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 2a3545bb3..d06eff27c 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -222,7 +222,7 @@ pybullet_loadSDF(PyObject* self, PyObject* args) PyObject* pylist=0; b3SharedMemoryStatusHandle statusHandle; int statusType; - b3SharedMemoryCommandHandle command; + b3SharedMemoryCommandHandle commandHandle; if (0==sm) { @@ -236,8 +236,8 @@ pybullet_loadSDF(PyObject* self, PyObject* args) return NULL; } - command = b3LoadSdfCommandInit(sm, sdfFileName); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + commandHandle = b3LoadSdfCommandInit(sm, sdfFileName); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); if (statusType!=CMD_SDF_LOADING_COMPLETED) { @@ -308,10 +308,11 @@ static PyObject* pybullet_setJointControl(PyObject* self, PyObject* args) len = PySequence_Size(targetValues); numJoints = b3GetNumJoints(sm,bodyIndex); b3SharedMemoryCommandHandle commandHandle; - int qIndex; + b3SharedMemoryStatusHandle statusHandle; + int qIndex; if (len!=numJoints) - { + { PyErr_SetString(SpamError, "Number of control target values doesn't match the number of joints."); Py_DECREF(seq); return NULL; @@ -336,20 +337,24 @@ static PyObject* pybullet_setJointControl(PyObject* self, PyObject* args) { case CONTROL_MODE_VELOCITY: { - b3JointControlSetDesiredVelocity(commandHandle, qIndex, value); - break; + b3JointControlSetDesiredVelocity(commandHandle, 6+qIndex, value); + b3JointControlSetKd(commandHandle,6+qIndex,1); + b3JointControlSetMaximumForce(commandHandle,6+qIndex,10000); + break; } case CONTROL_MODE_TORQUE: { - b3JointControlSetDesiredForceTorque(commandHandle, qIndex, value); + b3JointControlSetDesiredForceTorque(commandHandle, 6+qIndex, value); break; } case CONTROL_MODE_POSITION_VELOCITY_PD: { - b3JointControlSetDesiredPosition( commandHandle, qIndex, value); - break; + b3JointControlSetDesiredPosition( commandHandle, 7+qIndex, value); + b3JointControlSetKp(commandHandle,6+qIndex,1); + b3JointControlSetMaximumForce(commandHandle,6+qIndex,10000); + break; } default: { @@ -357,6 +362,9 @@ static PyObject* pybullet_setJointControl(PyObject* self, PyObject* args) } }; } + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + Py_DECREF(seq); Py_INCREF(Py_None); return Py_None; @@ -554,35 +562,59 @@ pybullet_getNumJoints(PyObject* self, PyObject* args) static PyObject* pybullet_initializeJointPositions(PyObject* self, PyObject* args) { + int size; if (0==sm) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - // TODO(hellojas): initialize all joint positions given a pylist of values - // - // - /* - ///b3CreatePoseCommandInit will initialize (teleport) the pose of a body/robot. You can individually set the base position, - ///base orientation and joint angles. This will set all velocities of base and joints to zero. - ///This is not a robot control command using actuators/joint motors, but manual repositioning the robot. - b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex); - int b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ); - int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW); - int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* jointPositions); - int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition); - - */ - // - // - - Py_INCREF(Py_None); - return Py_None; + size= PySequence_Size(args); + + if (size==2) + { + int bodyIndex; + PyObject* targetValues; + if (PyArg_ParseTuple(args, "iO", &bodyIndex, &targetValues)) + { + PyObject* seq; + int numJoints, len; + seq = PySequence_Fast(targetValues, "expected a sequence"); + len = PySequence_Size(targetValues); + numJoints = b3GetNumJoints(sm,bodyIndex); + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int qIndex; + + if (len!=numJoints) + { + PyErr_SetString(SpamError, "Number of joint position values doesn't match the number of joints."); + Py_DECREF(seq); + return NULL; + } + + commandHandle = b3CreatePoseCommandInit(sm, bodyIndex); + + for (qIndex=0;qIndex Date: Thu, 23 Jun 2016 08:40:36 -0700 Subject: [PATCH 45/53] re-introduce old method in pybullet for temporary back-wards compatibility b3JointControlCommandInit requires 3 args, but it was only 2, use b3JointControlCommandInit2 for now. --- examples/SharedMemory/PhysicsClientC_API.cpp | 7 ++++++- examples/SharedMemory/PhysicsClientC_API.h | 9 +++++++-- examples/SharedMemory/PhysicsClientExample.cpp | 2 +- examples/pybullet/pybullet.c | 2 +- 4 files changed, 15 insertions(+), 5 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index a952111c2..57121f0c8 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -151,7 +151,12 @@ b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHand } -b3SharedMemoryCommandHandle b3JointControlCommandInit( b3PhysicsClientHandle physClient, int bodyUniqueId, int controlMode) +b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode) +{ + return b3JointControlCommandInit2(physClient,0,controlMode); +} + +b3SharedMemoryCommandHandle b3JointControlCommandInit2( b3PhysicsClientHandle physClient, int bodyUniqueId, int controlMode) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index e12cf75eb..0f036415c 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -95,10 +95,15 @@ int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName); +///The b3JointControlCommandInit method is obsolete, use b3JointControlCommandInit2 instead +b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode); + + ///Set joint control variables such as desired position/angle, desired velocity, ///applied joint forces, dependent on the control mode (CONTROL_MODE_VELOCITY or CONTROL_MODE_TORQUE) -b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int controlMode); -///Only use when controlMode is CONTROL_MODE_POSITION_VELOCITY_PD +b3SharedMemoryCommandHandle b3JointControlCommandInit2(b3PhysicsClientHandle physClient, int bodyUniqueId, int controlMode); + + ///Only use when controlMode is CONTROL_MODE_POSITION_VELOCITY_PD int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int qIndex, double value); int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index e0338153a..a4199071a 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -377,7 +377,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) if (m_selectedBody>=0) { // b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, m_selectedBody, CONTROL_MODE_VELOCITY); - b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, m_selectedBody, CONTROL_MODE_POSITION_VELOCITY_PD); + b3SharedMemoryCommandHandle command = b3JointControlCommandInit2( m_physicsClientHandle, m_selectedBody, CONTROL_MODE_POSITION_VELOCITY_PD); // b3Printf("prepare control command for body %d", m_selectedBody); prepareControlCommand(command); diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index d06eff27c..0d2303fc5 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -327,7 +327,7 @@ static PyObject* pybullet_setJointControl(PyObject* self, PyObject* args) return NULL; } - commandHandle = b3JointControlCommandInit(sm, bodyIndex,controlMode); + commandHandle = b3JointControlCommandInit2(sm, bodyIndex,controlMode); for (qIndex=0;qIndex Date: Thu, 23 Jun 2016 14:00:44 -0700 Subject: [PATCH 46/53] fix loading urdf at default position (0,0,0) and adding b3 cmd to change orientation if args provided --- examples/pybullet/pybullet.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 9235f3e5a..3cc230128 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -135,7 +135,7 @@ pybullet_loadURDF(PyObject* self, PyObject* args) const char* urdfFileName=""; float startPosX =0; float startPosY =0; - float startPosZ = 1; + float startPosZ = 0; float startOrnX = 0; float startOrnY = 0; float startOrnZ = 0; @@ -165,6 +165,7 @@ pybullet_loadURDF(PyObject* self, PyObject* args) return NULL; } { + printf("(%f, %f, %f) (%f, %f, %f, %f)\n", startPosX,startPosY,startPosZ,startOrnX, startOrnY,startOrnZ, startOrnW); b3SharedMemoryStatusHandle statusHandle; int statusType; @@ -172,6 +173,7 @@ pybullet_loadURDF(PyObject* self, PyObject* args) //setting the initial position, orientation and other arguments are optional b3LoadUrdfCommandSetStartPosition(command, startPosX,startPosY,startPosZ); + b3LoadUrdfCommandSetStartOrientation(command, startOrnX, startOrnY,startOrnZ, startOrnW ); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); if (statusType!=CMD_URDF_LOADING_COMPLETED) From a21889e225d8020577fc5c36dda89e9c7929b74e Mon Sep 17 00:00:00 2001 From: Jasmine Hsu Date: Thu, 23 Jun 2016 14:01:57 -0700 Subject: [PATCH 47/53] remove print debugger --- examples/pybullet/pybullet.so | 1 + 1 file changed, 1 insertion(+) create mode 120000 examples/pybullet/pybullet.so diff --git a/examples/pybullet/pybullet.so b/examples/pybullet/pybullet.so new file mode 120000 index 000000000..59de5c3df --- /dev/null +++ b/examples/pybullet/pybullet.so @@ -0,0 +1 @@ +libpybullet.dylib \ No newline at end of file From 976e228be0c3b035386168fe415188d53468ad3c Mon Sep 17 00:00:00 2001 From: Jasmine Hsu Date: Thu, 23 Jun 2016 14:05:36 -0700 Subject: [PATCH 48/53] remove print debugger --- examples/pybullet/pybullet.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 3cc230128..a8ad37b30 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -165,7 +165,7 @@ pybullet_loadURDF(PyObject* self, PyObject* args) return NULL; } { - printf("(%f, %f, %f) (%f, %f, %f, %f)\n", startPosX,startPosY,startPosZ,startOrnX, startOrnY,startOrnZ, startOrnW); + // printf("(%f, %f, %f) (%f, %f, %f, %f)\n", startPosX,startPosY,startPosZ,startOrnX, startOrnY,startOrnZ, startOrnW); b3SharedMemoryStatusHandle statusHandle; int statusType; From d2a27972015ec8915810d876afc8c072a697d350 Mon Sep 17 00:00:00 2001 From: Jasmine Hsu Date: Thu, 23 Jun 2016 14:06:14 -0700 Subject: [PATCH 49/53] accidently added pybullet.so --- examples/pybullet/pybullet.so | 1 - 1 file changed, 1 deletion(-) delete mode 120000 examples/pybullet/pybullet.so diff --git a/examples/pybullet/pybullet.so b/examples/pybullet/pybullet.so deleted file mode 120000 index 59de5c3df..000000000 --- a/examples/pybullet/pybullet.so +++ /dev/null @@ -1 +0,0 @@ -libpybullet.dylib \ No newline at end of file From 6d1948e79e5caf74cfa2e3344ee308bd81fe2c33 Mon Sep 17 00:00:00 2001 From: "Erwin Coumans (Google)" Date: Fri, 24 Jun 2016 07:31:17 -0700 Subject: [PATCH 50/53] tweaks in pybullet and shared memory C-API: allow to reset the state of a single joint allow to set the target/mode for a single joint motor at a time rename pybullet API: initializeJointPositions -> resetJointState --- examples/BasicDemo/BasicExample.cpp | 2 +- examples/SharedMemory/PhysicsClientC_API.cpp | 33 + examples/SharedMemory/PhysicsClientC_API.h | 7 +- .../PhysicsServerCommandProcessor.cpp | 579 +++++++++--------- examples/SharedMemory/SharedMemoryCommands.h | 4 + examples/pybullet/pybullet.c | 247 ++++---- test/SharedMemory/test.c | 4 +- 7 files changed, 463 insertions(+), 413 deletions(-) diff --git a/examples/BasicDemo/BasicExample.cpp b/examples/BasicDemo/BasicExample.cpp index f5bd9486c..b6365df36 100644 --- a/examples/BasicDemo/BasicExample.cpp +++ b/examples/BasicDemo/BasicExample.cpp @@ -42,7 +42,7 @@ struct BasicExample : public CommonRigidBodyBase float dist = 41; float pitch = 52; float yaw = 35; - float targetPos[3]={0,0.46,0}; + float targetPos[3]={0,0,0}; m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]); } }; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 57121f0c8..203e0ebbe 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -167,6 +167,10 @@ b3SharedMemoryCommandHandle b3JointControlCommandInit2( b3PhysicsClientHandle ph command->m_sendDesiredStateCommandArgument.m_controlMode = controlMode; command->m_sendDesiredStateCommandArgument.m_bodyUniqueId = bodyUniqueId; command->m_updateFlags = 0; + for (int i=0;im_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[i] = 0; + } return (b3SharedMemoryCommandHandle) command; } @@ -176,6 +180,7 @@ int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, b3Assert(command); command->m_sendDesiredStateCommandArgument.m_desiredStateQ[qIndex] = value; command->m_updateFlags |= SIM_DESIRED_STATE_HAS_Q; + command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[qIndex] |= SIM_DESIRED_STATE_HAS_Q; return 0; } @@ -186,6 +191,8 @@ int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, b3Assert(command); command->m_sendDesiredStateCommandArgument.m_Kp[dofIndex] = value; command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KP; + command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_KP; + return 0; } @@ -195,6 +202,7 @@ int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, b3Assert(command); command->m_sendDesiredStateCommandArgument.m_Kd[dofIndex] = value; command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KD; + command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_KD; return 0; } @@ -205,6 +213,8 @@ int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, b3Assert(command); command->m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex] = value; command->m_updateFlags |= SIM_DESIRED_STATE_HAS_QDOT; + command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_QDOT; + return 0; } @@ -215,6 +225,8 @@ int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int b3Assert(command); command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value; command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE; + command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_MAX_FORCE; + return 0; } @@ -224,6 +236,8 @@ int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandl b3Assert(command); command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value; command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE; + command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_MAX_FORCE; + return 0; } @@ -372,6 +386,7 @@ int b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHan b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex) { + PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); b3Assert(cl->canSubmitCommand()); @@ -380,6 +395,11 @@ b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physCl command->m_type = CMD_INIT_POSE; command->m_updateFlags =0; command->m_initPoseArgs.m_bodyUniqueId = bodyIndex; + //a bit slow, initialing the full range to zero... + for (int i=0;im_initPoseArgs.m_hasInitialStateQ[i] = 0; + } return (b3SharedMemoryCommandHandle) command; } @@ -392,6 +412,11 @@ int b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle command->m_initPoseArgs.m_initialStateQ[0] = startPosX; command->m_initPoseArgs.m_initialStateQ[1] = startPosY; command->m_initPoseArgs.m_initialStateQ[2] = startPosZ; + + command->m_initPoseArgs.m_hasInitialStateQ[0] = 1; + command->m_initPoseArgs.m_hasInitialStateQ[1] = 1; + command->m_initPoseArgs.m_hasInitialStateQ[2] = 1; + return 0; } @@ -405,6 +430,12 @@ int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHan command->m_initPoseArgs.m_initialStateQ[4] = startOrnY; command->m_initPoseArgs.m_initialStateQ[5] = startOrnZ; command->m_initPoseArgs.m_initialStateQ[6] = startOrnW; + + command->m_initPoseArgs.m_hasInitialStateQ[3] = 1; + command->m_initPoseArgs.m_hasInitialStateQ[4] = 1; + command->m_initPoseArgs.m_hasInitialStateQ[5] = 1; + command->m_initPoseArgs.m_hasInitialStateQ[6] = 1; + return 0; } @@ -417,6 +448,7 @@ int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHand for (int i=0;im_initPoseArgs.m_initialStateQ[i+7] = jointPositions[i]; + command->m_initPoseArgs.m_hasInitialStateQ[i+7] = 1; } return 0; } @@ -433,6 +465,7 @@ int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3Shar if ((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && info.m_qIndex >=0) { command->m_initPoseArgs.m_initialStateQ[info.m_qIndex] = jointPosition; + command->m_initPoseArgs.m_hasInitialStateQ[info.m_qIndex] = 1; } return 0; } diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 0f036415c..aee561d8f 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -99,15 +99,16 @@ b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClien b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode); -///Set joint control variables such as desired position/angle, desired velocity, +///Set joint motor control variables such as desired position/angle, desired velocity, ///applied joint forces, dependent on the control mode (CONTROL_MODE_VELOCITY or CONTROL_MODE_TORQUE) b3SharedMemoryCommandHandle b3JointControlCommandInit2(b3PhysicsClientHandle physClient, int bodyUniqueId, int controlMode); - ///Only use when controlMode is CONTROL_MODE_POSITION_VELOCITY_PD +///Only use when controlMode is CONTROL_MODE_POSITION_VELOCITY_PD int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int qIndex, double value); int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); -//Only use when controlMode is CONTROL_MODE_VELOCITY + +///Only use when controlMode is CONTROL_MODE_VELOCITY int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); /* find a better name for dof/q/u indices, point to b3JointInfo */ int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); ///Only use if when controlMode is CONTROL_MODE_TORQUE, diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index b2c3c5c05..3cdaac902 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -55,12 +55,12 @@ struct SharedMemoryDebugDrawer : public btIDebugDraw virtual void draw3dText(const btVector3& location,const char* textString) { } - + virtual void setDebugMode(int debugMode) { m_debugMode = debugMode; } - + virtual int getDebugMode() const { return m_debugMode; @@ -100,11 +100,11 @@ struct InternalBodyHandle : public InteralBodyData BT_DECLARE_ALIGNED_ALLOCATOR(); int m_nextFreeHandle; - void SetNextFree(int next) + void SetNextFree(int next) { m_nextFreeHandle = next; } - int GetNextFree() const + int GetNextFree() const { return m_nextFreeHandle; } @@ -153,7 +153,7 @@ public: struct CommandLogger { FILE* m_file; - + void writeHeader(unsigned char* buffer) const { @@ -185,15 +185,15 @@ struct CommandLogger buffer[9] = 0; buffer[10] = 0; buffer[11] = 0; - + int ver = btGetVersion(); if (ver>=0 && ver<999) { sprintf((char*)&buffer[9],"%d",ver); } - + } - + void logCommand(const SharedMemoryCommand& command) { btCommandChunk chunk; @@ -240,10 +240,10 @@ struct CommandLogPlayback } unsigned char c = m_header[7]; m_fileIs64bit = (c=='-'); - + const bool VOID_IS_8 = ((sizeof(void*)==8)); m_bitsVary = (VOID_IS_8 != m_fileIs64bit); - + } @@ -260,7 +260,7 @@ struct CommandLogPlayback if (m_file) { size_t s = 0; - + if (m_fileIs64bit) { @@ -271,7 +271,7 @@ struct CommandLogPlayback bCommandChunkPtr4 chunk4; s = fread((void*)&chunk4,sizeof(bCommandChunkPtr4),1,m_file); } - + if (s==1) { s = fread(cmd,sizeof(SharedMemoryCommand),1,m_file); @@ -324,7 +324,7 @@ struct PhysicsServerCommandProcessorInternalData { m_numUsedHandles = 0; m_firstFreeHandle = -1; - + increaseHandleCapacity(1); } @@ -349,10 +349,10 @@ struct PhysicsServerCommandProcessorInternalData int additionalCapacity= m_bodyHandles.size(); increaseHandleCapacity(additionalCapacity); - + getHandle(handle)->SetNextFree(m_firstFreeHandle); } - + return handle; } @@ -369,16 +369,16 @@ struct PhysicsServerCommandProcessorInternalData } ///end handle management - - + + CommandLogger* m_commandLogger; CommandLogPlayback* m_logPlayback; - + btScalar m_physicsDeltaTime; btAlignedObjectArray m_multiBodyJointFeedbacks; - + btAlignedObjectArray m_worldImporters; btAlignedObjectArray m_urdfLinkNameMapper; @@ -391,17 +391,17 @@ struct PhysicsServerCommandProcessorInternalData btDefaultCollisionConfiguration* m_collisionConfiguration; btMultiBodyDynamicsWorld* m_dynamicsWorld; SharedMemoryDebugDrawer* m_remoteDebugDrawer; - - btAlignedObjectArray m_sdfRecentLoadedBodies; - - + btAlignedObjectArray m_sdfRecentLoadedBodies; + + + struct GUIHelperInterface* m_guiHelper; int m_sharedMemoryKey; bool m_verboseOutput; - - + + //data for picking objects class btRigidBody* m_pickedBody; class btTypedConstraint* m_pickedConstraint; @@ -426,7 +426,7 @@ struct PhysicsServerCommandProcessorInternalData m_pickedConstraint(0), m_pickingMultiBodyPoint2Point(0) { - + initHandles(); #if 0 btAlignedObjectArray bla; @@ -478,16 +478,16 @@ struct PhysicsServerCommandProcessorInternalData void PhysicsServerCommandProcessor::setGuiHelper(struct GUIHelperInterface* guiHelper) { - + if (guiHelper) { - + guiHelper->createPhysicsDebugDrawer(m_data->m_dynamicsWorld); } else { if (m_data->m_guiHelper && m_data->m_dynamicsWorld && m_data->m_dynamicsWorld->getDebugDrawer()) { - + m_data->m_dynamicsWorld->setDebugDrawer(0); } } @@ -521,45 +521,45 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld() ///collision configuration contains default setup for memory, collision setup m_data->m_collisionConfiguration = new btDefaultCollisionConfiguration(); //m_collisionConfiguration->setConvexConvexMultipointIterations(); - + ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) m_data->m_dispatcher = new btCollisionDispatcher(m_data->m_collisionConfiguration); - + m_data->m_broadphase = new btDbvtBroadphase(); - + m_data->m_solver = new btMultiBodyConstraintSolver; - + m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration); m_data->m_remoteDebugDrawer = new SharedMemoryDebugDrawer(); - - + + m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0)); } void PhysicsServerCommandProcessor::deleteDynamicsWorld() { - + for (int i=0;im_multiBodyJointFeedbacks.size();i++) { delete m_data->m_multiBodyJointFeedbacks[i]; } m_data->m_multiBodyJointFeedbacks.clear(); - - + + for (int i=0;im_worldImporters.size();i++) { delete m_data->m_worldImporters[i]; } m_data->m_worldImporters.clear(); - + for (int i=0;im_urdfLinkNameMapper.size();i++) { delete m_data->m_urdfLinkNameMapper[i]; } m_data->m_urdfLinkNameMapper.clear(); - - + + for (int i=0;im_strings.size();i++) { delete m_data->m_strings[i]; @@ -568,11 +568,11 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld() btAlignedObjectArray constraints; btAlignedObjectArray mbconstraints; - - + + if (m_data->m_dynamicsWorld) { - + int i; for (i = m_data->m_dynamicsWorld->getNumConstraints() - 1; i >= 0; i--) { @@ -586,7 +586,7 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld() mbconstraints.push_back(mbconstraint); m_data->m_dynamicsWorld->removeMultiBodyConstraint(mbconstraint); } - + for (i = m_data->m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) { btCollisionObject* obj = m_data->m_dynamicsWorld->getCollisionObjectArray()[i]; @@ -662,7 +662,7 @@ void PhysicsServerCommandProcessor::createJointMotors(btMultiBody* mb) for (int i=0;igetLink(mbLinkIndex).m_userPtr = motor; m_data->m_dynamicsWorld->addMultiBodyConstraint(motor); motor->finalizeMultiDof(); - + } } @@ -688,11 +688,11 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe b3Error("loadSdf: No valid m_dynamicsWorld"); return false; } - + m_data->m_sdfRecentLoadedBodies.clear(); - + BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter); - + bool useFixedBase = false; bool loadOk = u2b.loadSDF(fileName, useFixedBase); if (loadOk) @@ -709,16 +709,16 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe m_data->m_collisionShapes.push_back(compound->getChildShape(childIndex)); } } - + } - + btTransform rootTrans; rootTrans.setIdentity(); if (m_data->m_verboseOutput) { b3Printf("loaded %s OK!", fileName); } - + for (int m =0; mallocHandle(); - + InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId); { @@ -737,8 +737,8 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe int urdfLinkIndex = u2b.getRootLinkIndex(); u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,bodyHandle->m_rootLocalInertialFrame); } - - + + //todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user int rootLinkIndex = u2b.getRootLinkIndex(); @@ -748,26 +748,26 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe u2b.getRootTransformInWorld(rootTrans); bool useMultiBody = true; ConvertURDF2Bullet(u2b,creation, rootTrans,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix(),true); - - - + + + mb = creation.getBulletMultiBody(); if (mb) { bodyHandle->m_multiBody = mb; - + m_data->m_sdfRecentLoadedBodies.push_back(bodyUniqueId); - + createJointMotors(mb); - + //disable serialization of the collision objects (they are too big, and the client likely doesn't need them); bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks()); for (int i=0;igetNumLinks();i++) { //disable serialization of the collision objects - + int urdfLinkIndex = creation.m_mb2urdfLink[i]; btScalar mass; btVector3 localInertiaDiagonal(0,0,0); @@ -777,12 +777,12 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str()); m_data->m_strings.push_back(linkName); - + mb->getLink(i).m_linkName = linkName->c_str(); std::string* jointName = new std::string(u2b.getJointName(urdfLinkIndex).c_str()); m_data->m_strings.push_back(jointName); - + mb->getLink(i).m_jointName = jointName->c_str(); } std::string* baseName = new std::string(u2b.getLinkName(u2b.getRootLinkIndex())); @@ -792,10 +792,10 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe { b3Warning("No multibody loaded from URDF. Could add btRigidBody+btTypedConstraint solution later."); } - + } } - return loadOk; + return loadOk; } bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn, @@ -809,13 +809,13 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto } - + BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter); - + bool loadOk = u2b.loadURDF(fileName, useFixedBase); - - + + if (loadOk) { //get a body index @@ -836,7 +836,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto { b3Printf("loaded %s OK!", fileName); } - + btTransform tr; tr.setIdentity(); tr.setOrigin(pos); @@ -844,9 +844,9 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto //int rootLinkIndex = u2b.getRootLinkIndex(); // printf("urdf root link index = %d\n",rootLinkIndex); MyMultiBodyCreator creation(m_data->m_guiHelper); - + ConvertURDF2Bullet(u2b,creation, tr,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix()); - + for (int i=0;im_collisionShapes.push_back(compound->getChildShape(childIndex)); } } - + } - + btMultiBody* mb = creation.getBulletMultiBody(); if (useMultiBody) { - - + + if (mb) { - + bodyHandle->m_multiBody = mb; - + createJointMotors(mb); @@ -905,17 +905,17 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto util->m_memSerializer->registerNameForPointer(jointName->c_str(),jointName->c_str()); mb->getLink(i).m_jointName = jointName->c_str(); } - + std::string* baseName = new std::string(u2b.getLinkName(u2b.getRootLinkIndex())); m_data->m_strings.push_back(baseName); - + util->m_memSerializer->registerNameForPointer(baseName->c_str(),baseName->c_str()); mb->setBaseName(baseName->c_str()); - - + + util->m_memSerializer->insertHeader(); - + int len = mb->calculateSerializeBufferSize(); btChunk* chunk = util->m_memSerializer->allocate(len,1); const char* structType = mb->serialize(chunk->m_oldPtr, util->m_memSerializer); @@ -927,16 +927,16 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto b3Warning("No multibody loaded from URDF. Could add btRigidBody+btTypedConstraint solution later."); return false; } - + } else { btAssert(0); - + return true; } - + } - + return false; } @@ -944,10 +944,10 @@ void PhysicsServerCommandProcessor::replayLogCommand(char* bufferServerToClient, { if (m_data->m_logPlayback) { - + SharedMemoryCommand clientCmd; SharedMemoryStatus serverStatus; - + bool hasCommand = m_data->m_logPlayback->processNextCommand(&clientCmd); if (hasCommand) { @@ -982,7 +982,7 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char* } util->m_memSerializer->registerNameForPointer(mb->getBaseName(),mb->getBaseName()); - + util->m_memSerializer->insertHeader(); int len = mb->calculateSerializeBufferSize(); @@ -1002,12 +1002,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { ///we ignore overflow of integer for now - + { - + //until we implement a proper ring buffer, we assume always maximum of 1 outstanding commands - - + + //const SharedMemoryCommand& clientCmd =m_data->m_testBlock1->m_clientCommands[0]; #if 1 if (m_data->m_commandLogger) @@ -1017,7 +1017,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm #endif //m_data->m_testBlock1->m_numProcessedClientCommands++; - + //no timestamp yet int timeStamp = 0; @@ -1031,11 +1031,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { b3Printf("Processed CMD_SEND_BULLET_DATA_STREAM length %d",clientCmd.m_dataStreamArguments.m_streamChunkLength); } - + btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld); m_data->m_worldImporters.push_back(worldImporter); bool completedOk = worldImporter->loadFileFromMemory(m_data->m_testBlock1->m_bulletStreamDataClientToServer,clientCmd.m_dataStreamArguments.m_streamChunkLength); - + if (completedOk) { SharedMemoryStatus& status = m_data->createServerStatus(CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); @@ -1046,7 +1046,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm SharedMemoryStatus& status = m_data->createServerStatus(CMD_BULLET_DATA_STREAM_RECEIVED_FAILED,clientCmd.m_sequenceNumber,timeStamp); m_data->submitServerStatus(status); } - + break; } #endif @@ -1054,7 +1054,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm case CMD_REQUEST_DEBUG_LINES: { int curFlags =m_data->m_remoteDebugDrawer->getDebugMode(); - + int debugMode = clientCmd.m_requestDebugLinesArguments.m_debugMode;//clientCmd.btIDebugDraw::DBG_DrawWireframe|btIDebugDraw::DBG_DrawAabb; int startingLineIndex = clientCmd.m_requestDebugLinesArguments.m_startingLineIndex; if (startingLineIndex<0) @@ -1062,7 +1062,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm b3Warning("startingLineIndex should be non-negative"); startingLineIndex = 0; } - + if (clientCmd.m_requestDebugLinesArguments.m_startingLineIndex==0) { m_data->m_remoteDebugDrawer->m_lines2.resize(0); @@ -1083,9 +1083,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm b3Warning("m_startingLineIndex exceeds total number of debug lines"); startingLineIndex =m_data->m_remoteDebugDrawer->m_lines2.size(); } - + int numLines = btMin(maxNumLines,m_data->m_remoteDebugDrawer->m_lines2.size()-startingLineIndex); - + if (numLines) { @@ -1108,66 +1108,66 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm linesColor[i*3+2] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_color.z(); } } - + serverStatusOut.m_type = CMD_DEBUG_LINES_COMPLETED; serverStatusOut.m_sendDebugLinesArgs.m_numDebugLines = numLines; serverStatusOut.m_sendDebugLinesArgs.m_startingLineIndex = startingLineIndex; serverStatusOut.m_sendDebugLinesArgs.m_numRemainingDebugLines = m_data->m_remoteDebugDrawer->m_lines2.size()-(startingLineIndex+numLines); hasStatus = true; - + break; } case CMD_REQUEST_CAMERA_IMAGE_DATA: { - + int startPixelIndex = clientCmd.m_requestPixelDataArguments.m_startPixelIndex; int width, height; int numPixelsCopied = 0; - + if ( - (clientCmd.m_requestPixelDataArguments.m_startPixelIndex==0) && + (clientCmd.m_requestPixelDataArguments.m_startPixelIndex==0) && (clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT)!=0) { m_data->m_visualConverter.setWidthAndHeight(clientCmd.m_requestPixelDataArguments.m_pixelWidth, clientCmd.m_requestPixelDataArguments.m_pixelHeight); - } - + } + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_USE_HARDWARE_OPENGL)!=0) { m_data->m_guiHelper->copyCameraImageData(0,0,0,0,0,&width,&height,0); - } + } else { m_data->m_visualConverter.getWidthAndHeight(width,height); } - - - + + + int numTotalPixels = width*height; int numRemainingPixels = numTotalPixels - startPixelIndex; - - + + if (numRemainingPixels>0) { int maxNumPixels = bufferSizeInBytes/8-1; unsigned char* pixelRGBA = (unsigned char*)bufferServerToClient; int numRequestedPixels = btMin(maxNumPixels,numRemainingPixels); - + float* depthBuffer = (float*)(bufferServerToClient+numRequestedPixels*4); - + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_USE_HARDWARE_OPENGL)!=0) { m_data->m_guiHelper->copyCameraImageData(pixelRGBA,numRequestedPixels,depthBuffer,numRequestedPixels,startPixelIndex,&width,&height,&numPixelsCopied); } else { - + if (clientCmd.m_requestPixelDataArguments.m_startPixelIndex==0) { // printf("-------------------------------\nRendering\n"); - - + + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES)!=0) { m_data->m_visualConverter.render( @@ -1177,19 +1177,19 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { m_data->m_visualConverter.render(); } - + } - + m_data->m_visualConverter.copyCameraImageData(pixelRGBA,numRequestedPixels,depthBuffer,numRequestedPixels,startPixelIndex,&width,&height,&numPixelsCopied); } - + //each pixel takes 4 RGBA values and 1 float = 8 bytes - + } else { - + } - + serverStatusOut.m_type = CMD_CAMERA_IMAGE_COMPLETED; serverStatusOut.m_sendPixelDataArguments.m_numPixelsCopied = numPixelsCopied; serverStatusOut.m_sendPixelDataArguments.m_numRemainingPixels = numRemainingPixels - numPixelsCopied; @@ -1197,7 +1197,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverStatusOut.m_sendPixelDataArguments.m_imageWidth = width; serverStatusOut.m_sendPixelDataArguments.m_imageHeight= height; hasStatus = true; - + break; } @@ -1206,7 +1206,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm const SdfRequestInfoArgs& sdfInfoArgs = clientCmd.m_sdfRequestInfoArgs; //stream info into memory int streamSizeInBytes = createBodyInfoStream(sdfInfoArgs.m_bodyUniqueId, bufferServerToClient, bufferSizeInBytes); - + serverStatusOut.m_type = CMD_BODY_INFO_COMPLETED; serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = sdfInfoArgs.m_bodyUniqueId; serverStatusOut.m_dataStreamArguments.m_streamChunkLength = streamSizeInBytes; @@ -1220,9 +1220,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { b3Printf("Processed CMD_LOAD_SDF:%s", sdfArgs.m_sdfFileName); } - + bool completedOk = loadSdf(sdfArgs.m_sdfFileName,bufferServerToClient, bufferSizeInBytes); - + //serverStatusOut.m_type = CMD_SDF_LOADING_FAILED; serverStatusOut.m_sdfLoadedArgs.m_numBodies = m_data->m_sdfRecentLoadedBodies.size(); int maxBodies = btMin(MAX_SDF_BODIES, serverStatusOut.m_sdfLoadedArgs.m_numBodies); @@ -1230,14 +1230,14 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { serverStatusOut.m_sdfLoadedArgs.m_bodyUniqueIds[i] = m_data->m_sdfRecentLoadedBodies[i]; } - + serverStatusOut.m_type = CMD_SDF_LOADING_COMPLETED; hasStatus = true; break; } case CMD_LOAD_URDF: { - + const UrdfArgs& urdfArgs = clientCmd.m_urdfArguments; if (m_data->m_verboseOutput) { @@ -1267,32 +1267,32 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm bool completedOk = loadUrdf(urdfArgs.m_urdfFileName, initialPos,initialOrn, useMultiBody, useFixedBase,&bodyUniqueId, bufferServerToClient, bufferSizeInBytes); - - + + if (completedOk) { - + m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); - + serverStatusOut.m_type = CMD_URDF_LOADING_COMPLETED; serverStatusOut.m_dataStreamArguments.m_streamChunkLength = 0; - + if (m_data->m_urdfLinkNameMapper.size()) { serverStatusOut.m_dataStreamArguments.m_streamChunkLength = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize(); } serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId; hasStatus = true; - + } else { serverStatusOut.m_type = CMD_URDF_LOADING_FAILED; hasStatus = true; } - - - + + + break; } case CMD_CREATE_SENSOR: @@ -1322,7 +1322,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm mb->getLink(jointIndex).m_jointFeedback = fb; m_data->m_multiBodyJointFeedbacks.push_back(fb); }; - + } else { if (mb->getLink(jointIndex).m_jointFeedback) @@ -1337,7 +1337,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } } - + } else { b3Warning("No btMultiBody in the world. btRigidBody/btTypedConstraint sensor not hooked up yet"); @@ -1352,15 +1352,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm btJointFeedback* fb = new btJointFeedback(); m_data->m_jointFeedbacks.push_back(fb); c->setJointFeedback(fb); - - + + } */ #endif - + serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED; hasStatus = true; - + break; } case CMD_SEND_DESIRED_STATE: @@ -1369,7 +1369,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { b3Printf("Processed CMD_SEND_DESIRED_STATE"); } - + int bodyUniqueId = clientCmd.m_sendDesiredStateCommandArgument.m_bodyUniqueId; InteralBodyData* body = m_data->getHandle(bodyUniqueId); @@ -1377,7 +1377,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { btMultiBody* mb = body->m_multiBody; btAssert(mb); - + switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode) { case CONTROL_MODE_TORQUE: @@ -1386,38 +1386,23 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { b3Printf("Using CONTROL_MODE_TORQUE"); } - mb->clearForcesAndTorques(); - + // mb->clearForcesAndTorques(); int torqueIndex = 0; - #if 0 - //it is inconsistent to allow application of base force/torque, there is no 'joint' motor. Use a separate API for this. - - btVector3 f(0,0,0); - btVector3 t(0,0,0); - - if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) - { - f = btVector3 (clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[0], - clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[1], - clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[2]); - t = btVector3 (clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[3], - clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[4], - clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[5]); - } - torqueIndex+=6; - mb->addBaseForce(f); - mb->addBaseTorque(t); - #endif - for (int link=0;linkgetNumLinks();link++) + if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) { - - for (int dof=0;dofgetLink(link).m_dofCount;dof++) - { - double torque = 0.f; - if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) - torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[torqueIndex]; - mb->addJointTorqueMultiDof(link,dof,torque); - torqueIndex++; + for (int link=0;linkgetNumLinks();link++) + { + + for (int dof=0;dofgetLink(link).m_dofCount;dof++) + { + double torque = 0.f; + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[torqueIndex]&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) + { + torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[torqueIndex]; + mb->addJointTorqueMultiDof(link,dof,torque); + } + torqueIndex++; + } } } break; @@ -1428,7 +1413,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { b3Printf("Using CONTROL_MODE_VELOCITY"); } - + int numMotors = 0; //find the joint motors and apply the desired velocity and maximum force/torque { @@ -1437,22 +1422,30 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { if (supportsJointMotor(mb,link)) { - + btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr; - - + + if (motor) { btScalar desiredVelocity = 0.f; - if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_QDOT)!=0) - desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex]; - motor->setVelocityTarget(desiredVelocity); + bool hasDesiredVelocity = false; - btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime; - if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) - maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]*m_data->m_physicsDeltaTime; - - motor->setMaxAppliedImpulse(maxImp); + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex]&SIM_DESIRED_STATE_HAS_QDOT)!=0) + { + desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex]; + motor->setVelocityTarget(desiredVelocity); + hasDesiredVelocity = true; + } + if (hasDesiredVelocity) + { + btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime; + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex]&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) + { + maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]*m_data->m_physicsDeltaTime; + } + motor->setMaxAppliedImpulse(maxImp); + } numMotors++; } @@ -1470,7 +1463,6 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm b3Printf("Using CONTROL_MODE_POSITION_VELOCITY_PD"); } //compute the force base on PD control - mb->clearForcesAndTorques(); int numMotors = 0; //find the joint motors and apply the desired velocity and maximum force/torque @@ -1481,44 +1473,61 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { if (supportsJointMotor(mb,link)) { - + btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr; - + if (motor) { - - btScalar desiredVelocity = 0.f; - if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_QDOT)!=0) - desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex]; - btScalar desiredPosition = 0.f; - if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_Q)!=0) - desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex]; + bool hasDesiredPosOrVel = false; btScalar kp = 0.f; - if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_KP)!=0) - kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex]; - btScalar kd = 0.f; - if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_KD)!=0) - kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex]; + btScalar kd = 0.f; + btScalar desiredVelocity = 0.f; + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_QDOT)!=0) + { + hasDesiredPosOrVel = true; + desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex]; + kd = 0.1; + } + btScalar desiredPosition = 0.f; + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[posIndex] & SIM_DESIRED_STATE_HAS_Q)!=0) + { + hasDesiredPosOrVel = true; + desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex]; + kp = 0.1; + } - int dof1 = 0; - btScalar currentPosition = mb->getJointPosMultiDof(link)[dof1]; - btScalar currentVelocity = mb->getJointVelMultiDof(link)[dof1]; - btScalar positionStabiliationTerm = (desiredPosition-currentPosition)/m_data->m_physicsDeltaTime; - btScalar velocityError = (desiredVelocity - currentVelocity); - - desiredVelocity = kp * positionStabiliationTerm + - kd * velocityError; - - motor->setVelocityTarget(desiredVelocity); - - btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime; + if (hasDesiredPosOrVel) + { - if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) - maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex]*m_data->m_physicsDeltaTime; - - motor->setMaxAppliedImpulse(maxImp); + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_KP)!=0) + { + kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex]; + } + + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_KD)!=0) + { + kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex]; + } + + int dof1 = 0; + btScalar currentPosition = mb->getJointPosMultiDof(link)[dof1]; + btScalar currentVelocity = mb->getJointVelMultiDof(link)[dof1]; + btScalar positionStabiliationTerm = (desiredPosition-currentPosition)/m_data->m_physicsDeltaTime; + btScalar velocityError = (desiredVelocity - currentVelocity); + + desiredVelocity = kp * positionStabiliationTerm + + kd * velocityError; + + motor->setVelocityTarget(desiredVelocity); + btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime; + + if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) + maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex]*m_data->m_physicsDeltaTime; + + motor->setMaxAppliedImpulse(maxImp); + } numMotors++; } @@ -1535,10 +1544,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm b3Warning("m_controlMode not implemented yet"); break; } - + } } - + serverStatusOut.m_type = CMD_DESIRED_STATE_RECEIVED_COMPLETED; hasStatus = true; break; @@ -1551,7 +1560,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } int bodyUniqueId = clientCmd.m_requestActualStateInformationCommandArgument.m_bodyUniqueId; InteralBodyData* body = m_data->getHandle(bodyUniqueId); - + if (body && body->m_multiBody) { btMultiBody* mb = body->m_multiBody; @@ -1560,9 +1569,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId; int totalDegreeOfFreedomQ = 0; - int totalDegreeOfFreedomU = 0; - - + int totalDegreeOfFreedomU = 0; + + //always add the base, even for static (non-moving objects) //so that we can easily move the 'fixed' base when needed //do we don't use this conditional "if (!mb->hasFixedBase())" @@ -1570,7 +1579,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm btTransform tr; tr.setOrigin(mb->getBasePos()); tr.setRotation(mb->getWorldToBaseRot().inverse()); - + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[0] = body->m_rootLocalInertialFrame.getOrigin()[0]; serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[1] = @@ -1588,14 +1597,14 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm body->m_rootLocalInertialFrame.getRotation()[3]; - + //base position in world space, carthesian serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = tr.getOrigin()[0]; serverCmd.m_sendActualStateArgs.m_actualStateQ[1] = tr.getOrigin()[1]; serverCmd.m_sendActualStateArgs.m_actualStateQ[2] = tr.getOrigin()[2]; //base orientation, quaternion x,y,z,w, in world space, carthesian - serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0]; serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = tr.getRotation()[1]; serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = tr.getRotation()[2]; serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = tr.getRotation()[3]; @@ -1605,7 +1614,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = mb->getBaseVel()[0]; serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = mb->getBaseVel()[1]; serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = mb->getBaseVel()[2]; - + //base angular velocity (in world space, carthesian) serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = mb->getBaseOmega()[0]; serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = mb->getBaseOmega()[1]; @@ -1622,7 +1631,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { serverCmd.m_sendActualStateArgs.m_actualStateQdot[totalDegreeOfFreedomU++] = mb->getJointVelMultiDof(l)[d]; } - + if (0 == mb->getLink(l).m_jointFeedback) { for (int d=0;d<6;d++) @@ -1633,23 +1642,23 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { btVector3 sensedForce = mb->getLink(l).m_jointFeedback->m_reactionForces.getLinear(); btVector3 sensedTorque = mb->getLink(l).m_jointFeedback->m_reactionForces.getAngular(); - + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+0] = sensedForce[0]; serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+1] = sensedForce[1]; serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+2] = sensedForce[2]; - + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+3] = sensedTorque[0]; serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+4] = sensedTorque[1]; serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+5] = sensedTorque[2]; } - + serverCmd.m_sendActualStateArgs.m_jointMotorForce[l] = 0; - + if (supportsJointMotor(mb,l)) { - + btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)body->m_multiBody->getLink(l).m_userPtr; - + if (motor && m_data->m_physicsDeltaTime>btScalar(0)) { btScalar force =motor->getAppliedImpulse(0)/m_data->m_physicsDeltaTime; @@ -1663,10 +1672,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } btVector3 linkLocalInertialOrigin = body->m_linkLocalInertialFrames[l].getOrigin(); btQuaternion linkLocalInertialRotation = body->m_linkLocalInertialFrames[l].getRotation(); - + btVector3 linkOrigin = mb->getLink(l).m_cachedWorldTransform.getOrigin(); btQuaternion linkRotation = mb->getLink(l).m_cachedWorldTransform.getRotation(); - + serverCmd.m_sendActualStateArgs.m_linkState[l*7+0] = linkOrigin.getX(); serverCmd.m_sendActualStateArgs.m_linkState[l*7+1] = linkOrigin.getY(); serverCmd.m_sendActualStateArgs.m_linkState[l*7+2] = linkOrigin.getZ(); @@ -1674,24 +1683,24 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverCmd.m_sendActualStateArgs.m_linkState[l*7+4] = linkRotation.y(); serverCmd.m_sendActualStateArgs.m_linkState[l*7+5] = linkRotation.z(); serverCmd.m_sendActualStateArgs.m_linkState[l*7+6] = linkRotation.w(); - + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+0] = linkLocalInertialOrigin.getX(); serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+1] = linkLocalInertialOrigin.getY(); serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+2] = linkLocalInertialOrigin.getZ(); - + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+3] = linkLocalInertialRotation.x(); serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+4] = linkLocalInertialRotation.y(); serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+5] = linkLocalInertialRotation.z(); serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+6] = linkLocalInertialRotation.w(); - + } - + serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ; serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU; - + hasStatus = true; - + } else { if (body && body->m_rigidBody) @@ -1711,7 +1720,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverCmd.m_sendActualStateArgs.m_actualStateQ[2] = tr.getOrigin()[2]; //base orientation, quaternion x,y,z,w, in world space, carthesian - serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0]; serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = tr.getRotation()[1]; serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = tr.getRotation()[2]; serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = tr.getRotation()[3]; @@ -1721,7 +1730,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = rb->getLinearVelocity()[0]; serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = rb->getLinearVelocity()[1]; serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = rb->getLinearVelocity()[2]; - + //base angular velocity (in world space, carthesian) serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = rb->getAngularVelocity()[0]; serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = rb->getAngularVelocity()[1]; @@ -1745,7 +1754,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } case CMD_STEP_FORWARD_SIMULATION: { - + if (m_data->m_verboseOutput) { b3Printf("Step simulation request"); @@ -1756,13 +1765,13 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm applyJointDamping(i); } m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime,0); - + SharedMemoryStatus& serverCmd =serverStatusOut; serverCmd.m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED; hasStatus = true; break; } - + case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: { if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DELTA_TIME) @@ -1781,12 +1790,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } } - + SharedMemoryStatus& serverCmd =serverStatusOut; serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; hasStatus = true; break; - + }; case CMD_INIT_POSE: { @@ -1803,6 +1812,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION) { btVector3 zero(0,0,0); + btAssert(clientCmd.m_initPoseArgs.m_hasInitialStateQ[0] && + clientCmd.m_initPoseArgs.m_hasInitialStateQ[1] && + clientCmd.m_initPoseArgs.m_hasInitialStateQ[2]); + mb->setBaseVel(zero); mb->setBasePos(btVector3( clientCmd.m_initPoseArgs.m_initialStateQ[0], @@ -1811,6 +1824,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION) { + btAssert(clientCmd.m_initPoseArgs.m_hasInitialStateQ[3] && + clientCmd.m_initPoseArgs.m_hasInitialStateQ[4] && + clientCmd.m_initPoseArgs.m_hasInitialStateQ[5] && + clientCmd.m_initPoseArgs.m_hasInitialStateQ[6]); + mb->setBaseOmega(btVector3(0,0,0)); mb->setWorldToBaseRot(btQuaternion( clientCmd.m_initPoseArgs.m_initialStateQ[3], @@ -1823,9 +1841,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm int dofIndex = 7; for (int i=0;igetNumLinks();i++) { - if (mb->getLink(i).m_dofCount==1) + if ( (clientCmd.m_initPoseArgs.m_hasInitialStateQ[dofIndex]) && (mb->getLink(i).m_dofCount==1)) { - mb->setJointPos(i,clientCmd.m_initPoseArgs.m_initialStateQ[dofIndex]); mb->setJointVel(i,0); } @@ -1833,14 +1850,14 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } } } - + SharedMemoryStatus& serverCmd =serverStatusOut; serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; hasStatus = true; break; } - + case CMD_RESET_SIMULATION: { @@ -1857,7 +1874,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm createEmptyDynamicsWorld(); m_data->exitHandles(); m_data->initHandles(); - + SharedMemoryStatus& serverCmd =serverStatusOut; serverCmd.m_type = CMD_RESET_SIMULATION_COMPLETED; hasStatus = true; @@ -1887,7 +1904,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_INITIAL_ORIENTATION) { - + startTrans.setRotation(btQuaternion( clientCmd.m_createBoxShapeArguments.m_initialOrientation[0], clientCmd.m_createBoxShapeArguments.m_initialOrientation[1], @@ -1912,7 +1929,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm m_data->m_worldImporters.push_back(worldImporter); btCollisionShape* shape = 0; - + switch (shapeType) { case COLLISION_SHAPE_TYPE_CYLINDER_X: @@ -1969,8 +1986,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm shape = worldImporter->createBoxShape(halfExtents); } } - - + + bool isDynamic = (mass>0); btRigidBody* rb = worldImporter->createRigidBody(isDynamic,mass,startTrans,shape,0); rb->setRollingFriction(0.2); @@ -1985,11 +2002,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } m_data->m_guiHelper->createCollisionShapeGraphicsObject(rb->getCollisionShape()); m_data->m_guiHelper->createCollisionObjectGraphicsObject(rb,colorRGBA); - - + + SharedMemoryStatus& serverCmd =serverStatusOut; serverCmd.m_type = CMD_RIGID_BODY_CREATION_COMPLETED; - + int bodyUniqueId = m_data->allocHandle(); InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId); @@ -2008,7 +2025,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm clientCmd.m_pickBodyArguments.m_rayToWorld[1], clientCmd.m_pickBodyArguments.m_rayToWorld[2])); - + SharedMemoryStatus& serverCmd =serverStatusOut; serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; hasStatus = true; @@ -2040,7 +2057,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm default: { b3Error("Unknown command encountered"); - + SharedMemoryStatus& serverCmd =serverStatusOut; serverCmd.m_type = CMD_UNKNOWN_COMMAND_FLUSHED; hasStatus = true; @@ -2048,7 +2065,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } }; - + } } return hasStatus; @@ -2060,10 +2077,10 @@ void PhysicsServerCommandProcessor::renderScene() if (m_data->m_guiHelper) { m_data->m_guiHelper->syncPhysicsToGraphics(m_data->m_dynamicsWorld); - + m_data->m_guiHelper->render(m_data->m_dynamicsWorld); } - + } void PhysicsServerCommandProcessor::physicsDebugDraw(int debugDrawFlags) @@ -2114,7 +2131,7 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons btMultiBodyLinkCollider* multiCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(rayCallback.m_collisionObject); if (multiCol && multiCol->m_multiBody) { - + m_data->m_prevCanSleep = multiCol->m_multiBody->getCanSleep(); multiCol->m_multiBody->setCanSleep(false); @@ -2127,10 +2144,10 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons //it is not satisfying, hopefully we find a better solution (higher order integrator, using joint friction using a zero-velocity target motor with limited force etc?) btScalar scaling=1; p2p->setMaxAppliedImpulse(2*scaling); - + btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; world->addMultiBodyConstraint(p2p); - m_data->m_pickingMultiBodyPoint2Point =p2p; + m_data->m_pickingMultiBodyPoint2Point =p2p; } } @@ -2155,7 +2172,7 @@ bool PhysicsServerCommandProcessor::movePickedBody(const btVector3& rayFromWorld if (pickCon) { //keep it at the same picking distance - + btVector3 dir = rayToWorld-rayFromWorld; dir.normalize(); dir *= m_data->m_oldPickingDist; @@ -2164,21 +2181,21 @@ bool PhysicsServerCommandProcessor::movePickedBody(const btVector3& rayFromWorld pickCon->setPivotB(newPivotB); } } - + if (m_data->m_pickingMultiBodyPoint2Point) { //keep it at the same picking distance - + btVector3 dir = rayToWorld-rayFromWorld; dir.normalize(); dir *= m_data->m_oldPickingDist; btVector3 newPivotB = rayFromWorld + dir; - + m_data->m_pickingMultiBodyPoint2Point->setPivotInB(newPivotB); } - + return false; } diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 83fd91c34..9745b8eb6 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -115,6 +115,7 @@ enum EnumInitPoseFlags struct InitPoseArgs { int m_bodyUniqueId; + int m_hasInitialStateQ[MAX_DEGREE_OF_FREEDOM]; double m_initialStateQ[MAX_DEGREE_OF_FREEDOM]; }; @@ -179,11 +180,14 @@ struct SendDesiredStateArgs double m_Kp[MAX_DEGREE_OF_FREEDOM];//indexed by degree of freedom, 6 for base, and then the dofs for each link double m_Kd[MAX_DEGREE_OF_FREEDOM];//indexed by degree of freedom, 6 for base, and then the dofs for each link + int m_hasDesiredStateFlags[MAX_DEGREE_OF_FREEDOM]; + //desired state is only written by the client, read-only access by server is expected //m_desiredStateQ is indexed by position variables, //starting with 3 base position variables, 4 base orientation variables (quaternion), then link position variables double m_desiredStateQ[MAX_DEGREE_OF_FREEDOM]; + //m_desiredStateQdot is index by velocity degrees of freedom, 3 linear and 3 angular variables for the base and then link velocity variables double m_desiredStateQdot[MAX_DEGREE_OF_FREEDOM]; diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 0d2303fc5..b978feb47 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -3,6 +3,7 @@ #include "../SharedMemory/SharedMemoryInProcessPhysicsC_API.h" + #ifdef __APPLE__ #include #else @@ -139,9 +140,11 @@ pybullet_loadURDF(PyObject* self, PyObject* args) int bodyIndex = -1; const char* urdfFileName=""; + float startPosX =0; - float startPosY =0; - float startPosZ = 1; + float startPosY =0; + float startPosZ = 0; + float startOrnX = 0; float startOrnY = 0; float startOrnZ = 0; @@ -282,7 +285,7 @@ pybullet_resetSimulation(PyObject* self, PyObject* args) return Py_None; } -static PyObject* pybullet_setJointControl(PyObject* self, PyObject* args) +static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) { //todo(erwincoumans): set max forces, kp, kd @@ -296,25 +299,22 @@ static PyObject* pybullet_setJointControl(PyObject* self, PyObject* args) size= PySequence_Size(args); - if (size==3) + if (size==4) { - int bodyIndex, controlMode; - PyObject* targetValues; - if (PyArg_ParseTuple(args, "iiO", &bodyIndex, &controlMode, &targetValues)) + int bodyIndex, jointIndex, controlMode; + double targetValue; + + if (PyArg_ParseTuple(args, "iiid", &bodyIndex, &jointIndex, &controlMode, &targetValue)) { - PyObject* seq; - int numJoints, len; - seq = PySequence_Fast(targetValues, "expected a sequence"); - len = PySequence_Size(targetValues); - numJoints = b3GetNumJoints(sm,bodyIndex); + int numJoints; b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryStatusHandle statusHandle; - int qIndex; - - if (len!=numJoints) - { - PyErr_SetString(SpamError, "Number of control target values doesn't match the number of joints."); - Py_DECREF(seq); + struct b3JointInfo info; + + numJoints = b3GetNumJoints(sm,bodyIndex); + if ((jointIndex >= numJoints) || (jointIndex < 0)) + { + PyErr_SetString(SpamError, "Joint index out-of-range."); return NULL; } @@ -323,49 +323,43 @@ static PyObject* pybullet_setJointControl(PyObject* self, PyObject* args) (controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)) { PyErr_SetString(SpamError, "Illegral control mode."); - Py_DECREF(seq); return NULL; } commandHandle = b3JointControlCommandInit2(sm, bodyIndex,controlMode); - - for (qIndex=0;qIndex= numJoints) || (jointIndex < 0)) + { + PyErr_SetString(SpamError, "Joint index out-of-range."); return NULL; } + + commandHandle = b3CreatePoseCommandInit(sm, bodyIndex); - commandHandle = b3CreatePoseCommandInit(sm, bodyIndex); + b3CreatePoseCommandSetJointPosition(sm, commandHandle, jointIndex, targetValue); - for (qIndex=0;qIndex Date: Fri, 24 Jun 2016 11:06:56 -0700 Subject: [PATCH 51/53] move PD control from PhysicsServerCommandProcessor into btMultiBodyJointMotor improvements/changes in pybullet API --- build_and_run_cmake.sh | 2 +- .../PhysicsServerCommandProcessor.cpp | 24 +-- examples/pybullet/pybullet.c | 151 +++++++++++------- .../Featherstone/btMultiBodyJointMotor.cpp | 29 +++- .../Featherstone/btMultiBodyJointMotor.h | 21 ++- 5 files changed, 146 insertions(+), 81 deletions(-) diff --git a/build_and_run_cmake.sh b/build_and_run_cmake.sh index 1405060f4..28354357b 100755 --- a/build_and_run_cmake.sh +++ b/build_and_run_cmake.sh @@ -2,6 +2,6 @@ rm CMakeCache.txt mkdir build_cmake cd build_cmake -cmake .. +cmake -DBUILD_PYTHON=OFF -CMAKE_BUILD_TYPE=Release .. make -j12 examples/ExampleBrowser/App_ExampleBrowser diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 3cdaac902..99b883454 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1434,7 +1434,16 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex]&SIM_DESIRED_STATE_HAS_QDOT)!=0) { desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex]; - motor->setVelocityTarget(desiredVelocity); + btScalar kd = 0.1f; + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_KD)!=0) + { + kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[dofIndex]; + } + + motor->setVelocityTarget(desiredVelocity,kd); + + btScalar kp = 0.f; + motor->setPositionTarget(0,kp); hasDesiredVelocity = true; } if (hasDesiredVelocity) @@ -1511,16 +1520,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex]; } - int dof1 = 0; - btScalar currentPosition = mb->getJointPosMultiDof(link)[dof1]; - btScalar currentVelocity = mb->getJointVelMultiDof(link)[dof1]; - btScalar positionStabiliationTerm = (desiredPosition-currentPosition)/m_data->m_physicsDeltaTime; - btScalar velocityError = (desiredVelocity - currentVelocity); - - desiredVelocity = kp * positionStabiliationTerm + - kd * velocityError; - - motor->setVelocityTarget(desiredVelocity); + motor->setVelocityTarget(desiredVelocity,kd); + motor->setPositionTarget(desiredPosition,kp); + btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime; if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index b978feb47..78e770dbe 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -290,7 +290,13 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) //todo(erwincoumans): set max forces, kp, kd int size; - + int bodyIndex, jointIndex, controlMode; + double targetValue=0; + double maxForce=100000; + double gains=0.1; + int valid = 0; + + if (0==sm) { PyErr_SetString(SpamError, "Not connected to physics server."); @@ -301,69 +307,96 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) if (size==4) { - int bodyIndex, jointIndex, controlMode; - double targetValue; - if (PyArg_ParseTuple(args, "iiid", &bodyIndex, &jointIndex, &controlMode, &targetValue)) + if (!PyArg_ParseTuple(args, "iiid", &bodyIndex, &jointIndex, &controlMode, &targetValue)) { - int numJoints; - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - struct b3JointInfo info; + PyErr_SetString(SpamError, "Error parsing arguments"); + return NULL; + } + valid = 1; + } + if (size==5) + { + + if (!PyArg_ParseTuple(args, "iiidd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce)) + { + PyErr_SetString(SpamError, "Error parsing arguments"); + return NULL; + } + valid = 1; + } + if (size==6) + { + + if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce, &gains)) + { + PyErr_SetString(SpamError, "Error parsing arguments"); + return NULL; + } + valid = 1; + } + + + if (valid) + { + int numJoints; + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + struct b3JointInfo info; - numJoints = b3GetNumJoints(sm,bodyIndex); - if ((jointIndex >= numJoints) || (jointIndex < 0)) - { - PyErr_SetString(SpamError, "Joint index out-of-range."); - return NULL; - } - - if ((controlMode != CONTROL_MODE_VELOCITY) && - (controlMode != CONTROL_MODE_TORQUE) && - (controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)) - { - PyErr_SetString(SpamError, "Illegral control mode."); - return NULL; - } - - commandHandle = b3JointControlCommandInit2(sm, bodyIndex,controlMode); - - b3GetJointInfo(sm, bodyIndex, jointIndex, &info); - - switch (controlMode) - { - case CONTROL_MODE_VELOCITY: - { - b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetValue); - b3JointControlSetKd(commandHandle,info.m_uIndex,1); - b3JointControlSetMaximumForce(commandHandle,info.m_uIndex,10000); - break; - } - - case CONTROL_MODE_TORQUE: - { - b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, targetValue); - break; - } - - case CONTROL_MODE_POSITION_VELOCITY_PD: - { - b3JointControlSetDesiredPosition( commandHandle, info.m_qIndex, targetValue); - b3JointControlSetKp(commandHandle,info.m_uIndex,1); - b3JointControlSetMaximumForce(commandHandle,info.m_uIndex,10000); - break; - } - default: - { - } - }; - - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - - Py_INCREF(Py_None); - return Py_None; + numJoints = b3GetNumJoints(sm,bodyIndex); + if ((jointIndex >= numJoints) || (jointIndex < 0)) + { + PyErr_SetString(SpamError, "Joint index out-of-range."); + return NULL; } + if ((controlMode != CONTROL_MODE_VELOCITY) && + (controlMode != CONTROL_MODE_TORQUE) && + (controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)) + { + PyErr_SetString(SpamError, "Illegral control mode."); + return NULL; + } + + commandHandle = b3JointControlCommandInit2(sm, bodyIndex,controlMode); + + b3GetJointInfo(sm, bodyIndex, jointIndex, &info); + + switch (controlMode) + { + case CONTROL_MODE_VELOCITY: + { + b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetValue); + double kd = gains; + b3JointControlSetKd(commandHandle,info.m_uIndex,kd); + b3JointControlSetMaximumForce(commandHandle,info.m_uIndex,maxForce); + break; + } + + case CONTROL_MODE_TORQUE: + { + b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, targetValue); + break; + } + + case CONTROL_MODE_POSITION_VELOCITY_PD: + { + b3JointControlSetDesiredPosition( commandHandle, info.m_qIndex, targetValue); + double kp = gains; + b3JointControlSetKp(commandHandle,info.m_uIndex,kp); + b3JointControlSetMaximumForce(commandHandle,info.m_uIndex,maxForce); + break; + } + default: + { + } + }; + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + + Py_INCREF(Py_None); + return Py_None; } PyErr_SetString(SpamError, "error in setJointControl."); return NULL; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp index 50268611b..3d0f45b77 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp @@ -23,7 +23,10 @@ subject to the following restrictions: btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse) :btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true), - m_desiredVelocity(desiredVelocity) + m_desiredVelocity(desiredVelocity), + m_desiredPosition(0), + m_kd(0.1), + m_kp(0) { m_maxAppliedImpulse = maxMotorImpulse; @@ -51,7 +54,10 @@ void btMultiBodyJointMotor::finalizeMultiDof() btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse) //:btMultiBodyConstraint(body,0,link,-1,1,true), :btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true), - m_desiredVelocity(desiredVelocity) + m_desiredVelocity(desiredVelocity), + m_desiredPosition(0), + m_kd(0.1), + m_kp(0) { btAssert(linkDoF < body->getLink(link).m_dofCount); @@ -113,9 +119,22 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con for (int row=0;rowgetJointPosMultiDof(m_linkA)[dof]; + btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof]; + btScalar positionStabiliationTerm = (m_desiredPosition-currentPosition)/infoGlobal.m_timeStep; + btScalar velocityError = (m_desiredVelocity - currentVelocity); + btScalar rhs = m_kp * positionStabiliationTerm + m_kd * velocityError; + printf("m_kd = %f\n", m_kd); + printf("m_kp = %f\n", m_kp); + printf("m_desiredVelocity = %f\n", m_desiredVelocity); + printf("m_desiredPosition = %f\n", m_desiredPosition); + printf("m_maxAppliedImpulse = %f\n", m_maxAppliedImpulse); + printf("rhs = %f\n", rhs); + + + fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,rhs); constraintRow.m_orgConstraint = this; constraintRow.m_orgDofIndex = row; { diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h index d322f87e7..2a55be035 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h @@ -24,9 +24,12 @@ struct btSolverInfo; class btMultiBodyJointMotor : public btMultiBodyConstraint { protected: - - - btScalar m_desiredVelocity; + + btScalar m_desiredVelocity; + btScalar m_desiredPosition; + btScalar m_kd; + btScalar m_kp; + public: @@ -42,10 +45,18 @@ public: btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal); - virtual void setVelocityTarget(btScalar velTarget) + virtual void setVelocityTarget(btScalar velTarget, btScalar kd = 1.f) { - m_desiredVelocity = velTarget; + m_desiredVelocity = velTarget; + m_kd = kd; } + + virtual void setPositionTarget(btScalar posTarget, btScalar kp = 1.f) + { + m_desiredPosition = posTarget; + m_kp = kp; + } + virtual void debugDraw(class btIDebugDraw* drawer) { From a15eb3035e1aa72f7e80c525e0599bbe5d939858 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Fri, 24 Jun 2016 11:12:19 -0700 Subject: [PATCH 52/53] default gains 1 -> 0.1 --- .../Featherstone/btMultiBodyJointMotor.h | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h index 2a55be035..e433515d4 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h @@ -24,11 +24,11 @@ struct btSolverInfo; class btMultiBodyJointMotor : public btMultiBodyConstraint { protected: - - btScalar m_desiredVelocity; - btScalar m_desiredPosition; - btScalar m_kd; - btScalar m_kp; + + btScalar m_desiredVelocity; + btScalar m_desiredPosition; + btScalar m_kd; + btScalar m_kp; public: @@ -45,18 +45,18 @@ public: btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal); - virtual void setVelocityTarget(btScalar velTarget, btScalar kd = 1.f) + virtual void setVelocityTarget(btScalar velTarget, btScalar kd = 0.1f) { - m_desiredVelocity = velTarget; + m_desiredVelocity = velTarget; m_kd = kd; } - - virtual void setPositionTarget(btScalar posTarget, btScalar kp = 1.f) + + virtual void setPositionTarget(btScalar posTarget, btScalar kp = 0.1f) { - m_desiredPosition = posTarget; + m_desiredPosition = posTarget; m_kp = kp; - } - + } + virtual void debugDraw(class btIDebugDraw* drawer) { From 013dbda023eb728646c98d4d666c3dbba8a67a3c Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sun, 26 Jun 2016 18:18:30 -0700 Subject: [PATCH 53/53] implement a few more pybullet methods: pybullet_applyExternalForce, pybullet_applyExternalTorque, pybullet_setTimeStep, pybullet_resetBasePositionAndOrientation, pybullet_getQuaternionFromEuler, pybullet_getEulerFromQuaternion --- examples/SharedMemory/PhysicsClientC_API.cpp | 47 ++ examples/SharedMemory/PhysicsClientC_API.h | 5 + .../PhysicsServerCommandProcessor.cpp | 64 ++ examples/SharedMemory/SharedMemoryCommands.h | 22 + examples/SharedMemory/SharedMemoryPublic.h | 7 + examples/SharedMemory/premake4.lua | 2 + examples/pybullet/pybullet.c | 549 +++++++++++++----- 7 files changed, 547 insertions(+), 149 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 203e0ebbe..e67a2c144 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -903,3 +903,50 @@ void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImage } } +b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + + command->m_type = CMD_APPLY_EXTERNAL_FORCE; + command->m_updateFlags = 0; + command->m_externalForceArguments.m_numForcesAndTorques = 0; + return (b3SharedMemoryCommandHandle) command; +} + +void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flag) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_APPLY_EXTERNAL_FORCE); + int index = command->m_externalForceArguments.m_numForcesAndTorques; + command->m_externalForceArguments.m_bodyUniqueIds[index] = bodyUniqueId; + command->m_externalForceArguments.m_linkIds[index] = linkId; + command->m_externalForceArguments.m_forceFlags[index] = EF_FORCE+flag; + for (int i = 0; i < 3; ++i) { + command->m_externalForceArguments.m_forcesAndTorques[index+i] = force[i]; + command->m_externalForceArguments.m_positions[index+i] = position[i]; + } + + command->m_externalForceArguments.m_numForcesAndTorques++; +} + +void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flag) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_APPLY_EXTERNAL_FORCE); + int index = command->m_externalForceArguments.m_numForcesAndTorques; + command->m_externalForceArguments.m_bodyUniqueIds[index] = bodyUniqueId; + command->m_externalForceArguments.m_linkIds[index] = linkId; + command->m_externalForceArguments.m_forceFlags[index] = EF_TORQUE+flag; + + for (int i = 0; i < 3; ++i) { + command->m_externalForceArguments.m_forcesAndTorques[index+i] = torque[i]; + } + command->m_externalForceArguments.m_numForcesAndTorques++; +} + diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index aee561d8f..8f57f4db9 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -159,6 +159,11 @@ b3SharedMemoryCommandHandle b3MovePickedBody(b3PhysicsClientHandle physClient, d double rayToWorldZ); b3SharedMemoryCommandHandle b3RemovePickingConstraint(b3PhysicsClientHandle physClient); +/// Apply external force at the body (or link) center of mass, in world space/Cartesian coordinates. +b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient); +void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flags); +void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flags); + #ifdef __cplusplus } #endif diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 99b883454..6ead18363 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2056,6 +2056,70 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm hasStatus = true; break; } + case CMD_APPLY_EXTERNAL_FORCE: + { + for (int i = 0; i < clientCmd.m_externalForceArguments.m_numForcesAndTorques; ++i) + { + InteralBodyData* body = m_data->getHandle(clientCmd.m_externalForceArguments.m_bodyUniqueIds[i]); + if (body && body->m_multiBody) + { + btMultiBody* mb = body->m_multiBody; + bool isLinkFrame = ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_LINK_FRAME)!=0); + + if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE)!=0) + { + btVector3 forceLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+0], + clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+1], + clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+2]); + btVector3 positionLocal( + clientCmd.m_externalForceArguments.m_positions[i*3+0], + clientCmd.m_externalForceArguments.m_positions[i*3+1], + clientCmd.m_externalForceArguments.m_positions[i*3+2]); + + if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1) + { + btVector3 forceWorld = isLinkFrame ? forceLocal : mb->getBaseWorldTransform().getBasis()*forceLocal; + btVector3 relPosWorld = isLinkFrame ? positionLocal : mb->getBaseWorldTransform().getBasis()*positionLocal; + mb->addBaseForce(forceWorld); + mb->addBaseTorque(relPosWorld.cross(forceWorld)); + //b3Printf("apply base force of %f,%f,%f at %f,%f,%f\n", forceWorld[0],forceWorld[1],forceWorld[2],positionLocal[0],positionLocal[1],positionLocal[2]); + } else + { + int link = clientCmd.m_externalForceArguments.m_linkIds[i]; + btVector3 forceWorld = mb->getLink(link).m_cachedWorldTransform.getBasis()*forceLocal; + btVector3 relPosWorld = mb->getLink(link).m_cachedWorldTransform.getBasis()*positionLocal; + mb->addLinkForce(link, forceWorld); + mb->addLinkTorque(link,relPosWorld.cross(forceWorld)); + //b3Printf("apply link force of %f,%f,%f at %f,%f,%f\n", forceWorld[0],forceWorld[1],forceWorld[2], positionLocal[0],positionLocal[1],positionLocal[2]); + } + } + if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_TORQUE)!=0) + { + btVector3 torqueLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+0], + clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+1], + clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+2]); + + if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1) + { + btVector3 torqueWorld = isLinkFrame ? torqueLocal : mb->getBaseWorldTransform().getBasis()*torqueLocal; + mb->addBaseTorque(torqueWorld); + //b3Printf("apply base torque of %f,%f,%f\n", torqueWorld[0],torqueWorld[1],torqueWorld[2]); + } else + { + int link = clientCmd.m_externalForceArguments.m_linkIds[i]; + btVector3 torqueWorld = mb->getLink(link).m_cachedWorldTransform.getBasis()*torqueLocal; + mb->addLinkTorque(link, torqueWorld); + //b3Printf("apply link torque of %f,%f,%f\n", torqueWorld[0],torqueWorld[1],torqueWorld[2]); + } + } + } + } + + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; + hasStatus = true; + break; + } default: { b3Error("Unknown command encountered"); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 9745b8eb6..3ddb11e91 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -319,6 +319,27 @@ struct SdfRequestInfoArgs int m_bodyUniqueId; }; +///flags for b3ApplyExternalTorque and b3ApplyExternalForce +enum EnumExternalForcePrivateFlags +{ +// EF_LINK_FRAME=1, +// EF_WORLD_FRAME=2, + EF_TORQUE=4, + EF_FORCE=8, +}; + + + +struct ExternalForceArgs +{ + int m_numForcesAndTorques; + int m_bodyUniqueIds[MAX_SDF_BODIES]; + int m_linkIds[MAX_SDF_BODIES]; + double m_forcesAndTorques[3*MAX_SDF_BODIES]; + double m_positions[3*MAX_SDF_BODIES]; + int m_forceFlags[MAX_SDF_BODIES]; +}; + enum EnumSdfRequestInfoFlags { SDF_REQUEST_INFO_BODY=1, @@ -350,6 +371,7 @@ struct SharedMemoryCommand struct RequestDebugLinesArgs m_requestDebugLinesArguments; struct RequestPixelDataArgs m_requestPixelDataArguments; struct PickBodyArgs m_pickBodyArguments; + struct ExternalForceArgs m_externalForceArguments; }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 5e6da5c50..3aeeb8f11 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -26,6 +26,7 @@ enum EnumSharedMemoryClientCommand CMD_MOVE_PICKED_BODY, CMD_REMOVE_PICKING_CONSTRAINT_BODY, CMD_REQUEST_CAMERA_IMAGE_DATA, + CMD_APPLY_EXTERNAL_FORCE, CMD_MAX_CLIENT_COMMANDS }; @@ -143,5 +144,11 @@ enum { CONTROL_MODE_POSITION_VELOCITY_PD, }; +///flags for b3ApplyExternalTorque and b3ApplyExternalForce +enum EnumExternalForceFlags +{ + EF_LINK_FRAME=1, + EF_WORLD_FRAME=2, +}; #endif//SHARED_MEMORY_PUBLIC_H diff --git a/examples/SharedMemory/premake4.lua b/examples/SharedMemory/premake4.lua index e923b412f..34aea2de5 100644 --- a/examples/SharedMemory/premake4.lua +++ b/examples/SharedMemory/premake4.lua @@ -26,6 +26,8 @@ files { "PhysicsServer.h", "main.cpp", "PhysicsClientC_API.cpp", + "SharedMemoryCommands.h", + "SharedMemoryPublic.h", "PhysicsServer.cpp", "PosixSharedMemory.cpp", "Win32SharedMemory.cpp", diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 78e770dbe..5cdf39a59 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -437,6 +437,36 @@ pybullet_setGravity(PyObject* self, PyObject* args) return Py_None; } +static PyObject * +pybullet_setTimeStep(PyObject* self, PyObject* args) +{ + if (0==sm) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + double timeStep=0.001; + int ret; + + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); + b3SharedMemoryStatusHandle statusHandle; + + if (!PyArg_ParseTuple(args, "d", &timeStep)) + { + PyErr_SetString(SpamError, "setTimeStep expected a single value (double)."); + return NULL; + } + ret = b3PhysicsParamSetTimeStep(command, timeStep); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + //ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); + } + + Py_INCREF(Py_None); + return Py_None; +} + // Internal function used to get the base position and orientation @@ -632,140 +662,88 @@ pybullet_resetJointState(PyObject* self, PyObject* args) return NULL; } - - -// CURRENTLY NOT SUPPORTED -// Initalize a single joint position for a specific body index -// -// This method skips any physics simulation and -// teleports all joints to the new positions. - -// TODO(hellojas): initializing one joint currently not supported -// static PyObject* -// pybullet_initializeJointPosition(PyObject* self, PyObject* args) -// { -// if (0==sm) -// { -// PyErr_SetString(SpamError, "Not connected to physics server."); -// return NULL; -// } -// -// int i; -// int bodyIndex = -1; -// int jointIndex = -1; -// double jointPos = 0.0; -// -// int size= PySequence_Size(args); -// -// if (size==3) // get body index, joint index, and joint position value -// { -// if (PyArg_ParseTuple(args, "iid", &bodyIndex, &jointIndex, &jointPos)) -// { -// b3SharedMemoryCommandHandle cmd_handle = b3CreatePoseCommandInit(sm, bodyIndex); -// -// // printf("initializing joint %d at %f\n", jointIndex, jointPos); -// b3CreatePoseCommandSetJointPosition(sm, cmd_handle, jointIndex, jointPos); -// -// b3SharedMemoryStatusHandle status_handle = -// b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); -// -// const int status_type = b3GetStatusType(status_handle); -// -// } -// } -// -// Py_INCREF(Py_None); -// return Py_None; -// } -#if 0 -static void pybullet_internalGetJointPositions(int bodyIndex, int numJoints, double jointPositions[]) { - int i, j; - int numDegreeQ; - int numDegreeU; - int arrSizeOfPosAndOrn = 7; - - for (i =0;i euler yaw/pitch/roll convention from URDF/SDF, see Gazebo +///https://github.com/arpg/Gazebo/blob/master/gazebo/math/Quaternion.cc +static PyObject* pybullet_getEulerFromQuaternion(PyObject* self, PyObject* args) +{ + + double squ; + double sqx; + double sqy; + double sqz; + + double quat[4]; + + PyObject* quatObj; + + if (PyArg_ParseTuple(args, "O", &quatObj)) + { + PyObject* seq; + int len,i; + seq = PySequence_Fast(quatObj, "expected a sequence"); + len = PySequence_Size(quatObj); + if (len==4) + { + for (i = 0; i < 4; i++) + { + quat[i] = pybullet_internalGetFloatFromSequence(seq,i); + } + } else + { + PyErr_SetString(SpamError, "Quaternion need a 4 components [x,y,z,w]."); + Py_DECREF(seq); + return NULL; + } + Py_DECREF(seq); + } else + { + PyErr_SetString(SpamError, "Quaternion need a 4 components [x,y,z,w]."); + return NULL; + } + + { + double rpy[3]; + sqx = quat[0] * quat[0]; + sqy = quat[1] * quat[1]; + sqz = quat[2] * quat[2]; + squ = quat[3] * quat[3]; + rpy[0] = atan2(2 * (quat[1]*quat[2] + quat[3]*quat[0]), squ - sqx - sqy + sqz); + double sarg = -2 * (quat[0]*quat[2] - quat[3] * quat[1]); + rpy[1] = sarg <= -1.0 ? -0.5*3.141592538 : (sarg >= 1.0 ? 0.5*3.141592538 : asin(sarg)); + rpy[2] = atan2(2 * (quat[0]*quat[1] + quat[3]*quat[2]), squ + sqx - sqy - sqz); + { + PyObject *pylist; + int i; + pylist = PyTuple_New(3); + for (i=0;i<3;i++) + PyTuple_SetItem(pylist,i,PyFloat_FromDouble(rpy[i])); + return pylist; + } + } + Py_INCREF(Py_None); + return Py_None; +} + static PyMethodDef SpamMethods[] = { @@ -1109,6 +1343,9 @@ static PyMethodDef SpamMethods[] = { {"setGravity", pybullet_setGravity, METH_VARARGS, "Set the gravity acceleration (x,y,z)."}, + {"setTimeStep", pybullet_setTimeStep, METH_VARARGS, + "Set the amount of time to proceed at each call to stepSimulation. (unit is seconds, typically range is 0.01 or 0.001)"}, + {"loadURDF", pybullet_loadURDF, METH_VARARGS, "Create a multibody by loading a URDF file."}, @@ -1118,8 +1355,8 @@ static PyMethodDef SpamMethods[] = { {"getBasePositionAndOrientation", pybullet_getBasePositionAndOrientation, METH_VARARGS, "Get the world position and orientation of the base of the object. (x,y,z) position vector and (x,y,z,w) quaternion orientation."}, -// {"resetBasePositionAndOrientation", pybullet_getBasePositionAndOrientation, METH_VARARGS, -// "Reset the world position and orientation of the base of the object instantaneously, not through physics simulation. (x,y,z) position vector and (x,y,z,w) quaternion orientation."}, + {"resetBasePositionAndOrientation", pybullet_resetBasePositionAndOrientation, METH_VARARGS, + "Reset the world position and orientation of the base of the object instantaneously, not through physics simulation. (x,y,z) position vector and (x,y,z,w) quaternion orientation."}, {"getNumJoints", pybullet_getNumJoints, METH_VARARGS, "Get the number of joints for an object."}, @@ -1136,18 +1373,29 @@ static PyMethodDef SpamMethods[] = { {"setJointMotorControl", pybullet_setJointMotorControl, METH_VARARGS, "Set a single joint motor control mode and desired target value. There is no immediate state change, stepSimulation will process the motors."}, + {"applyExternalForce", pybullet_applyExternalForce, METH_VARARGS, + "for objectUniqueId, linkIndex (-1 for base/root link), apply a force [x,y,z] at the a position [x,y,z], flag to select FORCE_IN_LINK_FRAME or FORCE_IN_WORLD_FRAME coordinates"}, + + {"applyExternalTorque", pybullet_applyExternalTorque, METH_VARARGS, + "for objectUniqueId, linkIndex (-1 for base/root link) apply a torque [x,y,z] in Cartesian coordinates, flag to select TORQUE_IN_LINK_FRAME or TORQUE_IN_WORLD_FRAME coordinates"}, + + {"renderImage", pybullet_renderImage, METH_VARARGS, + "Render an image (given the pixel resolution width & height and camera view & projection matrices), and return the 8-8-8bit RGB pixel data and floating point depth values"}, + + {"getQuaternionFromEuler", pybullet_getQuaternionFromEuler, METH_VARARGS, + "Convert Euler [roll, pitch, yaw] as in URDF/SDF convention, to quaternion [x,y,z,w]"}, + + {"getEulerFromQuaternion", pybullet_getEulerFromQuaternion, METH_VARARGS, + "Convert quaternion [x,y,z,w] to Euler [roll, pitch, yaw] as in URDF/SDF convention"}, + + //todo(erwincoumans) //saveSnapshot //loadSnapshot + ////todo(erwincoumans) //collision info //raycast info - - //applyBaseForce - //applyLinkForce - - {"renderImage", pybullet_renderImage, METH_VARARGS, - "Render an image (given the pixel resolution width & height and camera view & projection matrices), and return the 8-8-8bit RGB pixel data and floating point depth values"}, - + {NULL, NULL, 0, NULL} /* Sentinel */ }; @@ -1155,7 +1403,7 @@ static PyMethodDef SpamMethods[] = { static struct PyModuleDef moduledef = { PyModuleDef_HEAD_INIT, "pybullet", /* m_name */ - "Python bindings for Bullet", /* m_doc */ + "Python bindings for Bullet Physics Robotics API (also known as Shared Memory API)", /* m_doc */ -1, /* m_size */ SpamMethods, /* m_methods */ NULL, /* m_reload */ @@ -1198,6 +1446,9 @@ initpybullet(void) PyModule_AddIntConstant (m, "VELOCITY_CONTROL", CONTROL_MODE_VELOCITY); // user read PyModule_AddIntConstant (m, "POSITION_CONTROL", CONTROL_MODE_POSITION_VELOCITY_PD); // user read + PyModule_AddIntConstant (m, "LINK_FRAME", EF_LINK_FRAME); + PyModule_AddIntConstant (m, "WORLD_FRAME", EF_WORLD_FRAME); + SpamError = PyErr_NewException("pybullet.error", NULL, NULL); Py_INCREF(SpamError); PyModule_AddObject(m, "error", SpamError);