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

Merge pull request #699 from erwincoumans/master

Browse Source 
Add kiva_shelf to prepare for picking/grasping task
This commit is contained in:
erwincoumans
2016-07-14 10:51:34 -07:00
committed by GitHub
parent 31e7f2e309 34187fba1c
commit 824c721710
26 changed files with 403 additions and 89 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
data/cube_no_friction.urdf Normal file
View File

@@ -0,0 +1,32 @@
<?xml version="0.0" ?>
<robot name="cube.urdf">
<link name="baseLink">
<contact>
<lateral_friction value="0.0"/>
<rolling_friction value="0.0"/>
<contact_cfm value="0.0"/>
<contact_erp value="1.0"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="1.0"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="cube.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="1 1 1"/>
</geometry>
</collision>
</link>
</robot>

7
data/kiva_shelf/0_Bullet3Demo.txt Normal file
View File

@@ -0,0 +1,7 @@
--start_demo_name=R2D2 Grasp
--mouse_move_multiplier=0.400000
--mouse_wheel_multiplier=0.010000
--background_color_red= 0.900000
--background_color_green= 0.900000
--background_color_blue= 1.000000
--fixed_timestep= 0.000000

BIN
data/kiva_shelf/meshes/pod_lowres.stl Normal file
View File

Binary file not shown.

55
data/kiva_shelf/model.sdf Normal file
View File

@@ -0,0 +1,55 @@
<?xml version="1.0" ?>
<sdf version="1.4">
<model name="Amazon Pod">
<static>1</static>
<pose>0 2 1.21 0 0 0</pose>
<link name="pod_link">
<inertial>
<pose>0.0 0.0 1.2045 0 0 0</pose>
<mass>76.26</mass>
<inertia>
<ixx>47</ixx>
<ixy>-0.003456</ixy>
<ixz>0.001474</ixz>
<izz>13.075</izz>
<iyz>-0.014439</iyz>
<iyy>47</iyy>
</inertia>
</inertial>
<visual name="pod_visual">
<pose>0 0 0 1.5707 0 0 </pose>
<geometry>
<mesh>
<uri>meshes/pod_lowres.stl</uri>
</mesh>
</geometry>
<material>
<diffuse>0.9 0.8 0.5 1</diffuse>
</material>
</visual>
<collision name="pod_collision">
<pose>0 0 0 1.5707 0 0</pose>
<geometry>
<mesh>
<uri>meshes/pod_lowres.stl</uri>
</mesh>
</geometry>
<surface>
<friction>
<ode>
<mu>0.8</mu>
<mu2>0.8</mu2>
<fdir1>0.0 0.0 0.0</fdir1>
<slip1>1.0</slip1>
<slip2>1.0</slip2>
</ode>
</friction>
</surface>
</collision>
</link>
</model>
</sdf>

45
data/kuka_iiwa/model.sdf
View File

@@ -1,6 +1,7 @@
<sdf version='1.6'>
<world name='default'>
<model name='lbr_iiwa'>
<static>1</static>
<link name='lbr_iiwa_link_0'>
<pose frame=''>0 0 0 0 -0 0</pose>
<inertial>
@@ -34,7 +35,7 @@
</geometry>
<material>
<ambient>1 0 0 1</ambient>
<diffuse>0 0 1 1</diffuse>
<diffuse>0.2 0.2 0.2 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
@@ -71,6 +72,12 @@
<uri>meshes/link_1.stl</uri>
</mesh>
</geometry>
<material>
<ambient>1 0 0 1</ambient>
<diffuse>0.5 0.7 1.0 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
</link>
<joint name='lbr_iiwa_joint_1' type='revolute'>
@@ -124,6 +131,12 @@
<uri>meshes/link_2.stl</uri>
</mesh>
</geometry>
<material>
<ambient>1 0 0 1</ambient>
<diffuse>0.5 0.7 1.0 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
</link>
<joint name='lbr_iiwa_joint_2' type='revolute'>
@@ -177,6 +190,12 @@
<uri>meshes/link_3.stl</uri>
</mesh>
</geometry>
<material>
<ambient>1 0 0 1</ambient>
<diffuse>1.0 0.42 0.04 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
</link>
<joint name='lbr_iiwa_joint_3' type='revolute'>
@@ -230,6 +249,12 @@
<uri>meshes/link_4.stl</uri>
</mesh>
</geometry>
<material>
<ambient>1 0 0 1</ambient>
<diffuse>0.5 0.7 1.0 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
</link>
<joint name='lbr_iiwa_joint_4' type='revolute'>
@@ -283,6 +308,12 @@
<uri>meshes/link_5.stl</uri>
</mesh>
</geometry>
<material>
<ambient>1 0 0 1</ambient>
<diffuse>0.5 0.7 1.0 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
</link>
<joint name='lbr_iiwa_joint_5' type='revolute'>
@@ -336,6 +367,12 @@
<uri>meshes/link_6.stl</uri>
</mesh>
</geometry>
<material>
<ambient>1 0 0 1</ambient>
<diffuse>1.0 0.42 0.04 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
</link>
<joint name='lbr_iiwa_joint_6' type='revolute'>
@@ -389,6 +426,12 @@
<uri>meshes/link_7.stl</uri>
</mesh>
</geometry>
<material>
<ambient>1 0 0 1</ambient>
<diffuse>0.2 0.2 0.2 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
</link>
<joint name='lbr_iiwa_joint_7' type='revolute'>

26
data/plane.urdf Normal file
View File

@@ -0,0 +1,26 @@
<?xml version="0.0" ?>
<robot name="cube.urdf">
<link name="baseLink">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value=".0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="plane.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="10 10 0.001"/>
</geometry>
</collision>
</link>
</robot>

2
examples/ExampleBrowser/ExampleEntries.cpp
View File

@@ -248,7 +248,7 @@ static ExampleEntry gDefaultExamples[]=
ExampleEntry(1, "Physics Client (Direct)", "Create a physics client that can communicate with a physics server directly in-process.", PhysicsClientCreateFunc,eCLIENTEXAMPLE_DIRECT),
ExampleEntry(1,"R2D2 Grasp","Load the R2D2 robot from URDF file and control it to grasp objects", R2D2GraspExampleCreateFunc, eROBOTIC_LEARN_GRASP),
ExampleEntry(1,"URDF Compliant Contact","Experiment/improve compliant rigid contact using parameters from URDF file (contact_cfm, contact_erp, lateral_friction, rolling_friction)", R2D2GraspExampleCreateFunc,eROBOTIC_LEARN_COMPLIANT_CONTACT),
ExampleEntry(1,"URDF Compliant Contact","Work-in-progress, experiment/improve compliant rigid contact using parameters from URDF file (contact_cfm, contact_erp, lateral_friction, rolling_friction)", R2D2GraspExampleCreateFunc,eROBOTIC_LEARN_COMPLIANT_CONTACT),

2
examples/ExampleBrowser/OpenGLExampleBrowser.cpp
View File

@@ -223,7 +223,7 @@ void MyKeyboardCallback(int key, int state)
b3MouseMoveCallback prevMouseMoveCallback = 0;
static void MyMouseMoveCallback( float x, float y)
{
bool handled = false;
bool handled = false;
if (sCurrentDemo)
handled = sCurrentDemo->mouseMoveCallback(x,y);
if (!handled && gui)

57
examples/Importers/ImportURDFDemo/UrdfParser.cpp
View File

@@ -225,6 +225,15 @@ bool UrdfParser::parseInertia(UrdfInertia& inertia, TiXmlElement* config, ErrorL
{
inertia.m_linkLocalFrame.setIdentity();
inertia.m_mass = 0.f;
if(m_parseSDF)
{
TiXmlElement* pose = config->FirstChildElement("pose");
if (pose)
{
parseTransform(inertia.m_linkLocalFrame, pose,logger,m_parseSDF);
}
}
// Origin
@@ -448,6 +457,15 @@ bool UrdfParser::parseCollision(UrdfCollision& collision, TiXmlElement* config,
collision.m_linkLocalFrame.setIdentity();
if(m_parseSDF)
{
TiXmlElement* pose = config->FirstChildElement("pose");
if (pose)
{
parseTransform(collision.m_linkLocalFrame, pose,logger,m_parseSDF);
}
}
// Origin
TiXmlElement *o = config->FirstChildElement("origin");
if (o)
@@ -474,7 +492,15 @@ bool UrdfParser::parseCollision(UrdfCollision& collision, TiXmlElement* config,
bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement* config, ErrorLogger* logger)
{
visual.m_linkLocalFrame.setIdentity();
if(m_parseSDF)
{
TiXmlElement* pose = config->FirstChildElement("pose");
if (pose)
{
parseTransform(visual.m_linkLocalFrame, pose,logger,m_parseSDF);
}
}
// Origin
TiXmlElement *o = config->FirstChildElement("origin");
if (o)
@@ -505,6 +531,8 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement*
{
UrdfMaterial* matPtr = new UrdfMaterial;
matPtr->m_name = "mat";
if (name_char)
matPtr->m_name = name_char;
TiXmlElement *diffuse = mat->FirstChildElement("diffuse");
if (diffuse) {
std::string diffuseText = diffuse->GetText();
@@ -512,7 +540,7 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement*
parseVector4(rgba,diffuseText);
matPtr->m_rgbaColor = rgba;
model.m_materials.insert(matPtr->m_name.c_str(),matPtr);
visual.m_materialName = "mat";
visual.m_materialName = matPtr->m_name;
visual.m_hasLocalMaterial = true;
}
}
@@ -670,6 +698,8 @@ bool UrdfParser::parseJointLimits(UrdfJoint& joint, TiXmlElement* config, ErrorL
joint.m_upperLimit = 0.f;
joint.m_effortLimit = 0.f;
joint.m_velocityLimit = 0.f;
joint.m_jointDamping = 0.f;
joint.m_jointFriction = 0.f;
if (m_parseSDF)
{
@@ -1281,15 +1311,22 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
return false;
}
TiXmlElement *world_xml = sdf_xml->FirstChildElement("world");
if (!world_xml)
{
logger->reportError("expected a world element");
return false;
}
//apparently, SDF doesn't require a "world" element, optional? URDF does.
TiXmlElement *world_xml = sdf_xml->FirstChildElement("world");
TiXmlElement* robot_xml = 0;
if (!world_xml)
{
logger->reportWarning("expected a world element, continuing without it.");
robot_xml = sdf_xml->FirstChildElement("model");
} else
{
robot_xml = world_xml->FirstChildElement("model");
}
// Get all model (robot) elements
for (TiXmlElement* robot_xml = world_xml->FirstChildElement("model"); robot_xml; robot_xml = robot_xml->NextSiblingElement("model"))
for (; robot_xml; robot_xml = robot_xml->NextSiblingElement("model"))
{
UrdfModel* localModel = new UrdfModel;
m_tmpModels.push_back(localModel);

9
examples/Importers/ImportURDFDemo/UrdfParser.h
View File

@@ -125,6 +125,15 @@ struct UrdfJoint
double m_jointDamping;
double m_jointFriction;
UrdfJoint()
:m_lowerLimit(0),
m_upperLimit(0),
m_effortLimit(0),
m_velocityLimit(0),
m_jointDamping(0),
m_jointFriction(0)
{
}
};
struct UrdfModel

6
examples/OpenGLWindow/GLInstancingRenderer.cpp
View File

@@ -19,7 +19,7 @@ subject to the following restrictions:
bool useShadowMap=true;//false;//true;
int shadowMapWidth=4096;//8192;
int shadowMapHeight=4096;
float shadowMapWorldSize=50;
float shadowMapWorldSize=25;
#define MAX_POINTS_IN_BATCH 1024
#define MAX_LINES_IN_BATCH 1024
@@ -75,7 +75,7 @@ float shadowMapWorldSize=50;
static InternalDataRenderer* sData2;
GLint lineWidthRange[2]={1,1};
static b3Vector3 gLightPos=b3MakeVector3(-5,200,-40);
static b3Vector3 gLightPos=b3MakeVector3(-5,12,-4);
struct b3GraphicsInstance
{
@@ -956,6 +956,8 @@ void GLInstancingRenderer::updateCamera(int upAxis)
m_data->m_activeCamera->setCameraUpAxis(upAxis);
m_data->m_activeCamera->setAspectRatio((float)m_screenWidth/(float)m_screenHeight);
m_data->m_defaultCamera1.update();
m_data->m_activeCamera->getCameraProjectionMatrix(m_data->m_projectionMatrix);
m_data->m_activeCamera->getCameraViewMatrix(m_data->m_viewMatrix);
}

3
examples/OpenGLWindow/Shaders/instancingPS.glsl
View File

@@ -25,7 +25,8 @@ void main(void)
vec4 texel = fragment.color*texture(Diffuse,vert.texcoord);//fragment.color;
vec3 ct,cf;
float intensity,at,af;
intensity = max(dot(lightDir,normalize(normal)),0);
intensity = 0.5+0.5*clamp( dot( normalize(normal),lightDir ), -1,1 );
cf = intensity*(vec3(1.0,1.0,1.0)-ambient)+ambient;
af = 1.0;

3
examples/OpenGLWindow/Shaders/instancingPS.h
View File

@@ -22,7 +22,8 @@ static const char* instancingFragmentShader= \
" vec4 texel = fragment.color*texture(Diffuse,vert.texcoord);//fragment.color;\n"
" vec3 ct,cf;\n"
" float intensity,at,af;\n"
" intensity = max(dot(lightDir,normalize(normal)),0);\n"
" \n"
" intensity = 0.5+0.5*clamp( dot( normalize(normal),lightDir ), -1,1 );\n"
" cf = intensity*(vec3(1.0,1.0,1.0)-ambient)+ambient;\n"
" af = 1.0;\n"
" \n"

2
examples/OpenGLWindow/Shaders/instancingVS.glsl
View File

@@ -61,7 +61,7 @@ out vec3 lightDir,normal,ambient;
void main(void)
{
vec4 q = instance_quaternion;
ambient = vec3(0.6,.6,0.6);
ambient = vec3(0.5,.5,0.5);
vec4 worldNormal = (quatRotate3( vertexnormal,q));
normal = normalize(worldNormal).xyz;

2
examples/OpenGLWindow/Shaders/instancingVS.h
View File

@@ -52,7 +52,7 @@ static const char* instancingVertexShader= \
"void main(void)\n"
"{\n"
" vec4 q = instance_quaternion;\n"
" ambient = vec3(0.6,.6,0.6);\n"
" ambient = vec3(0.5,.5,0.5);\n"
" \n"
" vec4 worldNormal = (quatRotate3( vertexnormal,q));\n"
" normal = normalize(worldNormal).xyz;\n"

3
examples/OpenGLWindow/Shaders/useShadowMapInstancingPS.glsl
View File

@@ -27,8 +27,7 @@ void main(void)
vec3 ct,cf;
float intensity,at,af;
intensity = clamp( dot( normalize(normal),lightDir ), 0,1 );
intensity = 0.5+0.5*clamp( dot( normalize(normal),lightDir ), -1,1 );
af = 1.0;

3
examples/OpenGLWindow/Shaders/useShadowMapInstancingPS.h
View File

@@ -21,8 +21,7 @@ static const char* useShadowMapInstancingFragmentShader= \
" vec3 ct,cf;\n"
" float intensity,at,af;\n"
" \n"
" intensity = clamp( dot( normalize(normal),lightDir ), 0,1 );\n"
" \n"
" intensity = 0.5+0.5*clamp( dot( normalize(normal),lightDir ), -1,1 );\n"
" \n"
" af = 1.0;\n"
" \n"

2
examples/OpenGLWindow/Shaders/useShadowMapInstancingVS.glsl
View File

@@ -65,7 +65,7 @@ out vec3 lightDir,normal,ambient;
void main(void)
{
vec4 q = instance_quaternion;
ambient = vec3(0.6,.6,0.6);
ambient = vec3(0.5,.5,0.5);
vec4 worldNormal = (quatRotate3( vertexnormal,q));

2
examples/OpenGLWindow/Shaders/useShadowMapInstancingVS.h
View File

@@ -55,7 +55,7 @@ static const char* useShadowMapInstancingVertexShader= \
"void main(void)\n"
"{\n"
" vec4 q = instance_quaternion;\n"
" ambient = vec3(0.6,.6,0.6);\n"
" ambient = vec3(0.5,.5,0.5);\n"
" \n"
" vec4 worldNormal = (quatRotate3( vertexnormal,q));\n"
" \n"

108
examples/RoboticsLearning/R2D2GraspExample.cpp
View File

@@ -59,50 +59,90 @@ 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()
b3RobotSimLoadFileResults results;
m_robotSim.loadFile(args,results);
}
{
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 +179,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);
int getNumJoints(int bodyUniqueId) const;

2
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -33,7 +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
#define MAX_SDF_BODIES 500
struct TmpFloat3
{

1
examples/TinyRenderer/TinyRenderer.cpp
View File

@@ -138,6 +138,7 @@ void TinyRenderObjectData::registerMeshShape(const float* vertices, int numVerti
*/
}
m_model->reserveMemory(numVertices,numIndices);
for (int i=0;i<numVertices;i++)
{
m_model->addVertex(vertices[i*9],

8
examples/TinyRenderer/model.cpp
View File

@@ -76,6 +76,14 @@ void Model::loadDiffuseTexture(const char* relativeFileName)
diffusemap_.read_tga_file(relativeFileName);
}
void Model::reserveMemory(int numVertices, int numIndices)
{
verts_.reserve(numVertices);
norms_.reserve(numVertices);
uv_.reserve(numVertices);
faces_.reserve(numIndices);
}
void Model::addVertex(float x,float y,float z, float normalX, float normalY, float normalZ, float u, float v)
{
verts_.push_back(Vec3f(x,y,z));

1
examples/TinyRenderer/model.h
View File

@@ -32,6 +32,7 @@ public:
}
void loadDiffuseTexture(const char* relativeFileName);
void setDiffuseTextureFromData(unsigned char* textureImage,int textureWidth,int textureHeight);
void reserveMemory(int numVertices, int numIndices);
void addVertex(float x,float y,float z, float normalX, float normalY, float normalZ, float u, float v);
void addTriangle(int vertexposIndex0, int normalIndex0, int uvIndex0,
int vertexposIndex1, int normalIndex1, int uvIndex1,