mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-14 22:00:05 +00:00
ab8f16961e
Apply clang-format-all.sh using the _clang-format file through all the cpp/.h files. make sure not to apply it to certain serialization structures, since some parser expects the * as part of the name, instead of type. This commit contains no other changes aside from adding and applying clang-format-all.sh
348 lines
12 KiB
C++
348 lines
12 KiB
C++
//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 = "COM9";
|
|
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);
|
|
int useKitchen = 0;
|
|
args.GetCmdLineArgument("useKitchen", useKitchen);
|
|
|
|
int deviceTypeFilter = VR_DEVICE_GENERIC_TRACKER;
|
|
args.GetCmdLineArgument("deviceTypeFilter", deviceTypeFilter);
|
|
|
|
printf("port=%s, speed=%d, connection mode=%s\n", port.c_str(), baud, mode.c_str());
|
|
|
|
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);
|
|
|
|
if (useKitchen)
|
|
{
|
|
b3RobotSimulatorLoadFileResults res;
|
|
sim->loadSDF("kitchens/1.sdf", res);
|
|
}
|
|
else
|
|
{
|
|
sim->loadURDF("plane_with_collision_audio.urdf");
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
#ifdef TOUCH
|
|
b3Vector3 handPos = b3MakeVector3(-0.10, -0.03, 0.02);
|
|
b3Vector3 rollPitchYaw = b3MakeVector3(0.5 * B3_PI, 0, 1.25 * B3_PI); //-B3_PI/2,0,B3_PI/2);
|
|
handOrn = sim->getQuaternionFromEuler(rollPitchYaw);
|
|
|
|
#else
|
|
b3Quaternion handOrn = sim->getQuaternionFromEuler(b3MakeVector3(3.14, -3.14 / 2, 0));
|
|
b3Vector3 handPos = b3MakeVector3(-0.05, 0, 0.02);
|
|
#endif
|
|
|
|
b3Vector3 handStartPosWorld = b3MakeVector3(0.500000, 0.300006, 0.900000);
|
|
b3Quaternion handStartOrnWorld = b3Quaternion ::getIdentity();
|
|
|
|
b3JointInfo jointInfo;
|
|
jointInfo.m_jointType = eFixedType;
|
|
|
|
printf("handStartOrnWorld=%f,%f,%f,%f\n", handStartOrnWorld[0], handStartOrnWorld[1], handStartOrnWorld[2], handStartOrnWorld[3]);
|
|
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];
|
|
|
|
sim->resetBasePositionAndOrientation(handUid, handStartPosWorld, handStartOrnWorld);
|
|
int handConstraintId = sim->createConstraint(handUid, -1, -1, -1, &jointInfo);
|
|
double maxFingerForce = 10;
|
|
double maxArmForce = 1000;
|
|
for (int j = 0; j < sim->getNumJoints(handUid); j++)
|
|
{
|
|
b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_POSITION_VELOCITY_PD);
|
|
controlArgs.m_maxTorqueValue = maxFingerForce;
|
|
controlArgs.m_kp = 0.1;
|
|
controlArgs.m_kd = 1;
|
|
controlArgs.m_targetPosition = 0;
|
|
controlArgs.m_targetVelocity = 0;
|
|
sim->setJointMotorControl(handUid, j, controlArgs);
|
|
}
|
|
|
|
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)));
|
|
|
|
b3Clock clock;
|
|
double startTime = clock.getTimeInSeconds();
|
|
double simWallClockSeconds = 20.;
|
|
|
|
int vidLogId = -1;
|
|
int minitaurLogId = -1;
|
|
int rotateCamera = 0;
|
|
serial::Serial my_serial;
|
|
|
|
try
|
|
{
|
|
// port, baudrate, timeout in milliseconds
|
|
my_serial.setBaudrate(baud);
|
|
my_serial.setPort(port);
|
|
my_serial.setBytesize(serial::sevenbits);
|
|
my_serial.setParity(serial::parity_odd);
|
|
my_serial.setStopbits(serial::stopbits_two);
|
|
my_serial.setTimeout(serial::Timeout::simpleTimeout(0.01));
|
|
my_serial.open();
|
|
}
|
|
catch (...)
|
|
{
|
|
printf("Cannot open port, use --port=PORTNAME\n");
|
|
exit(0);
|
|
}
|
|
|
|
if (!my_serial.isOpen())
|
|
{
|
|
printf("Cannot open serial port\n");
|
|
return -1;
|
|
}
|
|
|
|
my_serial.flush();
|
|
|
|
while (sim->canSubmitCommand())
|
|
{
|
|
clock.usleep(1);
|
|
|
|
b3VREventsData vrEvents;
|
|
|
|
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 = maxArmForce;
|
|
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;
|
|
try
|
|
{
|
|
result = my_serial.readline();
|
|
}
|
|
catch (...)
|
|
{
|
|
}
|
|
if (result.length())
|
|
{
|
|
my_serial.flush();
|
|
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");
|
|
}
|