This commit is contained in:
Erwin Coumans 2016-12-05 11:58:06 -08:00
commit 27d38a1ba8
10 changed files with 157 additions and 35 deletions

View File

@ -303,6 +303,10 @@
</joint>
<link name='finger_right'>
<contact>
<lateral_friction value="1.0"/>
<spinning_friction value="1.5"/>
</contact>
<pose frame=''>0.042 0 0.145 0 0 1.5708</pose>
<inertial>
<mass>0.2</mass>
@ -343,6 +347,10 @@
</joint>
<link name='finger_left'>
<contact>
<lateral_friction value="1.0"/>
<spinning_friction value="1.5"/>
</contact>
<pose frame=''>-0.042 0 0.145 0 0 4.71239</pose>
<inertial>
<mass>0.2</mass>

View File

@ -300,6 +300,10 @@
</joint>
<link name='finger_right'>
<contact>
<lateral_friction>1.0</lateral_friction>
<spinning_friction>1.5</spinning_friction>
</contact>
<pose frame=''>0.062 0 0.145 0 0 1.5708</pose>
<inertial>
<mass>0.2</mass>
@ -340,6 +344,10 @@
</joint>
<link name='finger_left'>
<contact>
<lateral_friction>1.0</lateral_friction>
<spinning_friction>1.5</spinning_friction>
</contact>
<pose frame=''>-0.062 0 0.145 0 0 4.71239</pose>
<inertial>
<mass>0.2</mass>

View File

@ -190,7 +190,7 @@ TinyRendererSetup::TinyRendererSetup(struct GUIHelperInterface* gui)
TinyRenderObjectData* ob = new TinyRenderObjectData(
m_internalData->m_rgbColorBuffer,
m_internalData->m_depthBuffer,
m_internalData->m_shadowBuffer,
&m_internalData->m_shadowBuffer,
&m_internalData->m_segmentationMaskBuffer,
m_internalData->m_renderObjects.size());

View File

@ -48,7 +48,7 @@ int gHuskyId = -1;
btTransform huskyTr = btTransform::getIdentity();
int gCreateObjectSimVR = -1;
int gEnableKukaControl = 1;
int gEnableKukaControl = 0;
btScalar simTimeScalingFactor = 1;
btScalar gRhsClamp = 1.f;
@ -667,8 +667,6 @@ PhysicsServerCommandProcessor::PhysicsServerCommandProcessor()
m_data = new PhysicsServerCommandProcessorInternalData();
createEmptyDynamicsWorld();
m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.00001;
m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
}
@ -717,6 +715,9 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.08;
m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.00001;
m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 50;
}
void PhysicsServerCommandProcessor::deleteCachedInverseKinematicsBodies()
@ -4015,7 +4016,7 @@ void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName)
}
btVector3 gVRGripperPos(0,0,0.2);
btVector3 gVRGripperPos(0.6, 0.4, 0.7);
btQuaternion gVRGripperOrn(0,0,0,1);
btVector3 gVRController2Pos(0,0,0.2);
btQuaternion gVRController2Orn(0,0,0,1);
@ -4155,7 +4156,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
{
int bodyId = 0;
if (loadUrdf("pr2_gripper.urdf", btVector3(0, 0, 0.1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()))
if (loadUrdf("pr2_gripper.urdf", btVector3(-0.2, 0.15, 0.9), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()))
{
InteralBodyData* parentBody = m_data->getHandle(bodyId);
if (parentBody->m_multiBody)
@ -4185,6 +4186,30 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
loadUrdf("kuka_iiwa/model_vr_limits.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;
InteralBodyData* kukaBody = m_data->getHandle(m_data->m_KukaId);
if (kukaBody->m_multiBody && kukaBody->m_multiBody->getNumDofs() == 7)
{
btScalar q[7];
q[0] = 0;// -SIMD_HALF_PI;
q[1] = 0;
q[2] = 0;
q[3] = SIMD_HALF_PI;
q[4] = 0;
q[5] = -SIMD_HALF_PI*0.66;
q[6] = 0;
for (int i = 0; i < 7; i++)
{
kukaBody->m_multiBody->setJointPos(i, q[i]);
}
btAlignedObjectArray<btQuaternion> scratch_q;
btAlignedObjectArray<btVector3> scratch_m;
kukaBody->m_multiBody->forwardKinematics(scratch_q, scratch_m);
int nLinks = kukaBody->m_multiBody->getNumLinks();
scratch_q.resize(nLinks + 1);
scratch_m.resize(nLinks + 1);
kukaBody->m_multiBody->updateCollisionObjectWorldTransforms(scratch_q, scratch_m);
}
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());
@ -4193,7 +4218,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
// Load one motor gripper for kuka
loadSdf("gripper/wsg50_one_motor_gripper_new_free_base.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true);
m_data->m_gripperId = bodyId + 1;
InteralBodyData* kukaBody = m_data->getHandle(m_data->m_KukaId);
InteralBodyData* gripperBody = m_data->getHandle(m_data->m_gripperId);
// Reset the default gripper motor maximum torque for damping to 0
@ -4235,6 +4260,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperRevolute1);
m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperRevolute2);
kukaBody = m_data->getHandle(m_data->m_KukaId);
if (kukaBody->m_multiBody && kukaBody->m_multiBody->getNumDofs()==7)
{
gripperBody->m_multiBody->setHasSelfCollision(0);
@ -4288,7 +4314,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
// Table area
loadUrdf("table/table.urdf", objectWorldTr[0].getOrigin(), objectWorldTr[0].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("tray/tray_textured.urdf", objectWorldTr[1].getOrigin(), objectWorldTr[1].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("tray/tray_textured.urdf", objectWorldTr[1].getOrigin(), objectWorldTr[1].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("cup_small.urdf", objectWorldTr[2].getOrigin(), objectWorldTr[2].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("pitcher_small.urdf", objectWorldTr[3].getOrigin(), objectWorldTr[3].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("teddy_vhacd.urdf", objectWorldTr[4].getOrigin(), objectWorldTr[4].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
@ -4329,9 +4355,10 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
if (motor)
{
btScalar posTarget = (-0.048)*btMin(btScalar(0.75), gVRGripper2Analog) / 0.75;
motor->setPositionTarget(posTarget, .2);
motor->setPositionTarget(posTarget, .8);
motor->setVelocityTarget(0.0, .5);
motor->setMaxAppliedImpulse(5.0);
motor->setMaxAppliedImpulse(1.0);
}
}

View File

@ -27,6 +27,7 @@
//@todo(erwincoumans) those globals are hacks for a VR demo, move this to Python/pybullet!
extern btVector3 gLastPickPos;
btVector3 gVRTeleportPos1(0,0,0);
btScalar gVRTeleportRotZ = 0;
btQuaternion gVRTeleportOrn(0, 0, 0,1);
extern btVector3 gVRGripperPos;
extern btQuaternion gVRGripperOrn;
@ -46,10 +47,47 @@ extern btScalar simTimeScalingFactor;
extern bool gVRGripperClosed;
#if B3_USE_MIDI
const char* startFileNameVR = "0_VRDemoSettings.txt";
#include <vector>
//remember the settings (you don't want to re-tune again and again...)
static void saveCurrentSettingsVR()
{
FILE* f = fopen(startFileNameVR, "w");
if (f)
{
fprintf(f, "--camPosX= %f\n", gVRTeleportPos1[0]);
fprintf(f, "--camPosY= %f\n", gVRTeleportPos1[1]);
fprintf(f, "--camPosZ= %f\n", gVRTeleportPos1[2]);
fprintf(f, "--camRotZ= %f\n", gVRTeleportRotZ);
fclose(f);
}
};
static void loadCurrentSettingsVR(b3CommandLineArgs& args)
{
int currentEntry = 0;
FILE* f = fopen(startFileNameVR, "r");
if (f)
{
char oneline[1024];
char* argv[] = { 0,&oneline[0] };
while (fgets(oneline, 1024, f) != NULL)
{
char *pos;
if ((pos = strchr(oneline, '\n')) != NULL)
*pos = '\0';
args.addArgs(2, argv);
}
fclose(f);
}
};
#if B3_USE_MIDI
static float getParamf(float rangeMin, float rangeMax, int midiVal)
{
float v = rangeMin + (rangeMax - rangeMin)* (float(midiVal / 127.));
@ -70,9 +108,10 @@ void midiCallback(double deltatime, std::vector< unsigned char > *message, void
{
if (message->at(1) == 16)
{
float rotZ = getParamf(-3.1415, 3.1415, message->at(2));
gVRTeleportOrn = btQuaternion(btVector3(0, 0, 1), rotZ);
b3Printf("gVRTeleportOrn rotZ = %f\n", rotZ);
gVRTeleportRotZ= getParamf(-3.1415, 3.1415, message->at(2));
gVRTeleportOrn = btQuaternion(btVector3(0, 0, 1), gVRTeleportRotZ);
saveCurrentSettingsVR();
b3Printf("gVRTeleportOrn rotZ = %f\n", gVRTeleportRotZ);
}
if (message->at(1) == 32)
@ -85,6 +124,7 @@ void midiCallback(double deltatime, std::vector< unsigned char > *message, void
if (message->at(1) == i)
{
gVRTeleportPos1[i] = getParamf(-2, 2, message->at(2));
saveCurrentSettingsVR();
b3Printf("gVRTeleportPos[%d] = %f\n", i,gVRTeleportPos1[i]);
}
@ -927,7 +967,7 @@ public:
virtual void processCommandLineArgs(int argc, char* argv[])
{
b3CommandLineArgs args(argc,argv);
loadCurrentSettingsVR(args);
if (args.GetCmdLineArgument("camPosX", gVRTeleportPos1[0]))
{
printf("camPosX=%f\n", gVRTeleportPos1[0]);
@ -1665,8 +1705,10 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
return;
if (gGraspingController < 0)
{
gGraspingController = controllerId;
gEnableKukaControl = true;
}
if (controllerId != gGraspingController)
{
if (button == 1 && state == 0)

View File

@ -568,7 +568,7 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const
if (vertices.size() && indices.size())
{
TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer,m_data->m_depthBuffer, m_data->m_shadowBuffer, &m_data->m_segmentationMaskBuffer, bodyUniqueId);
TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer,m_data->m_depthBuffer, &m_data->m_shadowBuffer, &m_data->m_segmentationMaskBuffer, bodyUniqueId);
unsigned char* textureImage=0;
int textureWidth=0;
int textureHeight=0;

View File

@ -855,7 +855,7 @@ void CMainApplication::RenderFrame()
// SwapWindow
{
B3_PROFILE("m_app->swapBuffer");
// m_app->swapBuffer();
m_app->swapBuffer();
//SDL_GL_SwapWindow( m_pWindow );
}

View File

@ -74,7 +74,7 @@ struct Shader : public IShader {
Vec4f m_colorRGBA;
Matrix& m_viewportMat;
b3AlignedObjectArray<float>& m_shadowBuffer;
b3AlignedObjectArray<float>* m_shadowBuffer;
int m_width;
int m_height;
@ -86,7 +86,7 @@ struct Shader : public IShader {
mat<4,3,float> varying_tri_light_view;
mat<3,3,float> varying_nrm; // normal per vertex to be interpolated by FS
Shader(Model* model, Vec3f light_dir_local, Vec3f light_color, Matrix& modelView, Matrix& lightModelView, Matrix& projectionMat, Matrix& modelMat, Matrix& viewportMat, Vec3f localScaling, const Vec4f& colorRGBA, int width, int height, b3AlignedObjectArray<float>& shadowBuffer)
Shader(Model* model, Vec3f light_dir_local, Vec3f light_color, Matrix& modelView, Matrix& lightModelView, Matrix& projectionMat, Matrix& modelMat, Matrix& viewportMat, Vec3f localScaling, const Vec4f& colorRGBA, int width, int height, b3AlignedObjectArray<float>* shadowBuffer)
:m_model(model),
m_light_dir_local(light_dir_local),
m_light_color(light_color),
@ -126,7 +126,7 @@ struct Shader : public IShader {
int index_x = b3Max(0, b3Min(m_width-1, int(p[0])));
int index_y = b3Max(0, b3Min(m_height-1, int(p[1])));
int idx = index_x + index_y*m_width; // index in the shadowbuffer array
float shadow = 0.8+0.2*(m_shadowBuffer[idx]<-depth+0.2); // magic coeff to avoid z-fighting
float shadow = 0.8+0.2*(m_shadowBuffer->at(idx)<-depth+0.2); // magic coeff to avoid z-fighting
Vec3f bn = (varying_nrm*bar).normalize();
Vec2f uv = varying_uv*bar;
@ -158,7 +158,7 @@ struct Shader : public IShader {
}
};
TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer,b3AlignedObjectArray<float>&shadowBuffer)
TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer,b3AlignedObjectArray<float>* shadowBuffer)
:m_rgbColorBuffer(rgbColorBuffer),
m_depthBuffer(depthBuffer),
m_shadowBuffer(shadowBuffer),
@ -167,6 +167,45 @@ m_model(0),
m_userData(0),
m_userIndex(-1),
m_objectIndex(-1)
{
Vec3f eye(1,1,3);
Vec3f center(0,0,0);
Vec3f up(0,0,1);
m_lightDirWorld.setValue(0,0,0);
m_lightColor.setValue(1, 1, 1);
m_localScaling.setValue(1,1,1);
m_modelMatrix = Matrix::identity();
}
TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer, b3AlignedObjectArray<float>* shadowBuffer, b3AlignedObjectArray<int>* segmentationMaskBuffer, int objectIndex)
:m_rgbColorBuffer(rgbColorBuffer),
m_depthBuffer(depthBuffer),
m_shadowBuffer(shadowBuffer),
m_segmentationMaskBufferPtr(segmentationMaskBuffer),
m_model(0),
m_userData(0),
m_userIndex(-1),
m_objectIndex(objectIndex)
{
Vec3f eye(1,1,3);
Vec3f center(0,0,0);
Vec3f up(0,0,1);
m_lightDirWorld.setValue(0,0,0);
m_lightColor.setValue(1, 1, 1);
m_localScaling.setValue(1,1,1);
m_modelMatrix = Matrix::identity();
}
TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer)
:m_rgbColorBuffer(rgbColorBuffer),
m_depthBuffer(depthBuffer),
m_segmentationMaskBufferPtr(0),
m_model(0),
m_userData(0),
m_userIndex(-1),
m_objectIndex(-1)
{
Vec3f eye(1,1,3);
Vec3f center(0,0,0);
@ -178,12 +217,9 @@ m_objectIndex(-1)
}
TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer, b3AlignedObjectArray<float>&shadowBuffer, b3AlignedObjectArray<int>* segmentationMaskBuffer, int objectIndex)
TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer, b3AlignedObjectArray<int>* segmentationMaskBuffer, int objectIndex)
:m_rgbColorBuffer(rgbColorBuffer),
m_depthBuffer(depthBuffer),
m_shadowBuffer(shadowBuffer),
m_segmentationMaskBufferPtr(segmentationMaskBuffer),
m_model(0),
m_userData(0),
@ -341,7 +377,7 @@ void TinyRenderer::renderObjectDepth(TinyRenderObjectData& renderData)
renderData.m_viewportMatrix = viewport(0,0,width, height);
b3AlignedObjectArray<float>& shadowbuffer = renderData.m_shadowBuffer;
float* shadowBufferPtr = (renderData.m_shadowBuffer && renderData.m_shadowBuffer->size())?&renderData.m_shadowBuffer->at(0):0;
int* segmentationMaskBufferPtr = 0;
TGAImage depthFrame(width, height, TGAImage::RGB);
@ -360,7 +396,7 @@ void TinyRenderer::renderObjectDepth(TinyRenderObjectData& renderData)
for (int j=0; j<3; j++) {
shader.vertex(i, j);
}
triangle(shader.varying_tri, shader, depthFrame, &shadowbuffer[0], segmentationMaskBufferPtr, renderData.m_viewportMatrix, renderData.m_objectIndex);
triangle(shader.varying_tri, shader, depthFrame, shadowBufferPtr, segmentationMaskBufferPtr, renderData.m_viewportMatrix, renderData.m_objectIndex);
}
}
@ -381,7 +417,7 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData)
renderData.m_viewportMatrix = viewport(0,0,width, height);
b3AlignedObjectArray<float>& zbuffer = renderData.m_depthBuffer;
b3AlignedObjectArray<float>& shadowbuffer = renderData.m_shadowBuffer;
b3AlignedObjectArray<float>* shadowBufferPtr = renderData.m_shadowBuffer;
int* segmentationMaskBufferPtr = (renderData.m_segmentationMaskBufferPtr && renderData.m_segmentationMaskBufferPtr->size())?&renderData.m_segmentationMaskBufferPtr->at(0):0;
TGAImage& frame = renderData.m_rgbColorBuffer;
@ -393,7 +429,7 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData)
Matrix modelViewMatrix = renderData.m_viewMatrix*renderData.m_modelMatrix;
Vec3f localScaling(renderData.m_localScaling[0],renderData.m_localScaling[1],renderData.m_localScaling[2]);
Shader shader(model, light_dir_local, light_color, modelViewMatrix, lightModelViewMatrix, renderData.m_projectionMatrix,renderData.m_modelMatrix, renderData.m_viewportMatrix, localScaling, model->getColorRGBA(), width, height, shadowbuffer);
Shader shader(model, light_dir_local, light_color, modelViewMatrix, lightModelViewMatrix, renderData.m_projectionMatrix,renderData.m_modelMatrix, renderData.m_viewportMatrix, localScaling, model->getColorRGBA(), width, height, shadowBufferPtr);
for (int i=0; i<model->nfaces(); i++)
{

View File

@ -30,11 +30,13 @@ struct TinyRenderObjectData
TGAImage& m_rgbColorBuffer;
b3AlignedObjectArray<float>& m_depthBuffer;//required, hence a reference
b3AlignedObjectArray<float>& m_shadowBuffer;
b3AlignedObjectArray<float>* m_shadowBuffer;//optional, hence a pointer
b3AlignedObjectArray<int>* m_segmentationMaskBufferPtr;//optional, hence a pointer
TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer,b3AlignedObjectArray<float>&shadowBuffer);
TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer,b3AlignedObjectArray<float>&shadowBuffer, b3AlignedObjectArray<int>* segmentationMaskBuffer,int objectIndex);
TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer);
TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer,b3AlignedObjectArray<int>* segmentationMaskBuffer,int objectIndex);
TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer,b3AlignedObjectArray<float>* shadowBuffer);
TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer,b3AlignedObjectArray<float>* shadowBuffer, b3AlignedObjectArray<int>* segmentationMaskBuffer,int objectIndex);
virtual ~TinyRenderObjectData();
void loadModel(const char* fileName);

View File

@ -147,7 +147,6 @@ int main(int argc, char* argv[])
renderData.m_rgbColorBuffer.set(x,y,color);
renderData.m_depthBuffer[x+y*textureWidth] = -1e30f;
renderData.m_shadowBuffer[x+y*textureWidth] = -1e30f;
}
}