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

ping-pong back/forward in PGS solving iterations to reduce bias in constraint order

Browse Source 
add experimental rhs clamp in btMultiBodyJointMotor to control maximum error resolution.
This commit is contained in:
erwin coumans
2016-09-28 11:17:11 -07:00
parent cf046093ab
commit b81eb79ef5
8 changed files with 63 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -114,7 +114,7 @@ int b3LoadUrdfCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle,
int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase);
int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics
b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex,
@@ -139,6 +139,7 @@ int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle status
double* jointPositions);
b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
///The b3JointControlCommandInit method is obsolete, use b3JointControlCommandInit2 instead
b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode);

48
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -30,6 +30,7 @@ btVector3 gLastPickPos(0, 0, 0);
bool gEnableRealTimeSimVR=false;
int gCreateObjectSimVR = -1;
btScalar simTimeScalingFactor = 1;
btScalar gRhsClamp = 1.f;
struct UrdfLinkNameMapUtil
{
@@ -764,6 +765,7 @@ void PhysicsServerCommandProcessor::createJointMotors(btMultiBody* mb)
btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,dof,desiredVelocity,maxMotorImpulse);
motor->setPositionTarget(0, 0);
motor->setVelocityTarget(0, 1);
motor->setRhsClamp(gRhsClamp);
//motor->setMaxAppliedImpulse(0);
mb->getLink(mbLinkIndex).m_userPtr = motor;
m_data->m_dynamicsWorld->addMultiBodyConstraint(motor);
@@ -1553,6 +1555,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
btScalar desiredVelocity = 0.f;
bool hasDesiredVelocity = false;
if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex]&SIM_DESIRED_STATE_HAS_QDOT)!=0)
{
desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex];
@@ -2554,7 +2557,23 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
world->addMultiBodyConstraint(rigidbodySlider);
}
} else if (clientCmd.m_createJointArguments.m_jointType == ePoint2PointType)
{
if (childBody->m_multiBody)
{
btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_createJointArguments.m_childJointIndex,pivotInParent,pivotInChild);
p2p->setMaxAppliedImpulse(500);
m_data->m_dynamicsWorld->addMultiBodyConstraint(p2p);
}
else
{
btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild);
p2p->setMaxAppliedImpulse(500);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
world->addMultiBodyConstraint(p2p);
}
}
}
}
SharedMemoryStatus& serverCmd =serverStatusOut;
@@ -2994,9 +3013,9 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
btMatrix3x3 frameInChild2(btQuaternion(0, 0, 1.0, 0));
btVector3 jointAxis2(1.0, 0, 0);
m_data->m_kukaGripperSlider1 = new btMultiBodySliderConstraint(gripperBody->m_multiBody, 0, gripperBody->m_multiBody, 3, pivotInParent1, pivotInChild1, frameInParent1, frameInChild1, jointAxis1);
m_data->m_kukaGripperSlider1->setMaxAppliedImpulse(500.0);
m_data->m_kukaGripperSlider1->setMaxAppliedImpulse(5.0);
m_data->m_kukaGripperSlider2 = new btMultiBodySliderConstraint(gripperBody->m_multiBody, 0, gripperBody->m_multiBody, 6, pivotInParent2, pivotInChild2, frameInParent2, frameInChild2, jointAxis2);
m_data->m_kukaGripperSlider2->setMaxAppliedImpulse(500.0);
m_data->m_kukaGripperSlider2->setMaxAppliedImpulse(5.0);
m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperSlider1);
m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperSlider2);
@@ -3046,8 +3065,9 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
if (motor)
{
btScalar posTarget = (-0.045)*btMin(btScalar(0.75), gVRGripperAnalog) / 0.75;
motor->setPositionTarget(posTarget, 1.0);
motor->setMaxAppliedImpulse(50.0);
motor->setPositionTarget(posTarget, .2);
motor->setVelocityTarget(0.0, .5);
motor->setMaxAppliedImpulse(5.0);
}
}
@@ -3079,6 +3099,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
motor->setVelocityTarget(0, 0.5);
btScalar maxImp = 1*m_data->m_physicsDeltaTime;
motor->setMaxAppliedImpulse(maxImp);
motor->setRhsClamp(gRhsClamp);
}
}
}
@@ -3104,13 +3125,14 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
}
q_new.resize(numDofs);
q_new[0] = 1.376;
q_new[1] = 1.102;
q_new[2] = 1.634;
q_new[3] = -0.406;
q_new[4] = 1.714;
q_new[5] = -2.023;
q_new[6] = -1.306;
//sensible rest-pose
q_new[0] = -SIMD_HALF_PI;
q_new[1] = 0;
q_new[2] = 0;
q_new[3] = SIMD_HALF_PI;
q_new[4] = 0;
q_new[5] = -SIMD_HALF_PI*0.66;
q_new[6] = 0;
if (closeToKuka)
{
@@ -3241,12 +3263,12 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
{
btScalar desiredVelocity = 0.f;
btScalar desiredPosition = q_new[link];
motor->setRhsClamp(gRhsClamp);
//printf("link %d: %f", link, q_new[link]);
motor->setVelocityTarget(desiredVelocity,1.0);
motor->setPositionTarget(desiredPosition,0.6);
btScalar maxImp = 1.0;
if (link == 0)
maxImp = 5.0;
motor->setMaxAppliedImpulse(maxImp);
numMotors++;
}

1
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -99,6 +99,7 @@ enum JointType {
eRevoluteType = 0,
ePrismaticType = 1,
eFixedType = 2,
ePoint2PointType = 3,
};
struct b3JointInfo