Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Add kiva_shelf to prepare for picking/grasping task

Browse Source 
Fix uninitialized variable jointDamping/jointFriction in SDF importer
Add SDF <pose> parsing in visual, inertial, collision elements.
Slight improvement in TinyRender loading performance of largish meshes (30k vertices)
Reduce #define MAX_SDF_BODIES to 500, due to some issue in client code, todo: figure out what the issue is.
b3RobotSimAPI support SDF file loading
Tiny improvement in OpenGL hardware renderer lighting, to distinguish faces without textures
This commit is contained in:
erwin coumans
2016-07-14 00:05:57 -07:00
parent 10cc6f14cb
commit 4a705d1e03
25 changed files with 399 additions and 88 deletions
Download Patch File Download Diff File Expand all files Collapse all files

107
examples/RoboticsLearning/R2D2GraspExample.cpp
View File

@@ -59,50 +59,89 @@ public:
{
bool connected = m_robotSim.connect(m_guiHelper);
b3Printf("robotSim connected = %d",connected);
b3RobotSimLoadURDFArgs args("");
if ((m_options & eROBOTIC_LEARN_GRASP)!=0)
{
args.m_urdfFileName = "r2d2.urdf";
args.m_startPosition.setValue(0,0,.5);
m_r2d2Index = m_robotSim.loadURDF(args);
int numJoints = m_robotSim.getNumJoints(m_r2d2Index);
b3Printf("numJoints = %d",numJoints);
{
b3RobotSimLoadFileArgs args("");
args.m_fileName = "r2d2.urdf";
args.m_startPosition.setValue(0,0,.5);
b3RobotSimLoadFileResults results;
if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1)
{
int m_r2d2Index = results.m_uniqueObjectIds[0];
int numJoints = m_robotSim.getNumJoints(m_r2d2Index);
b3Printf("numJoints = %d",numJoints);
for (int i=0;i<numJoints;i++)
{
b3JointInfo jointInfo;
m_robotSim.getJointInfo(m_r2d2Index,i,&jointInfo);
b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName);
for (int i=0;i<numJoints;i++)
{
b3JointInfo jointInfo;
m_robotSim.getJointInfo(m_r2d2Index,i,&jointInfo);
b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName);
}
int wheelJointIndices[4]={2,3,6,7};
int wheelTargetVelocities[4]={-10,-10,-10,-10};
for (int i=0;i<4;i++)
{
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_targetVelocity = wheelTargetVelocities[i];
controlArgs.m_maxTorqueValue = 1e30;
m_robotSim.setJointMotorControl(m_r2d2Index,wheelJointIndices[i],controlArgs);
}
}
}
int wheelJointIndices[4]={2,3,6,7};
int wheelTargetVelocities[4]={30,30,30,30};
for (int i=0;i<4;i++)
{
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_targetVelocity = wheelTargetVelocities[i];
controlArgs.m_maxTorqueValue = 1e30;
m_robotSim.setJointMotorControl(m_r2d2Index,wheelJointIndices[i],controlArgs);
b3RobotSimLoadFileArgs args("");
args.m_fileName = "kiva_shelf/model.sdf";
args.m_startPosition.setValue(0,0,.5);
args.m_startOrientation = b3Quaternion(0,B3_HALF_PI,0);
args.m_forceOverrideFixedBase = true;
args.m_fileType = B3_SDF_FILE;
//args.m_startOrientation = b3Quaternion()
m_robotSim.loadFile(args);
}
{
b3RobotSimLoadFileArgs args("");
args.m_fileName = "plane.urdf";
args.m_startPosition.setValue(0,0,0);
args.m_forceOverrideFixedBase = true;
b3RobotSimLoadFileResults results;
m_robotSim.loadFile(args,results);
m_robotSim.setGravity(b3MakeVector3(0,0,-10));
}
}
if ((m_options & eROBOTIC_LEARN_COMPLIANT_CONTACT)!=0)
{
args.m_urdfFileName = "cube.urdf";
args.m_startPosition.setValue(0,0,2.5);
m_robotSim.loadURDF(args);
args.m_startOrientation.setEulerZYX(0,0.2,0);
b3RobotSimLoadFileArgs args("");
b3RobotSimLoadFileResults results;
{
args.m_fileName = "cube.urdf";
args.m_startPosition.setValue(0,0,2.5);
args.m_startOrientation.setEulerZYX(0,0.2,0);
m_robotSim.loadFile(args,results);
}
{
args.m_fileName = "cube_no_friction.urdf";
args.m_startPosition.setValue(0,2,2.5);
args.m_startOrientation.setEulerZYX(0,0.2,0);
m_robotSim.loadFile(args,results);
}
{
b3RobotSimLoadFileArgs args("");
args.m_fileName = "plane.urdf";
args.m_startPosition.setValue(0,0,0);
args.m_startOrientation.setEulerZYX(0,0.2,0);
args.m_forceOverrideFixedBase = true;
b3RobotSimLoadFileResults results;
m_robotSim.loadFile(args,results);
m_robotSim.setGravity(b3MakeVector3(0,0,-10));
}
}
args.m_urdfFileName = "plane.urdf";
args.m_startPosition.setValue(0,0,0);
args.m_forceOverrideFixedBase = true;
m_robotSim.loadURDF(args);
m_robotSim.setGravity(b3MakeVector3(0,0,-10));
}
virtual void exitPhysics()
{
@@ -139,10 +178,10 @@ public:
virtual void resetCamera()
{
float dist = 10;
float pitch = 50;
float yaw = 13;
float targetPos[3]={-1,0,-0.3};
float dist = 3;
float pitch = -75;
float yaw = 30;
float targetPos[3]={-0.2,0.8,0.3};
if (m_app->m_renderer && m_app->m_renderer->getActiveCamera())
{
m_app->m_renderer->getActiveCamera()->setCameraDistance(dist);

79
examples/RoboticsLearning/b3RobotSimAPI.cpp
View File

@@ -598,36 +598,71 @@ void b3RobotSimAPI::setJointMotorControl(int bodyUniqueId, int jointIndex, const
}
}
int b3RobotSimAPI::loadURDF(const b3RobotSimLoadURDFArgs& args)
bool b3RobotSimAPI::loadFile(const struct b3RobotSimLoadFileArgs& args, b3RobotSimLoadFileResults& results)
{
bool statusOk = false;
int robotUniqueId = -1;
b3Assert(m_data->m_connected);
switch (args.m_fileType)
{
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(m_data->m_physicsClient, args.m_urdfFileName.c_str());
case B3_URDF_FILE:
{
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(m_data->m_physicsClient, args.m_fileName.c_str());
//setting the initial position, orientation and other arguments are optional
//setting the initial position, orientation and other arguments are optional
b3LoadUrdfCommandSetStartPosition(command, args.m_startPosition[0],
args.m_startPosition[1],
args.m_startPosition[2]);
b3LoadUrdfCommandSetStartOrientation(command,args.m_startOrientation[0]
,args.m_startOrientation[1]
,args.m_startOrientation[2]
,args.m_startOrientation[3]);
if (args.m_forceOverrideFixedBase)
{
b3LoadUrdfCommandSetUseFixedBase(command,true);
}
statusHandle = submitClientCommandAndWaitStatusMultiThreaded(m_data->m_physicsClient, command);
b3LoadUrdfCommandSetStartPosition(command, args.m_startPosition[0],
args.m_startPosition[1],
args.m_startPosition[2]);
b3LoadUrdfCommandSetStartOrientation(command,args.m_startOrientation[0]
,args.m_startOrientation[1]
,args.m_startOrientation[2]
,args.m_startOrientation[3]);
if (args.m_forceOverrideFixedBase)
{
b3LoadUrdfCommandSetUseFixedBase(command,true);
}
statusHandle = submitClientCommandAndWaitStatusMultiThreaded(m_data->m_physicsClient, command);
statusType = b3GetStatusType(statusHandle);
b3Assert(statusType==CMD_URDF_LOADING_COMPLETED);
robotUniqueId = b3GetStatusBodyIndex(statusHandle);
results.m_uniqueObjectIds.push_back(robotUniqueId);
statusOk = true;
break;
}
case B3_SDF_FILE:
{
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(m_data->m_physicsClient, args.m_fileName.c_str());
statusHandle = submitClientCommandAndWaitStatusMultiThreaded(m_data->m_physicsClient, command);
statusType = b3GetStatusType(statusHandle);
b3Assert(statusType==CMD_URDF_LOADING_COMPLETED);
robotUniqueId = b3GetStatusBodyIndex(statusHandle);
}
b3Assert(statusType == CMD_SDF_LOADING_COMPLETED);
if (statusType == CMD_SDF_LOADING_COMPLETED)
{
int numBodies = b3GetStatusBodyIndices(statusHandle, 0,0);
if (numBodies)
{
results.m_uniqueObjectIds.resize(numBodies);
int numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0],results.m_uniqueObjectIds.size());
return robotUniqueId;
}
statusOk = true;
}
break;
}
default:
{
b3Warning("Unknown file type in b3RobotSimAPI::loadFile");
}
}
return statusOk;
}
bool b3RobotSimAPI::connect(GUIHelperInterface* guiHelper)

32
examples/RoboticsLearning/b3RobotSimAPI.h
View File

@@ -6,22 +6,40 @@
#include "../SharedMemory/SharedMemoryPublic.h"
#include "Bullet3Common/b3Vector3.h"
#include "Bullet3Common/b3Quaternion.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
#include <string>
struct b3RobotSimLoadURDFArgs
enum b3RigidSimFileType
{
std::string m_urdfFileName;
B3_URDF_FILE=1,
B3_SDF_FILE,
B3_AUTO_DETECT_FILE//todo based on extension
};
struct b3RobotSimLoadFileArgs
{
std::string m_fileName;
b3Vector3 m_startPosition;
b3Quaternion m_startOrientation;
bool m_forceOverrideFixedBase;
int m_fileType;
b3RobotSimLoadURDFArgs(const std::string& urdfFileName)
:m_urdfFileName(urdfFileName),
b3RobotSimLoadFileArgs(const std::string& fileName)
:m_fileName(fileName),
m_startPosition(b3MakeVector3(0,0,0)),
m_startOrientation(b3Quaternion(0,0,0,1)),
m_forceOverrideFixedBase(false)
m_forceOverrideFixedBase(false),
m_fileType(B3_URDF_FILE)
{
}
};
struct b3RobotSimLoadFileResults
{
b3AlignedObjectArray<int> m_uniqueObjectIds;
b3RobotSimLoadFileResults()
{
}
};
@@ -63,7 +81,7 @@ public:
bool connect(struct GUIHelperInterface* guiHelper);
void disconnect();
int loadURDF(const struct b3RobotSimLoadURDFArgs& args);
bool loadFile(const struct b3RobotSimLoadFileArgs& args, b3RobotSimLoadFileResults& results=b3RobotSimLoadFileResults());
int getNumJoints(int bodyUniqueId) const;