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

Merge remote-tracking branch 'bp/master'

Browse Source
This commit is contained in:
YunfeiBai
2016-09-28 13:54:35 -07:00
parent 032156fbf2 3bb5e02b18
commit dacca09614
8 changed files with 63 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
data/gripper/wsg50_one_motor_gripper_free_base.sdf
View File

@@ -302,7 +302,7 @@
</joint> </joint>
<link name='finger_right'> <link name='finger_right'>
<pose frame=''>0.062 0 0.145 0 0 1.5708</pose> <pose frame=''>0.042 0 0.145 0 0 1.5708</pose>
<inertial> <inertial>
<mass>0.2</mass> <mass>0.2</mass>
<inertia> <inertia>
@@ -342,7 +342,7 @@
</joint> </joint>
<link name='finger_left'> <link name='finger_left'>
<pose frame=''>-0.062 0 0.145 0 0 4.71239</pose> <pose frame=''>-0.042 0 0.145 0 0 4.71239</pose>
<inertial> <inertial>
<mass>0.2</mass> <mass>0.2</mass>
<inertia> <inertia>

2
data/kuka_iiwa/model.urdf
View File

@@ -174,7 +174,7 @@
<child link="lbr_iiwa_link_4"/> <child link="lbr_iiwa_link_4"/>
<origin rpy="1.57079632679 0 0" xyz="0 0 0.2155"/> <origin rpy="1.57079632679 0 0" xyz="0 0 0.2155"/>
<axis xyz="0 0 1"/> <axis xyz="0 0 1"/>
<limit effort="300" lower="-2.09439510239" upper="2.09439510239" velocity="10"/> <limit effort="300" lower=".29439510239" upper="2.09439510239" velocity="10"/>
<dynamics damping="0.5"/> <dynamics damping="0.5"/>
</joint> </joint>
<link name="lbr_iiwa_link_4"> <link name="lbr_iiwa_link_4">

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 b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody); int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase); 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 ///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics
b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex,
@@ -139,6 +139,7 @@ int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle status
double* jointPositions); double* jointPositions);
b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName); b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
///The b3JointControlCommandInit method is obsolete, use b3JointControlCommandInit2 instead ///The b3JointControlCommandInit method is obsolete, use b3JointControlCommandInit2 instead
b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode); 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; bool gEnableRealTimeSimVR=false;
int gCreateObjectSimVR = -1; int gCreateObjectSimVR = -1;
btScalar simTimeScalingFactor = 1; btScalar simTimeScalingFactor = 1;
btScalar gRhsClamp = 1.f;
struct UrdfLinkNameMapUtil struct UrdfLinkNameMapUtil
{ {
@@ -764,6 +765,7 @@ void PhysicsServerCommandProcessor::createJointMotors(btMultiBody* mb)
btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,dof,desiredVelocity,maxMotorImpulse); btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,dof,desiredVelocity,maxMotorImpulse);
motor->setPositionTarget(0, 0); motor->setPositionTarget(0, 0);
motor->setVelocityTarget(0, 1); motor->setVelocityTarget(0, 1);
motor->setRhsClamp(gRhsClamp);
//motor->setMaxAppliedImpulse(0); //motor->setMaxAppliedImpulse(0);
mb->getLink(mbLinkIndex).m_userPtr = motor; mb->getLink(mbLinkIndex).m_userPtr = motor;
m_data->m_dynamicsWorld->addMultiBodyConstraint(motor); m_data->m_dynamicsWorld->addMultiBodyConstraint(motor);
@@ -1553,6 +1555,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
btScalar desiredVelocity = 0.f; btScalar desiredVelocity = 0.f;
bool hasDesiredVelocity = false; bool hasDesiredVelocity = false;
if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex]&SIM_DESIRED_STATE_HAS_QDOT)!=0) if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex]&SIM_DESIRED_STATE_HAS_QDOT)!=0)
{ {
desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex]; desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex];
@@ -2554,7 +2557,23 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
world->addMultiBodyConstraint(rigidbodySlider); 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; SharedMemoryStatus& serverCmd =serverStatusOut;
@@ -2994,9 +3013,9 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
btMatrix3x3 frameInChild2(btQuaternion(0, 0, 1.0, 0)); btMatrix3x3 frameInChild2(btQuaternion(0, 0, 1.0, 0));
btVector3 jointAxis2(1.0, 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 = 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 = 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_kukaGripperSlider1);
m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperSlider2); m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperSlider2);
@@ -3055,8 +3074,9 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
if (motor) if (motor)
{ {
btScalar posTarget = (-0.045)*btMin(btScalar(0.75), gVRGripperAnalog) / 0.75; btScalar posTarget = (-0.045)*btMin(btScalar(0.75), gVRGripperAnalog) / 0.75;
motor->setPositionTarget(posTarget, 1.0); motor->setPositionTarget(posTarget, .2);
motor->setMaxAppliedImpulse(50.0); motor->setVelocityTarget(0.0, .5);
motor->setMaxAppliedImpulse(5.0);
} }
} }
@@ -3088,6 +3108,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
motor->setVelocityTarget(0, 0.5); motor->setVelocityTarget(0, 0.5);
btScalar maxImp = 1*m_data->m_physicsDeltaTime; btScalar maxImp = 1*m_data->m_physicsDeltaTime;
motor->setMaxAppliedImpulse(maxImp); motor->setMaxAppliedImpulse(maxImp);
motor->setRhsClamp(gRhsClamp);
} }
} }
} }
@@ -3113,13 +3134,14 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
} }
q_new.resize(numDofs); q_new.resize(numDofs);
q_new[0] = 1.376; //sensible rest-pose
q_new[1] = 1.102; q_new[0] = -SIMD_HALF_PI;
q_new[2] = 1.634; q_new[1] = 0;
q_new[3] = -0.406; q_new[2] = 0;
q_new[4] = 1.714; q_new[3] = SIMD_HALF_PI;
q_new[5] = -2.023; q_new[4] = 0;
q_new[6] = -1.306; q_new[5] = -SIMD_HALF_PI*0.66;
q_new[6] = 0;
if (closeToKuka) if (closeToKuka)
{ {
@@ -3250,12 +3272,12 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
{ {
btScalar desiredVelocity = 0.f; btScalar desiredVelocity = 0.f;
btScalar desiredPosition = q_new[link]; btScalar desiredPosition = q_new[link];
motor->setRhsClamp(gRhsClamp);
//printf("link %d: %f", link, q_new[link]); //printf("link %d: %f", link, q_new[link]);
motor->setVelocityTarget(desiredVelocity,1.0); motor->setVelocityTarget(desiredVelocity,1.0);
motor->setPositionTarget(desiredPosition,0.6); motor->setPositionTarget(desiredPosition,0.6);
btScalar maxImp = 1.0; btScalar maxImp = 1.0;
if (link == 0)
maxImp = 5.0;
motor->setMaxAppliedImpulse(maxImp); motor->setMaxAppliedImpulse(maxImp);
numMotors++; numMotors++;
} }

1
examples/SharedMemory/SharedMemoryPublic.h
View File

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

5
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
View File

@@ -31,9 +31,12 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
//solve featherstone non-contact constraints //solve featherstone non-contact constraints
//printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size()); //printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size());
for (int j=0;j<m_multiBodyNonContactConstraints.size();j++) for (int j=0;j<m_multiBodyNonContactConstraints.size();j++)
{ {
btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[j]; int index = iteration&1? j : m_multiBodyNonContactConstraints.size()-1-j;
btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[index];
resolveSingleConstraintRowGeneric(constraint); resolveSingleConstraintRowGeneric(constraint);
if(constraint.m_multiBodyA) if(constraint.m_multiBodyA)

15
src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
View File

@@ -27,7 +27,8 @@ btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScal
m_desiredPosition(0), m_desiredPosition(0),
m_kd(1.), m_kd(1.),
m_kp(0), m_kp(0),
m_erp(1) m_erp(1),
m_rhsClamp(SIMD_INFINITY)
{ {
m_maxAppliedImpulse = maxMotorImpulse; m_maxAppliedImpulse = maxMotorImpulse;
@@ -59,7 +60,8 @@ btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int li
m_desiredPosition(0), m_desiredPosition(0),
m_kd(1.), m_kd(1.),
m_kp(0), m_kp(0),
m_erp(1) m_erp(1),
m_rhsClamp(SIMD_INFINITY)
{ {
btAssert(linkDoF < body->getLink(link).m_dofCount); btAssert(linkDoF < body->getLink(link).m_dofCount);
@@ -126,8 +128,17 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con
btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof]; btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof]; btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
btScalar positionStabiliationTerm = m_erp*(m_desiredPosition-currentPosition)/infoGlobal.m_timeStep; btScalar positionStabiliationTerm = m_erp*(m_desiredPosition-currentPosition)/infoGlobal.m_timeStep;
btScalar velocityError = (m_desiredVelocity - currentVelocity); btScalar velocityError = (m_desiredVelocity - currentVelocity);
btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity+m_kd * velocityError; btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity+m_kd * velocityError;
if (rhs>m_rhsClamp)
{
rhs=m_rhsClamp;
}
if (rhs<-m_rhsClamp)
{
rhs=-m_rhsClamp;
}
fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,false,1,false,rhs); fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,false,1,false,rhs);

6
src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h
View File

@@ -30,6 +30,7 @@ protected:
btScalar m_kd; btScalar m_kd;
btScalar m_kp; btScalar m_kp;
btScalar m_erp; btScalar m_erp;
btScalar m_rhsClamp;//maximum error
public: public:
@@ -66,7 +67,10 @@ public:
{ {
return m_erp; return m_erp;
} }
virtual void setRhsClamp(btScalar rhsClamp)
{
m_rhsClamp = rhsClamp;
}
virtual void debugDraw(class btIDebugDraw* drawer) virtual void debugDraw(class btIDebugDraw* drawer)
{ {
//todo(erwincoumans) //todo(erwincoumans)