Skip to content

Commit

Permalink
Add C++ version VRGloveSimulatorMain example, using the serial library.
Browse files Browse the repository at this point in the history 
First run the App_PhysicsServer_SharedMemory_VR_vs2010.exe to run the VR server,
then run App_VRGloveHandSimulator. You likely need to tune the minV/maxV for each finger (check values using Arduino IDE Serial Monitor)
  • Loading branch information
@erwincoumans
erwincoumans committed May 2, 2017
1 parent 305725e commit 9fef6c1
Show file tree
Hide file tree
Showing 13 changed files with 610 additions and 153 deletions.
1 change: 0 additions & 1 deletion examples/ExampleBrowser/OpenGLExampleBrowser.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,6 @@
//quick test for file import, @todo(erwincoumans) make it more general and add other file formats
#include "../Importers/ImportURDFDemo/ImportURDFSetup.h"
#include "../Importers/ImportBullet/SerializeSetup.h"
#include "../Utils/b3HashString.h"
#include "Bullet3Common/b3HashMap.h"

struct GL3TexLoader : public MyTextureLoader
Expand Down
1 change: 0 additions & 1 deletion examples/RobotSimulator/MinitaurSetup.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,6 @@
#include "b3RobotSimulatorClientAPI.h"

#include "Bullet3Common/b3HashMap.h"
#include "../Utils/b3HashString.h"

struct MinitaurSetupInternalData
{
Expand Down
23 changes: 9 additions & 14 deletions examples/RobotSimulator/RobotSimulatorMain.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@ int main(int argc, char* argv[])
//sim->connect(eCONNECT_UDP, "localhost", 1234);
sim->configureDebugVisualizer( COV_ENABLE_GUI, 0);
// sim->configureDebugVisualizer( COV_ENABLE_SHADOWS, 0);//COV_ENABLE_WIREFRAME
sim->setTimeOut(12345);
sim->setTimeOut(10);
//syncBodies is only needed when connecting to an existing physics server that has already some bodies
sim->syncBodies();
b3Scalar fixedTimeStep = 1./240.;
Expand All @@ -33,18 +33,14 @@ int main(int argc, char* argv[])
//b3BodyInfo bodyInfo;
//sim->getBodyInfo(blockId,&bodyInfo);

sim->loadURDF("plane_with_collision_audio.urdf");
sim->loadURDF("plane.urdf");

b3RobotSimulatorLoadUrdfFileArgs args;
args.m_startPosition.setValue(0,0,2);
for (int i=0;i<10;i++)
{
args.m_startPosition[0] = 0.5*i;
args.m_startPosition[1] = 0.5*i;
args.m_startPosition[2] = 2+i*2;
int r2d2 = sim->loadURDF("cube.urdf",args);
}
MinitaurSetup minitaur;
int minitaurUid = minitaur.setupMinitaur(sim, b3MakeVector3(0,0,.3));


//b3RobotSimulatorLoadUrdfFileArgs args;
//args.m_startPosition.setValue(2,0,1);
//int r2d2 = sim->loadURDF("r2d2.urdf",args);

//b3RobotSimulatorLoadFileResults sdfResults;
Expand Down Expand Up @@ -127,8 +123,8 @@ int main(int argc, char* argv[])
yaw+=0.1;
b3Vector3 basePos;
b3Quaternion baseOrn;
// sim->getBasePositionAndOrientation(minitaurUid,basePos,baseOrn);
// sim->resetDebugVisualizerCamera(distance,yaw,20,basePos);
sim->getBasePositionAndOrientation(minitaurUid,basePos,baseOrn);
sim->resetDebugVisualizerCamera(distance,yaw,20,basePos);
}
b3Clock::usleep(1000.*1000.*fixedTimeStep);
}
Expand All @@ -144,4 +140,3 @@ int main(int argc, char* argv[])

}


295 changes: 295 additions & 0 deletions examples/RobotSimulator/VRGloveSimulatorMain.cpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,295 @@
//VR Glove hand simulator is a C++ conversion from the Python pybullet vrhand_vive_tracker.py
//For more details about the VR glove, see also https://docs.google.com/document/d/1_qwXJRBTGKmhktdBtVQ6zdOdxwud1K30jt0G5dkAr10/edit

#include "b3RobotSimulatorClientAPI.h"
#include "../Utils/b3Clock.h"
#include "Bullet3Common/b3CommandLineArgs.h"
#include <string.h>
#include <stdio.h>
#include <assert.h>
#include "serial/serial.h"
#include "../Importers/ImportURDFDemo/urdfStringSplit.h"

double convertSensor(int inputV, int minV, int maxV)
{
b3Clamp(inputV,minV,maxV);
double outVal =(double)inputV;
double b = (outVal-(double)minV)/float(maxV-minV);
return (b);
}

void setJointMotorPositionControl(b3RobotSimulatorClientAPI* sim, int obUid, int linkIndex, double targetPosition)
{
b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_POSITION_VELOCITY_PD);
controlArgs.m_maxTorqueValue = 50.;
controlArgs.m_targetPosition = targetPosition;
controlArgs.m_targetVelocity = 0;
sim->setJointMotorControl(obUid,linkIndex,controlArgs);
}

int main(int argc, char* argv[])
{
b3CommandLineArgs args(argc,argv);
std::string port="COM10";
args.GetCmdLineArgument("port",port);
int baud = 115200;
args.GetCmdLineArgument("speed",baud);
std::string mode = "SHARED_MEMORY";
args.GetCmdLineArgument("mode",mode);
int disableGui = 0;
args.GetCmdLineArgument("disableGui",disableGui);
int disableShadows = 0;
args.GetCmdLineArgument("disableShadows",disableShadows);

printf("port=%s, speed=%d, connection mode=%s\n", port.c_str(), baud,mode.c_str());

// port, baudrate, timeout in milliseconds
serial::Serial my_serial(port, baud, serial::Timeout::simpleTimeout(1));
if (!my_serial.isOpen())
{
printf("Cannot open serial port\n");
return -1;
}

b3RobotSimulatorClientAPI* sim = new b3RobotSimulatorClientAPI();

//Can also use eCONNECT_UDP,eCONNECT_TCP, for example: sim->connect(eCONNECT_UDP, "localhost", 1234);
if (mode=="GUI")
{
sim->connect(eCONNECT_GUI);
} else
{
if (mode=="DIRECT")
{
sim->connect(eCONNECT_DIRECT);
} else
{
sim->connect(eCONNECT_SHARED_MEMORY);
}
}

sim->setRealTimeSimulation(true);
sim->setInternalSimFlags(0);
sim->resetSimulation();

if (disableGui)
{
sim->configureDebugVisualizer( COV_ENABLE_GUI, 0);
}

if (disableShadows)
{
sim->configureDebugVisualizer( COV_ENABLE_SHADOWS, 0);
}

sim->setTimeOut(12345);
//syncBodies is only needed when connecting to an existing physics server that has already some bodies
sim->syncBodies();
b3Scalar fixedTimeStep = 1./240.;

sim->setTimeStep(fixedTimeStep);

b3Quaternion q = sim->getQuaternionFromEuler(b3MakeVector3(0.1,0.2,0.3));
b3Vector3 rpy;
rpy = sim->getEulerFromQuaternion(q);

sim->setGravity(b3MakeVector3(0,0,-9.8));
sim->setContactBreakingThreshold(0.001);

sim->loadURDF("plane_with_collision_audio.urdf");

sim->loadURDF("jenga/jenga.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(1.300000,-0.700000,0.750000),b3Quaternion(0.000000,0.707107,0.000000,0.707107)));
sim->loadURDF("jenga/jenga.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(1.200000,-0.700000,0.750000),b3Quaternion(0.000000,0.707107,0.000000,0.707107)));
sim->loadURDF("jenga/jenga.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(1.100000,-0.700000,0.750000),b3Quaternion(0.000000,0.707107,0.000000,0.707107)));
sim->loadURDF("jenga/jenga.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(1.000000,-0.700000,0.750000),b3Quaternion(0.000000,0.707107,0.000000,0.707107)));
sim->loadURDF("jenga/jenga.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.900000,-0.700000,0.750000),b3Quaternion(0.000000,0.707107,0.000000,0.707107)));
sim->loadURDF("jenga/jenga.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.800000,-0.700000,0.750000),b3Quaternion(0.000000,0.707107,0.000000,0.707107)));
sim->loadURDF("jenga/jenga.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.700000,-0.700000,0.750000),b3Quaternion(0.000000,0.707107,0.000000,0.707107)));
sim->loadURDF("jenga/jenga.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.600000,-0.700000,0.750000),b3Quaternion(0.000000,0.707107,0.000000,0.707107)));
sim->loadURDF("table/table.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(1.000000,-0.200000,0.000000),b3Quaternion(0.000000,0.000000,0.707107,0.707107)));
sim->loadURDF("cube_small.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3( 0.950000,-0.100000,0.700000),b3Quaternion(0.000000,0.000000,0.707107,0.707107)));
sim->loadURDF("sphere_small.urdf",b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.850000,-0.400000,0.700000),b3Quaternion(0.000000,0.000000,0.707107,0.707107)));

int handUid = -1;

b3RobotSimulatorLoadFileResults mjcfResults;
const char* mjcfFileName = "MPL/mpl2.xml";
if (sim->loadMJCF(mjcfFileName,mjcfResults))
{
printf("mjcfResults = %d\n", mjcfResults.m_uniqueObjectIds.size());
if (mjcfResults.m_uniqueObjectIds.size()==1)
{
handUid = mjcfResults.m_uniqueObjectIds[0];
}
}
if (handUid<0)
{
printf("Cannot load MJCF file %s\n", mjcfFileName);
}

b3Quaternion handOrn = sim->getQuaternionFromEuler(b3MakeVector3(3.14,-3.14/2,0));
b3Vector3 handPos = b3MakeVector3(-0.05,0,0.02);

b3JointInfo jointInfo;
jointInfo.m_jointType = eFixedType;
b3Vector3 handStartPosWorld = b3MakeVector3(0.500000,0.300006,0.900000);
b3Quaternion handStartOrnWorld = b3Quaternion ::getIdentity();

jointInfo.m_childFrame[0] = handStartPosWorld[0];
jointInfo.m_childFrame[1] = handStartPosWorld[1];
jointInfo.m_childFrame[2] = handStartPosWorld[2];
jointInfo.m_childFrame[3] = handStartOrnWorld[0];
jointInfo.m_childFrame[4] = handStartOrnWorld[1];
jointInfo.m_childFrame[5] = handStartOrnWorld[2];
jointInfo.m_childFrame[6] = handStartOrnWorld[3];

jointInfo.m_parentFrame[0] = handPos[0];
jointInfo.m_parentFrame[1] = handPos[1];
jointInfo.m_parentFrame[2] = handPos[2];
jointInfo.m_parentFrame[3] = handOrn[0];
jointInfo.m_parentFrame[4] = handOrn[1];
jointInfo.m_parentFrame[5] = handOrn[2];
jointInfo.m_parentFrame[6] = handOrn[3];

int handConstraintId = sim->createConstraint(handUid,-1,-1,-1,&jointInfo);
double maxFingerForce = 50;

for (int j=0; j<sim->getNumJoints(handUid);j++)
{
b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_POSITION_VELOCITY_PD);
controlArgs.m_maxTorqueValue = maxFingerForce ;
controlArgs.m_targetPosition = 0;
controlArgs.m_targetVelocity = 0;
sim->setJointMotorControl(handUid,j,controlArgs);
}

b3Clock clock;
double startTime = clock.getTimeInSeconds();
double simWallClockSeconds = 20.;

int vidLogId = -1;
int minitaurLogId = -1;
int rotateCamera = 0;

while (sim->canSubmitCommand())
{
b3VREventsData vrEvents;
int deviceTypeFilter = VR_DEVICE_GENERIC_TRACKER;

sim->getVREvents(&vrEvents, deviceTypeFilter);
//instead of iterating over all vr events, we just take the most up-to-date one
if (vrEvents.m_numControllerEvents)
{
int i = vrEvents.m_numControllerEvents - 1;
b3VRControllerEvent& e = vrEvents.m_controllerEvents[i];
// printf("e.pos=%f,%f,%f\n",e.m_pos[0],e.m_pos[1],e.m_pos[2]);
b3JointInfo changeConstraintInfo;
changeConstraintInfo.m_flags = 0;
changeConstraintInfo.m_jointMaxForce = 50;
changeConstraintInfo.m_flags |= eJointChangeMaxForce;

changeConstraintInfo.m_childFrame[0] = e.m_pos[0];
changeConstraintInfo.m_childFrame[1] = e.m_pos[1];
changeConstraintInfo.m_childFrame[2] = e.m_pos[2];
changeConstraintInfo.m_flags |= eJointChangeChildFramePosition;

changeConstraintInfo.m_childFrame[3] = e.m_orn[0];
changeConstraintInfo.m_childFrame[4] = e.m_orn[1];
changeConstraintInfo.m_childFrame[5] = e.m_orn[2];
changeConstraintInfo.m_childFrame[6] = e.m_orn[3];
changeConstraintInfo.m_flags |= eJointChangeChildFrameOrientation;

sim->changeConstraint(handConstraintId, &changeConstraintInfo);
}

//read the serial output from the hand, extract into parts
std::string result = my_serial.readline();
//my_serial.flush();
if (result.length())
{
int res = result.find("\n");
while (res<0)
{
result += my_serial.readline();
res = result.find("\n");
}
btAlignedObjectArray<std::string> pieces;
btAlignedObjectArray<std::string> separators;
separators.push_back(",");
urdfStringSplit( pieces, result, separators);

//printf("serial: %s\n", result.c_str());
if (pieces.size()==6)
{
double pinkTarget = 0;
double middleTarget = 0;
double indexTarget = 0;
double thumbTarget = 0;
{
int pink = atoi(pieces[1].c_str());
int middle = atoi(pieces[2].c_str());
int index = atoi(pieces[3].c_str());
int thumb = atoi(pieces[4].c_str());

pinkTarget = convertSensor(pink,250,400);
middleTarget = convertSensor(middle,250,400);
indexTarget = convertSensor(index,250,400);
thumbTarget = convertSensor(thumb,250,400);
}

//printf("pink = %d, middle=%d, index=%d, thumb=%d\n", pink,middle,index,thumb);

setJointMotorPositionControl(sim,handUid,5,1.3);
setJointMotorPositionControl(sim,handUid,7,thumbTarget);
setJointMotorPositionControl(sim,handUid,9,thumbTarget);
setJointMotorPositionControl(sim,handUid,11,thumbTarget);

setJointMotorPositionControl(sim,handUid,15,indexTarget);
setJointMotorPositionControl(sim,handUid,17,indexTarget);
setJointMotorPositionControl(sim,handUid,19,indexTarget);

setJointMotorPositionControl(sim,handUid,22,middleTarget);
setJointMotorPositionControl(sim,handUid,24,middleTarget);
setJointMotorPositionControl(sim,handUid,27,middleTarget);

double ringTarget = 0.5f*(pinkTarget+middleTarget);
setJointMotorPositionControl(sim,handUid,30,ringTarget);
setJointMotorPositionControl(sim,handUid,32,ringTarget);
setJointMotorPositionControl(sim,handUid,34,ringTarget);

setJointMotorPositionControl(sim,handUid,38,pinkTarget);
setJointMotorPositionControl(sim,handUid,40,pinkTarget);
setJointMotorPositionControl(sim,handUid,42,pinkTarget);
}
}

b3KeyboardEventsData keyEvents;
sim->getKeyboardEvents(&keyEvents);

//sim->stepSimulation();

if (rotateCamera)
{
static double yaw=0;
double distance = 1;
yaw+=0.1;
b3Vector3 basePos;
b3Quaternion baseOrn;
// sim->getBasePositionAndOrientation(minitaurUid,basePos,baseOrn);
// sim->resetDebugVisualizerCamera(distance,yaw,20,basePos);
}
//b3Clock::usleep(1000.*1000.*fixedTimeStep);
}

printf("serial.close()\n");
my_serial.close();

printf("sim->disconnect\n");
sim->disconnect();

printf("delete sim\n");
delete sim;

printf("exit\n");

}
Loading

0 comments on commit 9fef6c1

Please sign in to comment.