virtual void stepSimulation(float deltaTime) { if ((m_options & eGRIPPER_GRASP)!=0) { if ((m_gripperIndex>=0)) { int fingerJointIndices[3]={0,1,3}; double fingerTargetVelocities[3]={sGripperVerticalVelocity,sGripperClosingTargetVelocity ,-sGripperClosingTargetVelocity }; double maxTorqueValues[3]={40.0,50.0,50.0}; for (int i=0;i<3;i++) { b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); controlArgs.m_targetVelocity = fingerTargetVelocities[i]; controlArgs.m_maxTorqueValue = maxTorqueValues[i]; controlArgs.m_kd = 1.; m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs); } } } if ((m_options & eONE_MOTOR_GRASP)!=0) { int fingerJointIndices[2]={0,1}; double fingerTargetVelocities[2]={sGripperVerticalVelocity,sGripperClosingTargetVelocity }; double maxTorqueValues[2]={800.0,800.0}; for (int i=0;i<2;i++) { b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); controlArgs.m_targetVelocity = fingerTargetVelocities[i]; controlArgs.m_maxTorqueValue = maxTorqueValues[i]; controlArgs.m_kd = 1.; m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs); } } if ((m_options & eGRASP_SOFT_BODY)!=0) { int fingerJointIndices[2]={0,1}; double fingerTargetVelocities[2]={sGripperVerticalVelocity,sGripperClosingTargetVelocity }; double maxTorqueValues[2]={50.0,10.0}; for (int i=0;i<2;i++) { b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); controlArgs.m_targetVelocity = fingerTargetVelocities[i]; controlArgs.m_maxTorqueValue = maxTorqueValues[i]; controlArgs.m_kd = 1.; m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs); } } m_robotSim.stepSimulation(); }
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"); }
virtual void initPhysics() { int mode = eCONNECT_EXISTING_EXAMPLE_BROWSER; m_robotSim.setGuiHelper(m_guiHelper); bool connected = m_robotSim.connect(mode); b3Printf("robotSim connected = %d",connected); if ((m_options & eGRIPPER_GRASP)!=0) { { SliderParams slider("Vertical velocity",&sGripperVerticalVelocity); slider.m_minVal=-2; slider.m_maxVal=2; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { SliderParams slider("Closing velocity",&sGripperClosingTargetVelocity ); slider.m_minVal=-1; slider.m_maxVal=1; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { b3RobotSimulatorLoadUrdfFileArgs args; args.m_startPosition.setValue(0, 0, .107); args.m_startOrientation.setEulerZYX(0, 0, 0); args.m_useMultiBody = true; m_robotSim.loadURDF("cube_small.urdf", args); } { b3RobotSimulatorLoadFileResults results; m_robotSim.loadSDF("gripper/wsg50_with_r2d2_gripper.sdf",results); if (results.m_uniqueObjectIds.size()==1) { m_gripperIndex = results.m_uniqueObjectIds[0]; int numJoints = m_robotSim.getNumJoints(m_gripperIndex); b3Printf("numJoints = %d",numJoints); for (int i=0;i<numJoints;i++) { b3JointInfo jointInfo; m_robotSim.getJointInfo(m_gripperIndex,i,&jointInfo); b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName); } /* int fingerJointIndices[2]={1,3}; double fingerTargetPositions[2]={-0.04,0.04}; for (int i=0;i<2;i++) { b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_POSITION_VELOCITY_PD); controlArgs.m_targetPosition = fingerTargetPositions[i]; controlArgs.m_kp = 5.0; controlArgs.m_kd = 3.0; controlArgs.m_maxTorqueValue = 1.0; m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs); } */ int fingerJointIndices[3]={0,1,3}; double fingerTargetVelocities[3]={-0.2,.5,-.5}; double maxTorqueValues[3]={40.0,50.0,50.0}; for (int i=0;i<3;i++) { b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); controlArgs.m_targetVelocity = fingerTargetVelocities[i]; controlArgs.m_maxTorqueValue = maxTorqueValues[i]; controlArgs.m_kd = 1.; m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs); } } } if (1) { m_robotSim.loadURDF("plane.urdf"); } m_robotSim.setGravity(b3MakeVector3(0,0,-10)); m_robotSim.setNumSimulationSubSteps(4); } if ((m_options & eTWO_POINT_GRASP)!=0) { { b3RobotSimulatorLoadUrdfFileArgs args; args.m_startPosition.setValue(0, 0, .107); m_robotSim.loadURDF("cube_small.urdf", args); } { b3RobotSimulatorLoadUrdfFileArgs args; args.m_startPosition.setValue(0.068, 0.02, 0.11); m_robotSim.loadURDF("cube_gripper_left.urdf", args); b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); controlArgs.m_targetVelocity = -0.1; controlArgs.m_maxTorqueValue = 10.0; controlArgs.m_kd = 1.; m_robotSim.setJointMotorControl(1,0,controlArgs); } { b3RobotSimulatorLoadUrdfFileArgs args; args.m_startPosition.setValue(-0.068, 0.02, 0.11); m_robotSim.loadURDF("cube_gripper_right.urdf", args); b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); controlArgs.m_targetVelocity = 0.1; controlArgs.m_maxTorqueValue = 10.0; controlArgs.m_kd = 1.; m_robotSim.setJointMotorControl(2,0,controlArgs); } if (1) { m_robotSim.loadURDF("plane.urdf"); } m_robotSim.setGravity(b3MakeVector3(0,0,-10)); m_robotSim.setNumSimulationSubSteps(4); } if ((m_options & eONE_MOTOR_GRASP)!=0) { m_robotSim.setNumSolverIterations(150); { SliderParams slider("Vertical velocity",&sGripperVerticalVelocity); slider.m_minVal=-2; slider.m_maxVal=2; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { SliderParams slider("Closing velocity",&sGripperClosingTargetVelocity ); slider.m_minVal=-1; slider.m_maxVal=1; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { b3RobotSimulatorLoadUrdfFileArgs args; args.m_startPosition.setValue(0, -0.2, .47); args.m_startOrientation.setEulerZYX(SIMD_HALF_PI, 0, 0); m_robotSim.loadURDF("dinnerware/pan_tefal.urdf", args); } { b3RobotSimulatorLoadFileResults args; b3RobotSimulatorLoadFileResults results; m_robotSim.loadSDF("gripper/wsg50_one_motor_gripper_new.sdf",results); if (results.m_uniqueObjectIds.size()==1) { m_gripperIndex = results.m_uniqueObjectIds[0]; int numJoints = m_robotSim.getNumJoints(m_gripperIndex); b3Printf("numJoints = %d",numJoints); for (int i=0;i<numJoints;i++) { b3JointInfo jointInfo; m_robotSim.getJointInfo(m_gripperIndex,i,&jointInfo); b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName); } for (int i=0;i<8;i++) { b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); controlArgs.m_maxTorqueValue = 0.0; m_robotSim.setJointMotorControl(m_gripperIndex,i,controlArgs); } } } if (1) { m_robotSim.loadURDF("plane.urdf"); } m_robotSim.setGravity(b3MakeVector3(0,0,-10)); b3JointInfo revoluteJoint1; revoluteJoint1.m_parentFrame[0] = -0.055; revoluteJoint1.m_parentFrame[1] = 0; revoluteJoint1.m_parentFrame[2] = 0.02; revoluteJoint1.m_parentFrame[3] = 0; revoluteJoint1.m_parentFrame[4] = 0; revoluteJoint1.m_parentFrame[5] = 0; revoluteJoint1.m_parentFrame[6] = 1.0; revoluteJoint1.m_childFrame[0] = 0; revoluteJoint1.m_childFrame[1] = 0; revoluteJoint1.m_childFrame[2] = 0; revoluteJoint1.m_childFrame[3] = 0; revoluteJoint1.m_childFrame[4] = 0; revoluteJoint1.m_childFrame[5] = 0; revoluteJoint1.m_childFrame[6] = 1.0; revoluteJoint1.m_jointAxis[0] = 1.0; revoluteJoint1.m_jointAxis[1] = 0.0; revoluteJoint1.m_jointAxis[2] = 0.0; revoluteJoint1.m_jointType = ePoint2PointType; b3JointInfo revoluteJoint2; revoluteJoint2.m_parentFrame[0] = 0.055; revoluteJoint2.m_parentFrame[1] = 0; revoluteJoint2.m_parentFrame[2] = 0.02; revoluteJoint2.m_parentFrame[3] = 0; revoluteJoint2.m_parentFrame[4] = 0; revoluteJoint2.m_parentFrame[5] = 0; revoluteJoint2.m_parentFrame[6] = 1.0; revoluteJoint2.m_childFrame[0] = 0; revoluteJoint2.m_childFrame[1] = 0; revoluteJoint2.m_childFrame[2] = 0; revoluteJoint2.m_childFrame[3] = 0; revoluteJoint2.m_childFrame[4] = 0; revoluteJoint2.m_childFrame[5] = 0; revoluteJoint2.m_childFrame[6] = 1.0; revoluteJoint2.m_jointAxis[0] = 1.0; revoluteJoint2.m_jointAxis[1] = 0.0; revoluteJoint2.m_jointAxis[2] = 0.0; revoluteJoint2.m_jointType = ePoint2PointType; m_robotSim.createConstraint(1, 2, 1, 4, &revoluteJoint1); m_robotSim.createConstraint(1, 3, 1, 6, &revoluteJoint2); } if ((m_options & eGRASP_SOFT_BODY)!=0) { { SliderParams slider("Vertical velocity",&sGripperVerticalVelocity); slider.m_minVal=-2; slider.m_maxVal=2; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { SliderParams slider("Closing velocity",&sGripperClosingTargetVelocity ); slider.m_minVal=-1; slider.m_maxVal=1; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { b3RobotSimulatorLoadFileResults results; m_robotSim.loadSDF("gripper/wsg50_one_motor_gripper_new.sdf",results); if (results.m_uniqueObjectIds.size()==1) { m_gripperIndex = results.m_uniqueObjectIds[0]; int numJoints = m_robotSim.getNumJoints(m_gripperIndex); b3Printf("numJoints = %d",numJoints); for (int i=0;i<numJoints;i++) { b3JointInfo jointInfo; m_robotSim.getJointInfo(m_gripperIndex,i,&jointInfo); b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName); } for (int i=0;i<8;i++) { b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); controlArgs.m_maxTorqueValue = 0.0; m_robotSim.setJointMotorControl(m_gripperIndex,i,controlArgs); } } } { b3RobotSimulatorLoadUrdfFileArgs args; args.m_startPosition.setValue(0,0,-0.2); args.m_startOrientation.setEulerZYX(0,0,0); m_robotSim.loadURDF("plane.urdf", args); } m_robotSim.setGravity(b3MakeVector3(0,0,-10)); m_robotSim.loadBunny(0.1,0.1,0.02); b3JointInfo revoluteJoint1; revoluteJoint1.m_parentFrame[0] = -0.055; revoluteJoint1.m_parentFrame[1] = 0; revoluteJoint1.m_parentFrame[2] = 0.02; revoluteJoint1.m_parentFrame[3] = 0; revoluteJoint1.m_parentFrame[4] = 0; revoluteJoint1.m_parentFrame[5] = 0; revoluteJoint1.m_parentFrame[6] = 1.0; revoluteJoint1.m_childFrame[0] = 0; revoluteJoint1.m_childFrame[1] = 0; revoluteJoint1.m_childFrame[2] = 0; revoluteJoint1.m_childFrame[3] = 0; revoluteJoint1.m_childFrame[4] = 0; revoluteJoint1.m_childFrame[5] = 0; revoluteJoint1.m_childFrame[6] = 1.0; revoluteJoint1.m_jointAxis[0] = 1.0; revoluteJoint1.m_jointAxis[1] = 0.0; revoluteJoint1.m_jointAxis[2] = 0.0; revoluteJoint1.m_jointType = ePoint2PointType; b3JointInfo revoluteJoint2; revoluteJoint2.m_parentFrame[0] = 0.055; revoluteJoint2.m_parentFrame[1] = 0; revoluteJoint2.m_parentFrame[2] = 0.02; revoluteJoint2.m_parentFrame[3] = 0; revoluteJoint2.m_parentFrame[4] = 0; revoluteJoint2.m_parentFrame[5] = 0; revoluteJoint2.m_parentFrame[6] = 1.0; revoluteJoint2.m_childFrame[0] = 0; revoluteJoint2.m_childFrame[1] = 0; revoluteJoint2.m_childFrame[2] = 0; revoluteJoint2.m_childFrame[3] = 0; revoluteJoint2.m_childFrame[4] = 0; revoluteJoint2.m_childFrame[5] = 0; revoluteJoint2.m_childFrame[6] = 1.0; revoluteJoint2.m_jointAxis[0] = 1.0; revoluteJoint2.m_jointAxis[1] = 0.0; revoluteJoint2.m_jointAxis[2] = 0.0; revoluteJoint2.m_jointType = ePoint2PointType; m_robotSim.createConstraint(0, 2, 0, 4, &revoluteJoint1); m_robotSim.createConstraint(0, 3, 0, 6, &revoluteJoint2); } if ((m_options & eSOFTBODY_MULTIBODY_COUPLING)!=0) { { b3RobotSimulatorLoadUrdfFileArgs args; args.m_startPosition.setValue(0,1.0,2.0); args.m_startOrientation.setEulerZYX(0,0,1.57); args.m_forceOverrideFixedBase = false; args.m_useMultiBody = true; int kukaId = m_robotSim.loadURDF("kuka_iiwa/model_free_base.urdf", args); int numJoints = m_robotSim.getNumJoints(kukaId); b3Printf("numJoints = %d",numJoints); for (int i=0;i<numJoints;i++) { b3JointInfo jointInfo; m_robotSim.getJointInfo(kukaId,i,&jointInfo); b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName); b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); controlArgs.m_maxTorqueValue = 0.0; m_robotSim.setJointMotorControl(kukaId,i,controlArgs); } } { b3RobotSimulatorLoadUrdfFileArgs args; args.m_startPosition.setValue(0,0,0); args.m_startOrientation.setEulerZYX(0,0,0); args.m_forceOverrideFixedBase = true; args.m_useMultiBody = false; m_robotSim.loadURDF("plane.urdf", args); } m_robotSim.setGravity(b3MakeVector3(0,0,-10)); m_robotSim.loadBunny(0.5,10.0,0.1); } }
bool initialize(FusePrivate* data) { int ret; ArchiveType* archive_type = NULL; if (data->drivers_path) path_to_drivers = realpath(data->drivers_path, NULL); LOAD_STANDARD_DRIVERS(); if (drivers->empty()) return false; FileSystem::setBufferLimit(data->buffer_limit); if (data->keep_trash) FileSystem::keep_trash = true; if (data->respect_rights) ArchiveDriver::respect_rights = true; if (data->keep_original) ArchiveDriver::keep_original = true; /* První část inicializace */ if (data->create_archive) { data->mode = FusePrivate::ARCHIVE_MOUNTED; char* path = (char*)malloc(PATH_MAX); memset(path, 0, PATH_MAX); getcwd(path, PATH_MAX); strcat(path, "/"); strcat(path, data->mounted); data->mounted = path; /* soubor již existuje */ if (access(path, F_OK) == 0) { cerr << "Archive file with specified name already exist" << endl; return false; } /* Najdeme koncovku a ovladač */ const char* ext = findFileExt(data->mounted, NULL); archive_type = TYPE_BY_EXT(ext); if (archive_type == NULL) { cerr << "Error: this type of archive files is not supported" << endl; /* Při destrukci FusePrivate bude data->mounted uvolňován */ data->mounted = NULL; return false; } if (!archive_type->write_support) { /* Při destrukci FusePrivate bude data->mounted uvolňován */ data->mounted = NULL; cerr << "Write support for this type of archive is not implemented, sorry" << endl; return false; } } else { data->mounted = realpath(data->mounted, NULL); ret = errno; if (data->mounted == NULL) { cerr << "Error: " << strerror(ret) << endl; return false; } ret = setMountMode(data); if (ret == -1) { cerr << "Unsupported type of source file, sorry" << endl; return false; } else if (ret > 0) { cerr << "Error: " << strerror(errno) << endl; return false; } } /* Kontrola kombinace parametrů */ if (!controlArgs(data)) { cerr << "Invalid combination of parameters" << endl; return false; } /* Druhá část inicializace - budování FileSystémů ze zdrojů */ FileSystem* fs = NULL; if (data->mode == FusePrivate::ARCHIVE_MOUNTED) { /* Pokud se vytváří nový archiv, ovladač je již načten. * Pokud ne, je třeba jej načíst. */ if (!data->create_archive) { /* data->mounted už je realpath-ed */ archive_type = GET_TYPE(data->mounted); if (archive_type == NULL) { cerr << "Error: Could not load driver for " << data->mounted << endl; return false; } } try { fs = new FileSystem(data->mounted, data->create_archive, archive_type); } catch (ArchiveDriver::ArchiveError&) { cerr << "Error: Failed to open/process archive file" << endl; return false; } data->filesystems->insert(fs); } else { DIR* dir; struct dirent* file; struct stat info; char filename[PATH_MAX]; char* filename_ptr; unsigned mounted_len = strlen(data->mounted); strcpy(filename, data->mounted); filename[mounted_len] = '/'; filename_ptr = filename + mounted_len + 1; /* Projdu celý připojený adresář a pokud obsahuje archivy podporovaných typů * vytvořím jejich filesystémy */ dir = opendir(data->mounted); while ((file = readdir(dir)) != NULL) { if (strcmp(file->d_name, ".") == 0 || strcmp(file->d_name, "..") == 0) continue; archive_type = NULL; // Získání celé cesty k archivu strcpy(filename_ptr, file->d_name); if (stat(filename, &info) != 0) continue; if (S_ISREG(info.st_mode) || S_ISLNK(info.st_mode)) { archive_type = GET_TYPE(filename); if (archive_type != NULL) { try { fs = new FileSystem(filename, false, archive_type); } catch (...) { continue; } data->filesystems->insert(fs); } } } closedir(dir); } // cout << "FILESYSTEM IS INITIALIZED" << endl; return true; }
virtual void initPhysics() { int mode = eCONNECT_EXISTING_EXAMPLE_BROWSER; m_robotSim.setGuiHelper(m_guiHelper); bool connected = m_robotSim.connect(mode); b3Printf("robotSim connected = %d",connected); if ((m_options & eROBOTIC_LEARN_GRASP)!=0) { { b3RobotSimulatorLoadUrdfFileArgs args; args.m_startPosition.setValue(0,0,.5); m_r2d2Index = m_robotSim.loadURDF("r2d2.urdf",args); if (m_r2d2Index>=0) { int numJoints = m_robotSim.getNumJoints(m_r2d2Index); b3Printf("numJoints = %d",numJoints); for (int i=0;i<numJoints;i++) { b3JointInfo jointInfo; m_robotSim.getJointInfo(m_r2d2Index,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++) { b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); controlArgs.m_targetVelocity = wheelTargetVelocities[i]; controlArgs.m_maxTorqueValue = 1e30; m_robotSim.setJointMotorControl(m_r2d2Index,wheelJointIndices[i],controlArgs); } } } { b3RobotSimulatorLoadFileResults results; m_robotSim.loadSDF("kiva_shelf/model.sdf",results); } { m_robotSim.loadURDF("plane.urdf"); } m_robotSim.setGravity(b3MakeVector3(0,0,-10)); } if ((m_options & eROBOTIC_LEARN_COMPLIANT_CONTACT)!=0) { b3RobotSimulatorLoadUrdfFileArgs args; b3RobotSimulatorLoadFileResults results; { args.m_startPosition.setValue(0,0,2.5); args.m_startOrientation.setEulerZYX(0,0.2,0); m_r2d2Index = m_robotSim.loadURDF("cube_soft.urdf",args); } { args.m_startPosition.setValue(0,2,2.5); args.m_startOrientation.setEulerZYX(0,0.2,0); m_robotSim.loadURDF("cube_no_friction.urdf",args); } { args.m_startPosition.setValue(0,0,0); args.m_startOrientation.setEulerZYX(0,0.2,0); args.m_forceOverrideFixedBase = true; m_robotSim.loadURDF("plane.urdf",args); } m_robotSim.setGravity(b3MakeVector3(0,0,-10)); } if ((m_options & eROBOTIC_LEARN_ROLLING_FRICTION)!=0) { b3RobotSimulatorLoadUrdfFileArgs args; b3RobotSimulatorLoadFileResults results; { args.m_startPosition.setValue(0,0,2.5); args.m_startOrientation.setEulerZYX(0,0,0); args.m_useMultiBody = true; m_robotSim.loadURDF("sphere2_rolling_friction.urdf",args); } { args.m_startPosition.setValue(0,2,2.5); args.m_startOrientation.setEulerZYX(0,0,0); args.m_useMultiBody = true; m_robotSim.loadURDF("sphere2.urdf", args); } { args.m_startPosition.setValue(0,0,0); args.m_startOrientation.setEulerZYX(0,0.2,0); args.m_useMultiBody = true; args.m_forceOverrideFixedBase = true; m_robotSim.loadURDF("plane.urdf", args); } m_robotSim.setGravity(b3MakeVector3(0,0,-10)); } }