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

Merge pull request #815 from YunfeiBai/master

Browse Source 
Rearrange the scene for VR demo.
This commit is contained in:
erwincoumans
2016-09-30 14:33:13 -07:00
committed by GitHub
parent d86cce521c 65091c309e
commit a287227384
7 changed files with 180 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
data/gripper/wsg50_one_motor_gripper_new_free_base.sdf
View File

@@ -2,7 +2,7 @@
<sdf version='1.6'>
<world name='default'>
<model name='wsg50_with_gripper'>
<pose frame=''>0 -2.3 2.1 0 0 0</pose>
<pose frame=''>1.4 -0.2 2.1 0 0 0</pose>
<link name='world'>
<pose frame=''>0 0 0 0 0 0</pose>
<inertial>

27
data/table/table.urdf
View File

@@ -24,6 +24,33 @@
<box size="1.5 1 0.05"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="-0.65 -0.4 0.29"/>
<geometry>
<mesh filename="table.obj" scale="0.1 0.1 0.58"/>
</geometry>
<material name="framemat0"/>
</visual>
<visual>
<origin rpy="0 0 0" xyz="-0.65 0.4 0.29"/>
<geometry>
<mesh filename="table.obj" scale="0.1 0.1 0.58"/>
</geometry>
<material name="framemat0"/>
</visual>
<visual>
<origin rpy="0 0 0" xyz="0.65 -0.4 0.29"/>
<geometry>
<mesh filename="table.obj" scale="0.1 0.1 0.58"/>
</geometry>
<material name="framemat0"/>
</visual>
<visual>
<origin rpy="0 0 0" xyz="0.65 0.4 0.29"/>
<geometry>
<mesh filename="table.obj" scale="0.1 0.1 0.58"/>
</geometry>
</visual>
</link>
</robot>

BIN
data/table_square/checker_grid.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 32 KiB

16
data/table_square/table.mtl Normal file
View File

@@ -0,0 +1,16 @@
newmtl table
Ns 10.0000
Ni 1.5000
d 1.0000
Tr 0.0000
Tf 1.0000 1.0000 1.0000
illum 2
Ka 0.0000 0.0000 0.0000
Kd 0.5880 0.5880 0.5880
Ks 0.0000 0.0000 0.0000
Ke 0.0000 0.0000 0.0000
map_Ka table.tga
map_Kd checker_grid.jpg

48
data/table_square/table.obj Normal file
View File

@@ -0,0 +1,48 @@
# table.obj
#
o table
mtllib table.mtl
v -0.500000 -0.500000 0.500000
v 0.500000 -0.500000 0.500000
v -0.500000 0.500000 0.500000
v 0.500000 0.500000 0.500000
v -0.500000 0.500000 -0.500000
v 0.500000 0.500000 -0.500000
v -0.500000 -0.500000 -0.500000
v 0.500000 -0.500000 -0.500000
vt 0.000000 0.000000
vt 1.000000 0.000000
vt 0.000000 1.000000
vt 1.000000 1.000000
vn 0.000000 0.000000 1.000000
vn 0.000000 1.000000 0.000000
vn 0.000000 0.000000 -1.000000
vn 0.000000 -1.000000 0.000000
vn 1.000000 0.000000 0.000000
vn -1.000000 0.000000 0.000000
g table
usemtl table
s 1
f 1/1/1 2/2/1 3/3/1
f 3/3/1 2/2/1 4/4/1
s 2
f 3 4 5
f 5 4 6
s 3
f 5 6 7
f 7 6 8
s 4
f 7 8 1
f 1 8 2
s 5
f 2 8 4
f 4 8 6
s 6
f 7 1 5
f 5 1 3

61
data/table_square/table_square.urdf Normal file
View File

@@ -0,0 +1,61 @@
<?xml version="0.0" ?>
<robot name="urdf_table">
<link name="world"/>
<joint name="fixed" type="fixed">
<parent link="world"/>
<child link="baseLink"/>
<origin xyz="0 0 0"/>
</joint>
<link name="baseLink">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.6"/>
<mass value="0"/>
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0.6"/>
<geometry>
<mesh filename="table.obj" scale="0.6 0.6 0.08"/>
</geometry>
<material name="framemat0">
<color
rgba="1 1 1 1" />
</material>
</visual>
<visual>
<origin rpy="0 0 0" xyz="-0.25 -0.25 0.28"/>
<geometry>
<box size="0.05 0.05 0.56"/>
</geometry>
<material name="framemat0"/>
</visual>
<visual>
<origin rpy="0 0 0" xyz="-0.25 0.25 0.28"/>
<geometry>
<box size="0.05 0.05 0.56"/>
</geometry>
<material name="framemat0"/>
</visual>
<visual>
<origin rpy="0 0 0" xyz="0.25 -0.25 0.28"/>
<geometry>
<box size="0.05 0.05 0.56"/>
</geometry>
<material name="framemat0"/>
</visual>
<visual>
<origin rpy="0 0 0" xyz="0.25 0.25 0.28"/>
<geometry>
<box size="0.05 0.05 0.56"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.6"/>
<geometry>
<box size="0.6 0.6 0.08"/>
</geometry>
</collision>
</link>
</robot>

56
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -3005,12 +3005,12 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
}
}
loadUrdf("kuka_iiwa/model.urdf", btVector3(0, -2.3, 0.6), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("kuka_iiwa/model.urdf", btVector3(1.4, -0.2, 0.6), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
m_data->m_KukaId = bodyId;
loadUrdf("lego/lego.urdf", btVector3(-0.4, -2.3, .7), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("lego/lego.urdf", btVector3(-0.4, -2.3, .8), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("lego/lego.urdf", btVector3(-0.4, -2.3, .9), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("r2d2.urdf", btVector3(2, -2, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .7), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .8), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .9), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("r2d2.urdf", btVector3(-2, -4, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
// Load one motor gripper for kuka
loadSdf("gripper/wsg50_one_motor_gripper_new_free_base.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true);
@@ -3033,7 +3033,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
for (int i = 0; i < 6; i++)
{
loadUrdf("jenga/jenga.urdf", btVector3(-.2-0.1*i,-2., .75), btQuaternion(btVector3(0,1,0),SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("jenga/jenga.urdf", btVector3(1.3-0.1*i,-0.7, .75), btQuaternion(btVector3(0,1,0),SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
}
//loadUrdf("nao/nao.urdf", btVector3(2,5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
@@ -3075,38 +3075,36 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
for (int i = 0; i < 10; i++)
{
loadUrdf("cube.urdf", btVector3(3, -2, 0.5 + i), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("cube.urdf", btVector3(-4, -2, 0.5 + i), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
}
loadUrdf("sphere2.urdf", btVector3(-5, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("sphere2.urdf", btVector3(-5, 0, 2), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("sphere2.urdf", btVector3(-5, 0, 3), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
btTransform objectLocalTr[] = {
btTransform(btQuaternion(0, 0, 0, 1), btVector3(0, -1.9, 0.0)),
btTransform(btQuaternion(0, 0, 0, 1), btVector3(0, -1.9, 0.64)),
btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.5, -2.2, 0.85)),
btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.4, -2.0, 0.85)),
btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.1, -2.1, 0.7)),
btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.1, -2.0, 0.7)),
btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.2, -1.9, 0.7)),
btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.2, -1.9, 0.9))
btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.0, 0.0, 0.0)),
btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.0, 0.15, 0.64)),
btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.1, 0.15, 0.85)),
btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.4, 0.05, 0.85)),
btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.3, -0.05, 0.7)),
btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.1, 0.05, 0.7)),
btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.2, 0.15, 0.7)),
btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.2, 0.15, 0.9))
};
btAlignedObjectArray<btTransform> objectWorldTr;
int numOb = sizeof(objectLocalTr) / sizeof(btTransform);
objectWorldTr.resize(numOb);
btTransform tr2;
tr2.setIdentity();
tr2.setOrigin(btVector3(2.3, -2.45, 0));
btTransform tr;
tr.setIdentity();
tr.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_HALF_PI));
tr.setOrigin(btVector3(1.0, -0.2, 0));
for (int i = 0; i < numOb; i++)
{
objectWorldTr[i] = tr*tr2.inverse()*objectLocalTr[i];
objectWorldTr[i] = tr*objectLocalTr[i];
}
// Table area
@@ -3126,15 +3124,15 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
loadUrdf("cube_small.urdf", btVector3(0.3, 0.6, 0.85), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
// Chess area
loadUrdf("table_square.urdf", btVector3(2.0, 0, 0.0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("pawn.urdf", btVector3(1.8, -0.1, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("queen.urdf", btVector3(1.9, -0.2, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("king.urdf", btVector3(2.0, 0, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("bishop.urdf", btVector3(2.1, 0.1, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("rook.urdf", btVector3(2.2, 0, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("knight.urdf", btVector3(2.2, 0.2, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("table_square/table_square.urdf", btVector3(-1.0, 0, 0.0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("pawn.urdf", btVector3(-0.8, -0.1, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("queen.urdf", btVector3(-0.9, -0.2, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("king.urdf", btVector3(-1.0, 0, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("bishop.urdf", btVector3(-1.1, 0.1, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("rook.urdf", btVector3(-1.2, 0, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("knight.urdf", btVector3(-1.2, 0.2, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("husky/husky.urdf", btVector3(5, 2, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("husky/husky.urdf", btVector3(2, -5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
m_data->m_huskyId = bodyId;
m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, -10));
@@ -3197,7 +3195,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
{
btMultiBody* mb = bodyHandle->m_multiBody;
btScalar sqLen = (mb->getBaseWorldTransform().getOrigin() - gVRController2Pos).length2();
btScalar distanceThreshold = 1.0;
btScalar distanceThreshold = 1.3;
bool closeToKuka=(sqLen<(distanceThreshold*distanceThreshold));
int numDofs = bodyHandle->m_multiBody->getNumDofs();