AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-13 05:10:08 +00:00
Code Issues Releases Wiki Activity

URDF: don't parse joint limit for continuous joints

Browse Source 
tweak KUKA iiwa inverse kinematics test in physics server
This commit is contained in:
erwincoumans 2016-09-22 23:21:24 -07:00
parent e356f4f1f6
commit b6bb937dc0
4 changed files with 28 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
build_visual_studio.bat
View File

@ -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

15
data/kuka_iiwa/model.urdf
View File

@ -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>

15
examples/Importers/ImportURDFDemo/UrdfParser.cpp
View File

@ -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)
{

25
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -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]);
}
}
}