Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

center the gripper

Browse Source 
fix collision issue, introduced in previous commit (uninitialized new variable)
This commit is contained in:
erwincoumans
2016-11-09 22:14:04 -08:00
parent 6661b8d977
commit b16d9abddd
2 changed files with 23 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -3828,6 +3828,8 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
{
m_data->m_gripperRigidbodyFixed->setFrameInB(btMatrix3x3(gVRGripperOrn));
m_data->m_gripperRigidbodyFixed->setPivotInB(gVRGripperPos);
btScalar avg = 0.f;
for (int i = 0; i < m_data->m_gripperMultiBody->getNumLinks(); i++)
{
if (supportsJointMotor(m_data->m_gripperMultiBody, i))
@@ -3838,7 +3840,14 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
motor->setErp(0.2);
btScalar posTarget = 0.1 + (1 - btMin(btScalar(0.75),gVRGripperAnalog)*btScalar(1.5))*SIMD_HALF_PI*0.29;
btScalar maxPosTarget = 0.55;
btScalar correction = 0.f;
if (avg)
{
correction = m_data->m_gripperMultiBody->getJointPos(i) - avg;
}
if (m_data->m_gripperMultiBody->getJointPos(i) < 0)
{
m_data->m_gripperMultiBody->setJointPos(i,0);
@@ -3848,10 +3857,19 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
m_data->m_gripperMultiBody->setJointPos(i, maxPosTarget);
}
motor->setPositionTarget(posTarget, 1);
if (avg)
{
motor->setPositionTarget(avg, 1);
}
else
{
motor->setPositionTarget(posTarget - correction*5., 1);
}
motor->setVelocityTarget(0, 0.5);
btScalar maxImp = 1*m_data->m_physicsDeltaTime;
btScalar maxImp = (1+0.1*i)*m_data->m_physicsDeltaTime;
motor->setMaxAppliedImpulse(maxImp);
avg = m_data->m_gripperMultiBody->getJointPos(i);
//motor->setRhsClamp(gRhsClamp);
}
}
@@ -3859,7 +3877,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
}
// Inverse kinematics for KUKA
//if (0)
if (m_data->m_KukaId>=0)
{
InternalBodyHandle* bodyHandle = m_data->getHandle(m_data->m_KukaId);
if (bodyHandle && bodyHandle->m_multiBody && bodyHandle->m_multiBody->getNumDofs()==7)