This commit is contained in:
Erwin Coumans
2017-03-07 22:37:35 -08:00
29 changed files with 102 additions and 50 deletions

View File

@@ -49,30 +49,30 @@
</visual>
<asset>
<mesh name="index0" file="index0.stl"/>
<mesh name="index1" file="index1.stl"/>
<mesh name="index2" file="index2.stl"/>
<mesh name="index3" file="index3.stl"/>
<mesh name="middle0" file="middle0.stl"/>
<mesh name="middle1" file="middle1.stl"/>
<mesh name="middle2" file="middle2.stl"/>
<mesh name="middle3" file="middle3.stl"/>
<mesh name="index0" file="index0_collision.stl"/>
<mesh name="index1" file="index1_collision.stl"/>
<mesh name="index2" file="index2_collision.stl"/>
<mesh name="index3" file="index3_collision.stl"/>
<mesh name="middle0" file="middle0_collision.stl"/>
<mesh name="middle1" file="middle1_collision.stl"/>
<mesh name="middle2" file="middle2_collision.stl"/>
<mesh name="middle3" file="middle3_collision.stl"/>
<mesh name="palm" file="palm.stl"/>
<mesh name="pinky0" file="pinky0.stl"/>
<mesh name="pinky1" file="pinky1.stl"/>
<mesh name="pinky2" file="pinky2.stl"/>
<mesh name="pinky3" file="pinky3.stl"/>
<mesh name="ring0" file="ring0.stl"/>
<mesh name="ring1" file="ring1.stl"/>
<mesh name="ring2" file="ring2.stl"/>
<mesh name="ring3" file="ring3.stl"/>
<mesh name="thumb0" file="thumb0.stl"/>
<mesh name="thumb1" file="thumb1.stl"/>
<mesh name="thumb2" file="thumb2.stl"/>
<mesh name="thumb3" file="thumb3.stl"/>
<mesh name="wristx" file="wristx.stl"/>
<mesh name="wristy" file="wristy.stl"/>
<mesh name="wristz" file="wristz.stl"/>
<mesh name="pinky0" file="pinky0_collision.stl"/>
<mesh name="pinky1" file="pinky1_collision.stl"/>
<mesh name="pinky2" file="pinky2_collision.stl"/>
<mesh name="pinky3" file="pinky3_collision.stl"/>
<mesh name="ring0" file="ring0_collision.stl"/>
<mesh name="ring1" file="ring1_collision.stl"/>
<mesh name="ring2" file="ring2_collision.stl"/>
<mesh name="ring3" file="ring3_collision.stl"/>
<mesh name="thumb0" file="thumb0_collision.stl"/>
<mesh name="thumb1" file="thumb1_collision.stl"/>
<mesh name="thumb2" file="thumb2_collision.stl"/>
<mesh name="thumb3" file="thumb3_collision.stl"/>
<mesh name="wristx" file="wristx_collision.stl"/>
<mesh name="wristy" file="wristy_collision.stl"/>
<mesh name="wristz" file="wristz_collision.stl"/>
<texture type="skybox" builtin="gradient" rgb1=".4 .6 .8" rgb2="0 0 0"
width="100" height="100"/>

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

View File

@@ -49,30 +49,30 @@
</visual>
<asset>
<mesh name="index0" file="index0.stl"/>
<mesh name="index1" file="index1.stl"/>
<mesh name="index2" file="index2.stl"/>
<mesh name="index3" file="index3.stl"/>
<mesh name="middle0" file="middle0.stl"/>
<mesh name="middle1" file="middle1.stl"/>
<mesh name="middle2" file="middle2.stl"/>
<mesh name="middle3" file="middle3.stl"/>
<mesh name="palm" file="palm.stl"/>
<mesh name="pinky0" file="pinky0.stl"/>
<mesh name="pinky1" file="pinky1.stl"/>
<mesh name="pinky2" file="pinky2.stl"/>
<mesh name="pinky3" file="pinky3.stl"/>
<mesh name="ring0" file="ring0.stl"/>
<mesh name="ring1" file="ring1.stl"/>
<mesh name="ring2" file="ring2.stl"/>
<mesh name="ring3" file="ring3.stl"/>
<mesh name="thumb0" file="thumb0.stl"/>
<mesh name="thumb1" file="thumb1.stl"/>
<mesh name="thumb2" file="thumb2.stl"/>
<mesh name="thumb3" file="thumb3.stl"/>
<mesh name="wristx" file="wristx.stl"/>
<mesh name="wristy" file="wristy.stl"/>
<mesh name="wristz" file="wristz.stl"/>
<mesh name="index0" file="index0_collision.stl"/>
<mesh name="index1" file="index1_collision.stl"/>
<mesh name="index2" file="index2_collision.stl"/>
<mesh name="index3" file="index3_collision.stl"/>
<mesh name="middle0" file="middle0_collision.stl"/>
<mesh name="middle1" file="middle1_collision.stl"/>
<mesh name="middle2" file="middle2_collision.stl"/>
<mesh name="middle3" file="middle3_collision.stl"/>
<mesh name="palm" file="palm_collision.stl"/>
<mesh name="pinky0" file="pinky0_collision.stl"/>
<mesh name="pinky1" file="pinky1_collision.stl"/>
<mesh name="pinky2" file="pinky2_collision.stl"/>
<mesh name="pinky3" file="pinky3_collision.stl"/>
<mesh name="ring0" file="ring0_collision.stl"/>
<mesh name="ring1" file="ring1_collision.stl"/>
<mesh name="ring2" file="ring2_collision.stl"/>
<mesh name="ring3" file="ring3_collision.stl"/>
<mesh name="thumb0" file="thumb0_collision.stl"/>
<mesh name="thumb1" file="thumb1_collision.stl"/>
<mesh name="thumb2" file="thumb2_collision.stl"/>
<mesh name="thumb3" file="thumb3_collision.stl"/>
<mesh name="wristx" file="wristx_collision.stl"/>
<mesh name="wristy" file="wristy_collision.stl"/>
<mesh name="wristz" file="wristz_collision.stl"/>
<texture type="skybox" builtin="gradient" rgb1=".4 .6 .8" rgb2="0 0 0"
width="100" height="100"/>

View File

@@ -98,8 +98,11 @@ static GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName)
}
fclose(file);
}
shape->m_numIndices = shape->m_indices->size();
shape->m_numvertices = shape->m_vertices->size();
if (shape)
{
shape->m_numIndices = shape->m_indices->size();
shape->m_numvertices = shape->m_vertices->size();
}
return shape;
}

View File

@@ -0,0 +1,49 @@
#script to track a robot with a VR controller attached to it.
import time
import pybullet as p
#first try to connect to shared memory (VR), if it fails use local GUI
c = p.connect(p.SHARED_MEMORY)
if (c<0):
c = p.connect(p.GUI)
p.resetSimulation()
p.setGravity(0,0,0)
print(c)
if (c<0):
p.connect(p.GUI)
#load the MuJoCo MJCF hand
minitaur = p.loadURDF("quadruped/minitaur.urdf")
robot_cid = -1
POSITION=1
ORIENTATION=2
BUTTONS=6
p.setRealTimeSimulation(1)
controllerId = -1
while True:
events = p.getVREvents()
for e in (events):
#print (e[BUTTONS])
if (e[BUTTONS][33]&p.VR_BUTTON_WAS_TRIGGERED ):
if (controllerId >= 0):
controllerId = -1
else:
controllerId = e[0]
if (e[0] == controllerId):
if (robot_cid>=0):
p.changeConstraint(robot_cid,e[POSITION],e[ORIENTATION], maxForce=5000)
if (e[BUTTONS][32]&p.VR_BUTTON_WAS_TRIGGERED ):
if (robot_cid>=0):
p.removeConstraint(robot_cid)
#q = [0,0,0,1]
euler = p.getEulerFromQuaternion(e[ORIENTATION])
q = p.getQuaternionFromEuler([euler[0],euler[1],0]) #-euler[0],-euler[1],-euler[2]])
robot_cid = p.createConstraint(minitaur,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0.1,0,0],e[POSITION],q,e[ORIENTATION])

View File

@@ -49,7 +49,7 @@ inline int btGetVersion()
#define ATTRIBUTE_ALIGNED16(a) a
#define ATTRIBUTE_ALIGNED64(a) a
#define ATTRIBUTE_ALIGNED128(a) a
#elif (_M_ARM)
#elif defined(_M_ARM)
#define SIMD_FORCE_INLINE __forceinline
#define ATTRIBUTE_ALIGNED16(a) __declspec() a
#define ATTRIBUTE_ALIGNED64(a) __declspec() a