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

fix minitaur.urdf: move lower-leg inertia to center, add missing collision for one of the motors, add contact parameters for friction_anchor, spinning friction, compliance (stiffness/damping)

Browse Source 
fix in indexing for maximal coordinates (unused by default, still experimental, requires many iterations for Minitaur due to extreme mass-ratio, hence use of reduces/generalized coordinates)
modify quadruped.py to test maximal coordinates
wrap angular servo (positional) target within [-PI,PI] in btGeneric6DofSpring2Constraint
add 'j' key to show body frames in wireframe/debug mode
This commit is contained in:
erwincoumans
2017-09-02 01:05:42 -07:00
parent ee30479a28
commit eb97e06280
9 changed files with 281 additions and 108 deletions
Download Patch File Download Diff File Expand all files Collapse all files

63
src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp
View File

@@ -729,6 +729,21 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
if (limot->m_enableMotor && limot->m_servoMotor)
{
btScalar error = limot->m_currentPosition - limot->m_servoTarget;
btScalar curServoTarget = limot->m_servoTarget;
if (rotational)
{
if (error > SIMD_PI)
{
error -= SIMD_2_PI;
curServoTarget +=SIMD_2_PI;
}
if (error < -SIMD_PI)
{
error += SIMD_2_PI;
curServoTarget -=SIMD_2_PI;
}
}
calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
btScalar targetvelocity = error<0 ? -limot->m_targetVelocity : limot->m_targetVelocity;
btScalar tag_vel = -targetvelocity;
@@ -739,13 +754,13 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
btScalar hiLimit;
if(limot->m_loLimit > limot->m_hiLimit)
{
lowLimit = error > 0 ? limot->m_servoTarget : -SIMD_INFINITY;
hiLimit = error < 0 ? limot->m_servoTarget : SIMD_INFINITY;
lowLimit = error > 0 ? curServoTarget : -SIMD_INFINITY;
hiLimit = error < 0 ? curServoTarget : SIMD_INFINITY;
}
else
{
lowLimit = error > 0 && limot->m_servoTarget>limot->m_loLimit ? limot->m_servoTarget : limot->m_loLimit;
hiLimit = error < 0 && limot->m_servoTarget<limot->m_hiLimit ? limot->m_servoTarget : limot->m_hiLimit;
lowLimit = error > 0 && curServoTarget>limot->m_loLimit ? curServoTarget : limot->m_loLimit;
hiLimit = error < 0 && curServoTarget<limot->m_hiLimit ? curServoTarget : limot->m_hiLimit;
}
mot_fact = getMotorFactor(limot->m_currentPosition, lowLimit, hiLimit, tag_vel, info->fps * limot->m_motorERP);
}
@@ -998,13 +1013,49 @@ void btGeneric6DofSpring2Constraint::setTargetVelocity(int index, btScalar veloc
m_angularLimits[index - 3].m_targetVelocity = velocity;
}
void btGeneric6DofSpring2Constraint::setServoTarget(int index, btScalar target)
void btGeneric6DofSpring2Constraint::setServoTarget(int index, btScalar targetOrg)
{
btAssert((index >= 0) && (index < 6));
if (index<3)
m_linearLimits.m_servoTarget[index] = target;
{
m_linearLimits.m_servoTarget[index] = targetOrg;
}
else
{
//wrap between -PI and PI, see also
//https://stackoverflow.com/questions/4633177/c-how-to-wrap-a-float-to-the-interval-pi-pi
btScalar target = targetOrg+SIMD_PI;
if (1)
{
btScalar m = target - SIMD_2_PI * floor(target/SIMD_2_PI);
// handle boundary cases resulted from floating-point cut off:
{
if (m>=SIMD_2_PI)
{
target = 0;
} else
{
if (m<0 )
{
if (SIMD_2_PI+m == SIMD_2_PI)
target = 0;
else
target = SIMD_2_PI+m;
}
else
{
target = m;
}
}
}
target -= SIMD_PI;
}
m_angularLimits[index - 3].m_servoTarget = target;
}
}
void btGeneric6DofSpring2Constraint::setMaxMotorForce(int index, btScalar force)