virtual void initPhysics()
{
///create some graphics proxy for the tracking target
///the endeffector tries to track it using Inverse Kinematics
{
int sphereId = m_app->registerGraphicsUnitSphereShape(SPHERE_LOD_MEDIUM);
b3Quaternion orn(0, 0, 0, 1);
b3Vector4 color = b3MakeVector4(1., 0.3, 0.3, 1);
b3Vector3 scaling = b3MakeVector3(.02, .02, .02);
m_targetSphereInstance = m_app->m_renderer->registerGraphicsInstance(sphereId, m_targetPos, orn, color, scaling);
}
m_app->m_renderer->writeTransforms();
int mode = eCONNECT_EXISTING_EXAMPLE_BROWSER;
m_robotSim.setGuiHelper(m_guiHelper);
bool connected = m_robotSim.connect(mode);
// 0;//m_robotSim.connect(m_guiHelper);
b3Printf("robotSim connected = %d",connected);
{
m_kukaIndex = m_robotSim.loadURDF("kuka_iiwa/model.urdf");
if (m_kukaIndex >=0)
{
int numJoints = m_robotSim.getNumJoints(m_kukaIndex);
b3Printf("numJoints = %d",numJoints);
for (int i=0;i<numJoints;i++)
{
b3JointInfo jointInfo;
m_robotSim.getJointInfo(m_kukaIndex,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_kukaIndex,wheelJointIndices[i],controlArgs);
}
*/
}
{
m_robotSim.loadURDF("plane.urdf");
m_robotSim.setGravity(b3MakeVector3(0,0,0));
}
}
}
Tutorial(GUIHelperInterface* guiHelper, int tutorialIndex)
:m_app(guiHelper->getAppInterface()),
m_guiHelper(guiHelper),
m_tutorialIndex(tutorialIndex),
m_stage(0),
m_counter(0),
m_timeSeriesCanvas0(0),
m_timeSeriesCanvas1(0)
{
int numBodies = 1;
m_app->setUpAxis(1);
m_app->m_renderer->enableBlend(true);
switch (m_tutorialIndex)
{
case TUT_VELOCITY:
{
numBodies=10;
m_timeSeriesCanvas0 = new TimeSeriesCanvas(m_app->m_2dCanvasInterface,512,256,"Constant Velocity");
m_timeSeriesCanvas0 ->setupTimeSeries(2,60, 0);
m_timeSeriesCanvas0->addDataSource("X position (m)", 255,0,0);
m_timeSeriesCanvas0->addDataSource("X velocity (m/s)", 0,0,255);
m_timeSeriesCanvas0->addDataSource("dX/dt (m/s)", 0,0,0);
break;
}
case TUT_ACCELERATION:
{
numBodies=10;
m_timeSeriesCanvas1 = new TimeSeriesCanvas(m_app->m_2dCanvasInterface,256,512,"Constant Acceleration");
m_timeSeriesCanvas1 ->setupTimeSeries(50,60, 0);
m_timeSeriesCanvas1->addDataSource("Y position (m)", 255,0,0);
m_timeSeriesCanvas1->addDataSource("Y velocity (m/s)", 0,0,255);
m_timeSeriesCanvas1->addDataSource("dY/dt (m/s)", 0,0,0);
break;
}
case TUT_COLLISION:
{
numBodies=2;
m_timeSeriesCanvas1 = new TimeSeriesCanvas(m_app->m_2dCanvasInterface,512,200,"Distance");
m_timeSeriesCanvas1 ->setupTimeSeries(1.5,60, 0);
m_timeSeriesCanvas1->addDataSource("distance", 255,0,0);
break;
}
case TUT_SOLVE_CONTACT_CONSTRAINT:
{
numBodies=2;
m_timeSeriesCanvas1 = new TimeSeriesCanvas(m_app->m_2dCanvasInterface,512,200,"Collision Impulse");
m_timeSeriesCanvas1 ->setupTimeSeries(1.5,60, 0);
m_timeSeriesCanvas1->addDataSource("Distance", 0,0,255);
m_timeSeriesCanvas1->addDataSource("Impulse magnutide", 255,0,0);
{
SliderParams slider("Restitution",&gRestitution);
slider.m_minVal=0;
slider.m_maxVal=1;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
SliderParams slider("Mass A",&gMassA);
slider.m_minVal=0;
slider.m_maxVal=100;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
SliderParams slider("Mass B",&gMassB);
slider.m_minVal=0;
slider.m_maxVal=100;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
break;
}
default:
{
m_timeSeriesCanvas0 = new TimeSeriesCanvas(m_app->m_2dCanvasInterface,512,256,"Unknown");
m_timeSeriesCanvas0 ->setupTimeSeries(1,60, 0);
}
};
if (m_tutorialIndex==TUT_VELOCITY)
{
int boxId = m_app->registerCubeShape(100,1,100);
b3Vector3 pos = b3MakeVector3(0,-3.5,0);
b3Quaternion orn(0,0,0,1);
b3Vector4 color = b3MakeVector4(1,1,1,1);
b3Vector3 scaling = b3MakeVector3(1,1,1);
m_app->m_renderer->registerGraphicsInstance(boxId,pos,orn,color,scaling);
}
for (int i=0;i<numBodies;i++)
{
m_bodies.push_back(new LWRigidBody());
}
for (int i=0;i<m_bodies.size();i++)
{
m_bodies[i]->m_worldPose.m_position.setValue((i/4)*5,3,(i&3)*5);
}
{
int textureIndex = -1;
if (1)
{
int width,height,n;
const char* filename = "data/cube.png";
const unsigned char* image=0;
const char* prefix[]={"./","../","../../","../../../","../../../../"};
int numprefix = sizeof(prefix)/sizeof(const char*);
for (int i=0;!image && i<numprefix;i++)
{
char relativeFileName[1024];
sprintf(relativeFileName,"%s%s",prefix[i],filename);
image = stbi_load(relativeFileName, &width, &height, &n, 0);
}
b3Assert(image);
if (image)
{
textureIndex = m_app->m_renderer->registerTexture(image,width,height);
}
}
// int boxId = m_app->registerCubeShape(1,1,1,textureIndex);
int boxId = m_app->registerGraphicsUnitSphereShape(SPHERE_LOD_HIGH, textureIndex);
b3Vector4 color = b3MakeVector4(1,1,1,0.8);
b3Vector3 scaling = b3MakeVector3(SPHERE_RADIUS,SPHERE_RADIUS,SPHERE_RADIUS);
for (int i=0;i<m_bodies.size();i++)
{
m_bodies[i]->m_collisionShape.m_sphere.m_radius = SPHERE_RADIUS;
m_bodies[i]->m_collisionShape.m_type = LW_SPHERE_TYPE;
m_bodies[i]->m_graphicsIndex = m_app->m_renderer->registerGraphicsInstance(boxId,m_bodies[i]->m_worldPose.m_position, m_bodies[i]->m_worldPose.m_orientation,color,scaling);
m_app->m_renderer->writeSingleInstanceTransformToCPU(m_bodies[i]->m_worldPose.m_position, m_bodies[i]->m_worldPose.m_orientation, m_bodies[i]->m_graphicsIndex);
}
}
if (m_tutorialIndex == TUT_SOLVE_CONTACT_CONSTRAINT)
{
m_bodies[0]->m_invMass = gMassA? 1./gMassA : 0;
m_bodies[0]->m_collisionShape.m_sphere.computeLocalInertia(gMassA,m_bodies[0]->m_localInertia);
m_bodies[1]->m_invMass =gMassB? 1./gMassB : 0;
m_bodies[1]->m_collisionShape.m_sphere.computeLocalInertia(gMassB,m_bodies[1]->m_localInertia);
if (gMassA)
m_bodies[0]->m_linearVelocity.setValue(0,0,1);
if (gMassB)
m_bodies[1]->m_linearVelocity.setValue(0,0,-1);
}
m_app->m_renderer->writeTransforms();
}
