diff --git a/data/gripper/wsg50_one_motor_gripper_free_base.sdf b/data/gripper/wsg50_one_motor_gripper_free_base.sdf index 73f244507..90de11f5c 100644 --- a/data/gripper/wsg50_one_motor_gripper_free_base.sdf +++ b/data/gripper/wsg50_one_motor_gripper_free_base.sdf @@ -302,7 +302,7 @@ - 0.062 0 0.145 0 0 1.5708 + 0.042 0 0.145 0 0 1.5708 0.2 @@ -342,7 +342,7 @@ - -0.062 0 0.145 0 0 4.71239 + -0.042 0 0.145 0 0 4.71239 0.2 diff --git a/data/kuka_iiwa/model.urdf b/data/kuka_iiwa/model.urdf index a6e53d3ab..c3ef25fd6 100644 --- a/data/kuka_iiwa/model.urdf +++ b/data/kuka_iiwa/model.urdf @@ -174,7 +174,7 @@ - + diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index d35f472fb..a998ddea4 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -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); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 8d14e5ce4..405a7ffcf 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -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++; } diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 9b1068470..1490fae68 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -99,6 +99,7 @@ enum JointType { eRevoluteType = 0, ePrismaticType = 1, eFixedType = 2, + ePoint2PointType = 3, }; struct b3JointInfo diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index f2ab1b1ee..a9b929f36 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -31,9 +31,12 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl //solve featherstone non-contact constraints //printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size()); + for (int j=0;jgetLink(link).m_dofCount); @@ -126,8 +128,17 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof]; btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof]; btScalar positionStabiliationTerm = m_erp*(m_desiredPosition-currentPosition)/infoGlobal.m_timeStep; + btScalar velocityError = (m_desiredVelocity - currentVelocity); 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); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h index ceac3b65e..7cf57311a 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h @@ -30,6 +30,7 @@ protected: btScalar m_kd; btScalar m_kp; btScalar m_erp; + btScalar m_rhsClamp;//maximum error public: @@ -66,7 +67,10 @@ public: { return m_erp; } - + virtual void setRhsClamp(btScalar rhsClamp) + { + m_rhsClamp = rhsClamp; + } virtual void debugDraw(class btIDebugDraw* drawer) { //todo(erwincoumans)