URDF: don't parse joint limit for continuous jointsiktest

tweak KUKA iiwa inverse kinematics test in physics server

4 files changed, 28 insertions, 34 deletions

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 @@
     </collision>
   </link>
   
-    <!-- joint between link_7 and lbr_iiwa_link_endeffector -->
-  <joint name="lbr_iiwa_joint_8" type="fixed">
-    <parent link="lbr_iiwa_link_7"/>
-    <child link="lbr_iiwa_link_endeffector"/>
-    <origin rpy="0 0 0" xyz="0 0.0 0.08"/>
-    <axis xyz="0 0 1"/>
-  </joint>
-  <link name="lbr_iiwa_link_endeffector">
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0.02"/>
-      <mass value="0.3"/>
-      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
-    </inertial>
-  </link>
-
 </robot>
 
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 && (endEffectorLinkIndex<bodyHandle->m_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<numDofs;i++)
-					   {
-						   //printf("q_new[i] = %f\n", q_new[i]);
-					   }
 					}
 				}