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

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

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

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

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

4
examples/OpenGLWindow/GLInstancingRenderer.cpp
View File

@@ -19,7 +19,7 @@ subject to the following restrictions:
bool useShadowMap=true;//false;//true; bool useShadowMap=true;//false;//true;
int shadowMapWidth=4096;//8192; int shadowMapWidth=4096;//8192;
int shadowMapHeight=4096; int shadowMapHeight=4096;
float shadowMapWorldSize=50; float shadowMapWorldSize=25;
#define MAX_POINTS_IN_BATCH 1024 #define MAX_POINTS_IN_BATCH 1024
#define MAX_LINES_IN_BATCH 1024 #define MAX_LINES_IN_BATCH 1024
@@ -75,7 +75,7 @@ float shadowMapWorldSize=50;
static InternalDataRenderer* sData2; static InternalDataRenderer* sData2;
GLint lineWidthRange[2]={1,1}; GLint lineWidthRange[2]={1,1};
static b3Vector3 gLightPos=b3MakeVector3(-5,200,-40); static b3Vector3 gLightPos=b3MakeVector3(-5,12,-4);
struct b3GraphicsInstance struct b3GraphicsInstance
{ {

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; vec4 texel = fragment.color*texture(Diffuse,vert.texcoord);//fragment.color;
vec3 ct,cf; vec3 ct,cf;
float intensity,at,af; 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; cf = intensity*(vec3(1.0,1.0,1.0)-ambient)+ambient;
af = 1.0; 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" " vec4 texel = fragment.color*texture(Diffuse,vert.texcoord);//fragment.color;\n"
" vec3 ct,cf;\n" " vec3 ct,cf;\n"
" float intensity,at,af;\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" " cf = intensity*(vec3(1.0,1.0,1.0)-ambient)+ambient;\n"
" af = 1.0;\n" " af = 1.0;\n"
" \n" " \n"

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

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

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

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

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

@@ -27,8 +27,7 @@ void main(void)
vec3 ct,cf; vec3 ct,cf;
float intensity,at,af; 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; af = 1.0;

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

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

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

@@ -65,7 +65,7 @@ out vec3 lightDir,normal,ambient;
void main(void) void main(void)
{ {
vec4 q = instance_quaternion; vec4 q = instance_quaternion;
ambient = vec3(0.6,.6,0.6); ambient = vec3(0.5,.5,0.5);
vec4 worldNormal = (quatRotate3( vertexnormal,q)); 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" "void main(void)\n"
"{\n" "{\n"
" vec4 q = instance_quaternion;\n" " vec4 q = instance_quaternion;\n"
" ambient = vec3(0.6,.6,0.6);\n" " ambient = vec3(0.5,.5,0.5);\n"
" \n" " \n"
" vec4 worldNormal = (quatRotate3( vertexnormal,q));\n" " vec4 worldNormal = (quatRotate3( vertexnormal,q));\n"
" \n" " \n"

107
examples/RoboticsLearning/R2D2GraspExample.cpp
View File

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

81
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; int robotUniqueId = -1;
b3Assert(m_data->m_connected); b3Assert(m_data->m_connected);
switch (args.m_fileType)
{ {
b3SharedMemoryStatusHandle statusHandle; 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
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; int statusType;
b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(m_data->m_physicsClient, args.m_urdfFileName.c_str()); b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(m_data->m_physicsClient, args.m_fileName.c_str());
statusHandle = submitClientCommandAndWaitStatusMultiThreaded(m_data->m_physicsClient, command);
//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);
statusType = b3GetStatusType(statusHandle); statusType = b3GetStatusType(statusHandle);
b3Assert(statusType==CMD_URDF_LOADING_COMPLETED); b3Assert(statusType == CMD_SDF_LOADING_COMPLETED);
robotUniqueId = b3GetStatusBodyIndex(statusHandle); 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) bool b3RobotSimAPI::connect(GUIHelperInterface* guiHelper)

32
examples/RoboticsLearning/b3RobotSimAPI.h
View File

@@ -6,22 +6,40 @@
#include "../SharedMemory/SharedMemoryPublic.h" #include "../SharedMemory/SharedMemoryPublic.h"
#include "Bullet3Common/b3Vector3.h" #include "Bullet3Common/b3Vector3.h"
#include "Bullet3Common/b3Quaternion.h" #include "Bullet3Common/b3Quaternion.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
#include <string> #include <string>
enum b3RigidSimFileType
struct b3RobotSimLoadURDFArgs
{ {
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; b3Vector3 m_startPosition;
b3Quaternion m_startOrientation; b3Quaternion m_startOrientation;
bool m_forceOverrideFixedBase; bool m_forceOverrideFixedBase;
int m_fileType;
b3RobotSimLoadURDFArgs(const std::string& urdfFileName) b3RobotSimLoadFileArgs(const std::string& fileName)
:m_urdfFileName(urdfFileName), :m_fileName(fileName),
m_startPosition(b3MakeVector3(0,0,0)), m_startPosition(b3MakeVector3(0,0,0)),
m_startOrientation(b3Quaternion(0,0,0,1)), 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); bool connect(struct GUIHelperInterface* guiHelper);
void disconnect(); void disconnect();
int loadURDF(const struct b3RobotSimLoadURDFArgs& args); bool loadFile(const struct b3RobotSimLoadFileArgs& args, b3RobotSimLoadFileResults& results=b3RobotSimLoadFileResults());
int getNumJoints(int bodyUniqueId) const; int getNumJoints(int bodyUniqueId) const;

2
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -33,7 +33,7 @@
#define MAX_SDF_FILENAME_LENGTH 1024 #define MAX_SDF_FILENAME_LENGTH 1024
#define MAX_FILENAME_LENGTH MAX_URDF_FILENAME_LENGTH #define MAX_FILENAME_LENGTH MAX_URDF_FILENAME_LENGTH
#define MAX_NUM_LINKS MAX_DEGREE_OF_FREEDOM #define MAX_NUM_LINKS MAX_DEGREE_OF_FREEDOM
#define MAX_SDF_BODIES 1024 #define MAX_SDF_BODIES 500
struct TmpFloat3 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++) for (int i=0;i<numVertices;i++)
{ {
m_model->addVertex(vertices[i*9], 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); 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) 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)); 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 loadDiffuseTexture(const char* relativeFileName);
void setDiffuseTextureFromData(unsigned char* textureImage,int textureWidth,int textureHeight); 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 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, void addTriangle(int vertexposIndex0, int normalIndex0, int uvIndex0,
int vertexposIndex1, int normalIndex1, int uvIndex1, int vertexposIndex1, int normalIndex1, int uvIndex1,