diff --git a/build_visual_studio.bat b/build_visual_studio.bat index 0b852e498..302194631 100644 --- a/build_visual_studio.bat +++ b/build_visual_studio.bat @@ -1,6 +1,9 @@ +IF NOT EXIST bin mkdir bin +IF NOT EXIST bin\openvr_api.dll copy examples\ThirdPartyLibs\openvr\bin\win32\openvr_api.dll bin + cd build3 -rem premake4 --enable_openvr --targetdir="../bin" vs2010 +premake4 --enable_openvr --targetdir="../bin" vs2010 -premake4 --double --enable_pybullet --python_include_dir="C:/Python-3.5.2/include" --python_lib_dir="C:/Python-3.5.2/libs" --enable_openvr --targetdir="../bin" vs2010 +rem premake4 --double --enable_pybullet --python_include_dir="C:/Python-3.5.2/include" --python_lib_dir="C:/Python-3.5.2/libs" --enable_openvr --targetdir="../bin" vs2010 pause diff --git a/data/kuka_iiwa/model.urdf b/data/kuka_iiwa/model.urdf index bc555df93..a6e53d3ab 100644 --- a/data/kuka_iiwa/model.urdf +++ b/data/kuka_iiwa/model.urdf @@ -285,20 +285,5 @@ - - - - - - - - - - - - - - - diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index 3abc77fa2..817cb0ce5 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -1057,12 +1057,15 @@ bool UrdfParser::parseJoint(UrdfJoint& joint, TiXmlElement *config, ErrorLogger* TiXmlElement *limit_xml = axis_xml->FirstChildElement("limit"); if (limit_xml) { - if (!parseJointLimits(joint, limit_xml,logger)) - { - logger->reportError("Could not parse limit element for joint:"); - logger->reportError(joint.m_name.c_str()); - return false; - } + if (joint.m_type != URDFContinuousJoint) + { + if (!parseJointLimits(joint, limit_xml,logger)) + { + logger->reportError("Could not parse limit element for joint:"); + logger->reportError(joint.m_name.c_str()); + return false; + } + } } else if (joint.m_type == URDFRevoluteJoint) { diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 6b24fc462..cf38701a7 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -28,7 +28,7 @@ #include "SharedMemoryCommands.h" btVector3 gLastPickPos(0, 0, 0); -bool gEnableRealTimeSimVR=true; +bool gEnableRealTimeSimVR=false; int gCreateObjectSimVR = -1; btScalar simTimeScalingFactor = 1; @@ -753,7 +753,7 @@ void PhysicsServerCommandProcessor::createJointMotors(btMultiBody* mb) if (supportsJointMotor(mb,mbLinkIndex)) { - float maxMotorImpulse = 10000.f; + float maxMotorImpulse = 1.f; int dof = 0; btScalar desiredVelocity = 0.f; btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,dof,desiredVelocity,maxMotorImpulse); @@ -3080,7 +3080,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) ikHelperPtr = tmpHelper; } - int endEffectorLinkIndex = 7; + int endEffectorLinkIndex = 6; if (ikHelperPtr && (endEffectorLinkIndexm_multiBody->getNumLinks())) { @@ -3138,6 +3138,8 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) btVector3DoubleData endEffectorWorldPosition; btVector3DoubleData endEffectorWorldOrientation; + btVector3DoubleData targetWorldPosition; + btQuaternionDoubleData targetWorldOrientation; btVector3 endEffectorPosWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform.getOrigin(); btQuaternion endEffectorOriWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform.getRotation(); @@ -3145,24 +3147,25 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) endEffectorPosWorld.serializeDouble(endEffectorWorldPosition); endEffectorOri.serializeDouble(endEffectorWorldOrientation); + gVRController2Pos.serializeDouble(targetWorldPosition); + gVRController2Orn.serializeDouble(targetWorldOrientation); static btScalar time=0.f; time+=0.01; btVector3 targetPos(0.4-0.4*b3Cos( time), 0, 0.8+0.4*b3Cos( time)); targetPos +=mb->getBasePos(); - btQuaternion targetOri(0, 1.0, 0, 0); - btQuaternion downOrn(0,1,0,0); + btQuaternion fwdOri(btVector3(1,0,0),-SIMD_HALF_PI); + (0, 1.0, 0, 0); + double downOrn[4] = {0,1,0,0}; + //double downOrn[4] = {0,1,0,0}; + + fwdOri.serializeDouble(targetWorldOrientation); - ikHelperPtr->computeIK( //targetPos,targetOri, - gVRController2Pos, downOrn,//gVRController2Orn, + ikHelperPtr->computeIK(targetWorldPosition.m_floats, targetWorldOrientation.m_floats, endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats, &q_current[0], numDofs, endEffectorLinkIndex, &q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2, dampIk); - for (int i=0;i