From 2d40a1831542ee54911ab0e10f1afc730532c862 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Fri, 1 Jun 2018 09:34:18 -0700 Subject: [PATCH 1/7] picking shouldn't activate (wakeup) sleeping objects --- .../PhysicsServerCommandProcessor.cpp | 31 ++++++++++--------- 1 file changed, 17 insertions(+), 14 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index e74533c34..1bf44c362 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -9479,23 +9479,26 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons btMultiBodyLinkCollider* multiCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(rayCallback.m_collisionObject); if (multiCol && multiCol->m_multiBody) { + //todo: don't activate 'static' colliders/objects + if (!(multiCol->m_multiBody->getNumLinks()==0 && multiCol->m_multiBody->getBaseMass()==0)) + { + m_data->m_prevCanSleep = multiCol->m_multiBody->getCanSleep(); + multiCol->m_multiBody->setCanSleep(false); - m_data->m_prevCanSleep = multiCol->m_multiBody->getCanSleep(); - multiCol->m_multiBody->setCanSleep(false); + btVector3 pivotInA = multiCol->m_multiBody->worldPosToLocal(multiCol->m_link, pickPos); - btVector3 pivotInA = multiCol->m_multiBody->worldPosToLocal(multiCol->m_link, pickPos); + btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(multiCol->m_multiBody,multiCol->m_link,0,pivotInA,pickPos); + //if you add too much energy to the system, causing high angular velocities, simulation 'explodes' + //see also http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=949 + //so we try to avoid it by clamping the maximum impulse (force) that the mouse pick can apply + //it is not satisfying, hopefully we find a better solution (higher order integrator, using joint friction using a zero-velocity target motor with limited force etc?) + btScalar scaling=1; + p2p->setMaxAppliedImpulse(2*scaling); - btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(multiCol->m_multiBody,multiCol->m_link,0,pivotInA,pickPos); - //if you add too much energy to the system, causing high angular velocities, simulation 'explodes' - //see also http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=949 - //so we try to avoid it by clamping the maximum impulse (force) that the mouse pick can apply - //it is not satisfying, hopefully we find a better solution (higher order integrator, using joint friction using a zero-velocity target motor with limited force etc?) - btScalar scaling=1; - p2p->setMaxAppliedImpulse(2*scaling); - - btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; - world->addMultiBodyConstraint(p2p); - m_data->m_pickingMultiBodyPoint2Point =p2p; + btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; + world->addMultiBodyConstraint(p2p); + m_data->m_pickingMultiBodyPoint2Point =p2p; + } } } From 54ddbad29f2d73f5cbecdd1e5e70c6a09f38920c Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Fri, 1 Jun 2018 21:56:34 -0700 Subject: [PATCH 2/7] remove printf --- .../btMultiBodyConstraintSolver.cpp | 33 ++----------------- 1 file changed, 3 insertions(+), 30 deletions(-) diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 68ba8d68f..c8f7fb0aa 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -26,7 +26,6 @@ subject to the following restrictions: btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) { - printf("btSequentialImpulseConstraintSolver::solveSingleIteration\n"); btScalar leastSquaredResidual = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer); //solve featherstone non-contact constraints @@ -39,7 +38,6 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[index]; - printf("m_multiBodyNonContactConstraints resolveSingleConstraintRowGeneric\n"); btScalar residual = resolveSingleConstraintRowGeneric(constraint); leastSquaredResidual += residual*residual; @@ -48,8 +46,7 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl if(constraint.m_multiBodyB) constraint.m_multiBodyB->setPosUpdated(false); } - printf("featherstone normal contact\n"); - printf("numContact=%d\n", m_multiBodyNormalContactConstraints.size()); + //solve featherstone normal contact for (int j0=0;j0setPosUpdated(false); } - printf("featherstone frictional contact\n"); //solve featherstone frictional contact if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode&SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) == 0)) { @@ -163,7 +158,6 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl } } } - printf("end featjerstp\n"); return leastSquaredResidual; } @@ -209,19 +203,6 @@ btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const bt int ndofA = 0; int ndofB = 0; - printf("c.m_solverBodyIdA=%d\n", c.m_solverBodyIdA); - printf("c.m_solverBodyIdB=%d\n", c.m_solverBodyIdB); - - printf("c.m_multiBodyA=%p\n", c.m_multiBodyA); - printf("c.m_multiBodyB=%p\n", c.m_multiBodyB); - printf("c.m_jacAindex=%d\n",c.m_jacAindex); - printf("c.m_jacBindex=%d\n",c.m_jacBindex); - printf("c.m_deltaVelAindex=%d\n",c.m_deltaVelAindex); - printf("c.m_deltaVelBindex=%d\n",c.m_deltaVelBindex); - if (c.m_deltaVelAindex>500) - { - printf("???\n"); - } if (c.m_multiBodyA) { ndofA = c.m_multiBodyA->getNumDofs() + 6; @@ -558,13 +539,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol } const int ndofA = multiBodyA->getNumDofs() + 6; - solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); - if (solverConstraint.m_deltaVelAindex>10000) - { - printf("????????????????????????????????????????\n"); - printf("solverConstraint.m_deltaVelAindex==%d\n", solverConstraint.m_deltaVelAindex); - } if (solverConstraint.m_deltaVelAindex <0) { @@ -1682,13 +1657,11 @@ void btMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject** bodie //printf("solveMultiBodyGroup start\n"); m_tmpMultiBodyConstraints = multiBodyConstraints; m_tmpNumMultiBodyConstraints = numMultiBodyConstraints; - - printf("m_tmpNumMultiBodyConstraints =%d\n",m_tmpNumMultiBodyConstraints ); - printf("solveGroup\n"); + btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher); m_tmpMultiBodyConstraints = 0; m_tmpNumMultiBodyConstraints = 0; -} +} \ No newline at end of file From e721a9cdf535c934b9c6ce08a5246fd52828768c Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Fri, 1 Jun 2018 22:02:17 -0700 Subject: [PATCH 3/7] revert testing changes --- src/BulletDynamics/Featherstone/btMultiBody.cpp | 2 +- src/BulletDynamics/Featherstone/btMultiBody.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 580775060..578814c36 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -37,7 +37,7 @@ bool gJointFeedbackInJointFrame = false; namespace { const btScalar SLEEP_EPSILON = btScalar(0.05); // this is a squared velocity (m^2 s^-2) - const btScalar SLEEP_TIMEOUT = btScalar(0.3); // in seconds + const btScalar SLEEP_TIMEOUT = btScalar(2); // in seconds } namespace { diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index 04cb5ac0a..23ea5533f 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -485,7 +485,7 @@ void addJointTorque(int i, btScalar Q); } void setCompanionId(int id) { - printf("for %p setCompanionId(%d)\n",this, id); + //printf("for %p setCompanionId(%d)\n",this, id); m_companionId = id; } From a450600eb8cfadf6d2801e9cdc73936da3538943 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Fri, 1 Jun 2018 22:17:46 -0700 Subject: [PATCH 4/7] revert gjk test --- .../NarrowPhaseCollision/btGjkPairDetector.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp index 01b73b537..4d29089a4 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp @@ -32,10 +32,10 @@ subject to the following restrictions: //must be above the machine epsilon #ifdef BT_USE_DOUBLE_PRECISION #define REL_ERROR2 btScalar(1.0e-12) - btScalar gGjkEpaPenetrationTolerance = BT_LARGE_FLOAT; + btScalar gGjkEpaPenetrationTolerance = 1.0e-12; #else #define REL_ERROR2 btScalar(1.0e-6) - btScalar gGjkEpaPenetrationTolerance = BT_LARGE_FLOAT; + btScalar gGjkEpaPenetrationTolerance = 0.001; #endif //temp globals, to improve GJK/EPA/penetration calculations From 68ea22bfd03bcf85ef10b0a8e30c60cdbd2b6726 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Fri, 1 Jun 2018 22:50:06 -0700 Subject: [PATCH 5/7] undo git merge conflict mess-up with IK --- .../PhysicsServerCommandProcessor.cpp | 466 +++++++++++------- 1 file changed, 279 insertions(+), 187 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 1bf44c362..a3cb2bdf6 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -6904,7 +6904,8 @@ bool PhysicsServerCommandProcessor::processInitPoseCommand(const struct SharedMe } } - + + btAlignedObjectArray scratch_q; btAlignedObjectArray scratch_m; @@ -8201,201 +8202,295 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con int endEffectorLinkIndex = clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex; - - if (ikHelperPtr && (endEffectorLinkIndexm_multiBody->getNumLinks())) + btAlignedObjectArray startingPositions; + startingPositions.reserve(bodyHandle->m_multiBody->getNumLinks()); + + btVector3 targetPosWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[0], + clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[1], + clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[2]); + + btQuaternion targetOrnWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[0], + clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[1], + clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[2], + clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[3]); + + + btTransform targetBaseCoord; + if (clientCmd.m_updateFlags& IK_HAS_CURRENT_JOINT_POSITIONS) { - const int numDofs = bodyHandle->m_multiBody->getNumDofs(); - int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6; - b3AlignedObjectArray jacobian_linear; - jacobian_linear.resize(3*numDofs); - b3AlignedObjectArray jacobian_angular; - jacobian_angular.resize(3*numDofs); - int jacSize = 0; - - btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody); + targetBaseCoord.setOrigin(targetPosWorld); + targetBaseCoord.setRotation(targetOrnWorld); + } else + { + btTransform targetWorld; + targetWorld.setOrigin(targetPosWorld); + targetWorld.setRotation(targetOrnWorld); + btTransform tr = bodyHandle->m_multiBody->getBaseWorldTransform(); + targetBaseCoord = tr.inverse()*targetWorld; + } + + + { + int DofIndex = 0; + for (int i = 0; i < bodyHandle->m_multiBody->getNumLinks(); ++i) + { + if (bodyHandle->m_multiBody->getLink(i).m_jointType >= 0 && bodyHandle->m_multiBody->getLink(i).m_jointType <= 2) + { + // 0, 1, 2 represent revolute, prismatic, and spherical joint types respectively. Skip the fixed joints. + double curPos = 0; + if (clientCmd.m_updateFlags& IK_HAS_CURRENT_JOINT_POSITIONS) + { + curPos = clientCmd.m_calculateInverseKinematicsArguments.m_currentPositions[DofIndex]; + } else + { + curPos = bodyHandle->m_multiBody->getJointPos(i); + } + startingPositions.push_back(curPos); + DofIndex++; + } + } + } + + int numIterations = 20; + if (clientCmd.m_updateFlags& IK_HAS_MAX_ITERATIONS) + { + numIterations = clientCmd.m_calculateInverseKinematicsArguments.m_maxNumIterations; + } + double residualThreshold = 1e-4; + if (clientCmd.m_updateFlags& IK_HAS_RESIDUAL_THRESHOLD) + { + residualThreshold = clientCmd.m_calculateInverseKinematicsArguments.m_residualThreshold; + } + + btScalar currentDiff = 1e30f; + b3AlignedObjectArray jacobian_linear; + b3AlignedObjectArray jacobian_angular; + btAlignedObjectArray q_current; + btAlignedObjectArray q_new; + btAlignedObjectArray lower_limit; + btAlignedObjectArray upper_limit; + btAlignedObjectArray joint_range; + btAlignedObjectArray rest_pose; + const int numDofs = bodyHandle->m_multiBody->getNumDofs(); + int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6; + btInverseDynamics::vecx nu(numDofs+baseDofs), qdot(numDofs + baseDofs), q(numDofs + baseDofs), joint_force(numDofs + baseDofs); + + for (int i=0;i residualThreshold;i++) + { + BT_PROFILE("InverseKinematics1Step"); + if (ikHelperPtr && (endEffectorLinkIndexm_multiBody->getNumLinks())) + { + + + jacobian_linear.resize(3*numDofs); + jacobian_angular.resize(3*numDofs); + int jacSize = 0; + + btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody); - - btAlignedObjectArray q_current; - q_current.resize(numDofs); - - - - if (tree && ((numDofs+ baseDofs) == tree->numDoFs())) - { - jacSize = jacobian_linear.size(); - // Set jacobian value + q_current.resize(numDofs); + + + if (tree && ((numDofs+ baseDofs) == tree->numDoFs())) + { + btInverseDynamics::vec3 world_origin; + btInverseDynamics::mat33 world_rot; + + jacSize = jacobian_linear.size(); + // Set jacobian value - - - btInverseDynamics::vecx nu(numDofs+baseDofs), qdot(numDofs + baseDofs), q(numDofs + baseDofs), joint_force(numDofs + baseDofs); - int DofIndex = 0; - for (int i = 0; i < bodyHandle->m_multiBody->getNumLinks(); ++i) { - if (bodyHandle->m_multiBody->getLink(i).m_jointType >= 0 && bodyHandle->m_multiBody->getLink(i).m_jointType <= 2) { // 0, 1, 2 represent revolute, prismatic, and spherical joint types respectively. Skip the fixed joints. - q_current[DofIndex] = bodyHandle->m_multiBody->getJointPos(i); - q[DofIndex+baseDofs] = bodyHandle->m_multiBody->getJointPos(i); - qdot[DofIndex+baseDofs] = 0; - nu[DofIndex+baseDofs] = 0; - DofIndex++; - } - } // Set the gravity to correspond to the world gravity - btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity()); - - if (-1 != tree->setGravityInWorldFrame(id_grav) && - -1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force)) - { - tree->calculateJacobians(q); - btInverseDynamics::mat3x jac_t(3, numDofs+ baseDofs); - btInverseDynamics::mat3x jac_r(3,numDofs + baseDofs); - // Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link. - tree->getBodyJacobianTrans(endEffectorLinkIndex+1, &jac_t); - tree->getBodyJacobianRot(endEffectorLinkIndex+1, &jac_r); - for (int i = 0; i < 3; ++i) - { - for (int j = 0; j < numDofs; ++j) - { - jacobian_linear[i*numDofs+j] = jac_t(i,(baseDofs+j)); - jacobian_angular[i*numDofs+j] = jac_r(i,(baseDofs+j)); - } - } - } - - - - btAlignedObjectArray q_new; - q_new.resize(numDofs); - int ikMethod = 0; - if ((clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)&&(clientCmd.m_updateFlags&IK_HAS_NULL_SPACE_VELOCITY)) - { - //Nullspace task only works with DLS now. TODO: add nullspace task to SDLS. - ikMethod = IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE; - } - else if (clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION) - { - if (clientCmd.m_updateFlags & IK_SDLS) + + int DofIndex = 0; + for (int i = 0; i < bodyHandle->m_multiBody->getNumLinks(); ++i) { - ikMethod = IK2_VEL_SDLS_WITH_ORIENTATION; + if (bodyHandle->m_multiBody->getLink(i).m_jointType >= 0 && bodyHandle->m_multiBody->getLink(i).m_jointType <= 2) + { + // 0, 1, 2 represent revolute, prismatic, and spherical joint types respectively. Skip the fixed joints. + + double curPos = startingPositions[DofIndex]; + q_current[DofIndex] = curPos; + q[DofIndex+baseDofs] = curPos; + qdot[DofIndex+baseDofs] = 0; + nu[DofIndex+baseDofs] = 0; + DofIndex++; + } + } // Set the gravity to correspond to the world gravity + btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity()); + + { + BT_PROFILE("calculateInverseDynamics"); + if (-1 != tree->setGravityInWorldFrame(id_grav) && + -1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force)) + { + tree->calculateJacobians(q); + btInverseDynamics::mat3x jac_t(3, numDofs+ baseDofs); + btInverseDynamics::mat3x jac_r(3,numDofs + baseDofs); + // Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link. + tree->getBodyJacobianTrans(endEffectorLinkIndex+1, &jac_t); + tree->getBodyJacobianRot(endEffectorLinkIndex+1, &jac_r); + + //calculatePositionKinematics is already done inside calculateInverseDynamics + tree->getBodyOrigin(endEffectorLinkIndex+1,&world_origin); + tree->getBodyTransform(endEffectorLinkIndex+1,&world_rot); + + for (int i = 0; i < 3; ++i) + { + for (int j = 0; j < numDofs; ++j) + { + jacobian_linear[i*numDofs+j] = jac_t(i,(baseDofs+j)); + jacobian_angular[i*numDofs+j] = jac_r(i,(baseDofs+j)); + } + } + } + } + + + + + q_new.resize(numDofs); + int ikMethod = 0; + if ((clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)&&(clientCmd.m_updateFlags&IK_HAS_NULL_SPACE_VELOCITY)) + { + //Nullspace task only works with DLS now. TODO: add nullspace task to SDLS. + ikMethod = IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE; + } + else if (clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION) + { + if (clientCmd.m_updateFlags & IK_SDLS) + { + ikMethod = IK2_VEL_SDLS_WITH_ORIENTATION; + } + else + { + ikMethod = IK2_VEL_DLS_WITH_ORIENTATION; + } + } + else if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY) + { + //Nullspace task only works with DLS now. TODO: add nullspace task to SDLS. + ikMethod = IK2_VEL_DLS_WITH_NULLSPACE; } else { - ikMethod = IK2_VEL_DLS_WITH_ORIENTATION; + if (clientCmd.m_updateFlags & IK_SDLS) + { + ikMethod = IK2_VEL_SDLS; + } + else + { + ikMethod = IK2_VEL_DLS;; + } } - } - else if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY) - { - //Nullspace task only works with DLS now. TODO: add nullspace task to SDLS. - ikMethod = IK2_VEL_DLS_WITH_NULLSPACE; - } - else - { - if (clientCmd.m_updateFlags & IK_SDLS) - { - ikMethod = IK2_VEL_SDLS; - } - else - { - ikMethod = IK2_VEL_DLS;; - } - } - if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY) - { - btAlignedObjectArray lower_limit; - btAlignedObjectArray upper_limit; - btAlignedObjectArray joint_range; - btAlignedObjectArray rest_pose; - lower_limit.resize(numDofs); - upper_limit.resize(numDofs); - joint_range.resize(numDofs); - rest_pose.resize(numDofs); - for (int i = 0; i < numDofs; ++i) + if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY) { - lower_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_lowerLimit[i]; - upper_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_upperLimit[i]; - joint_range[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointRange[i]; - rest_pose[i] = clientCmd.m_calculateInverseKinematicsArguments.m_restPose[i]; + + lower_limit.resize(numDofs); + upper_limit.resize(numDofs); + joint_range.resize(numDofs); + rest_pose.resize(numDofs); + for (int i = 0; i < numDofs; ++i) + { + lower_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_lowerLimit[i]; + upper_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_upperLimit[i]; + joint_range[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointRange[i]; + rest_pose[i] = clientCmd.m_calculateInverseKinematicsArguments.m_restPose[i]; + } + { + BT_PROFILE("computeNullspaceVel"); + ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]); + } } - ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]); - } - - btTransform endEffectorTransformWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform * bodyHandle->m_linkLocalInertialFrames[endEffectorLinkIndex].inverse(); + + + //btTransform endEffectorTransformWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform * bodyHandle->m_linkLocalInertialFrames[endEffectorLinkIndex].inverse(); - btVector3DoubleData endEffectorWorldPosition; - btQuaternionDoubleData endEffectorWorldOrientation; + btVector3DoubleData endEffectorWorldPosition; + btQuaternionDoubleData endEffectorWorldOrientation; + + //get the position from the inverse dynamics (based on q) instead of endEffectorTransformWorld + btVector3 endEffectorPosWorldOrg = world_origin; + btQuaternion endEffectorOriWorldOrg; + world_rot.getRotation(endEffectorOriWorldOrg); + + btTransform endEffectorBaseCoord; + endEffectorBaseCoord.setOrigin(endEffectorPosWorldOrg); + endEffectorBaseCoord.setRotation(endEffectorOriWorldOrg); + + //don't need the next two lines + //btTransform linkInertiaInv = bodyHandle->m_linkLocalInertialFrames[endEffectorLinkIndex].inverse(); + //endEffectorBaseCoord = endEffectorBaseCoord * linkInertiaInv; + + //btTransform tr = bodyHandle->m_multiBody->getBaseWorldTransform(); + //endEffectorBaseCoord = tr.inverse()*endEffectorTransformWorld; + //endEffectorBaseCoord = tr.inverse()*endEffectorTransformWorld; + + btQuaternion endEffectorOriBaseCoord= endEffectorBaseCoord.getRotation(); + + //btVector4 endEffectorOri(endEffectorOriBaseCoord.x(), endEffectorOriBaseCoord.y(), endEffectorOriBaseCoord.z(), endEffectorOriBaseCoord.w()); - btVector3 endEffectorPosWorldOrg = endEffectorTransformWorld.getOrigin(); - btQuaternion endEffectorOriWorldOrg = endEffectorTransformWorld.getRotation(); - btTransform endEffectorWorld; - endEffectorWorld.setOrigin(endEffectorPosWorldOrg); - endEffectorWorld.setRotation(endEffectorOriWorldOrg); - - btTransform tr = bodyHandle->m_multiBody->getBaseWorldTransform(); - - btTransform endEffectorBaseCoord = tr.inverse()*endEffectorWorld; - - btQuaternion endEffectorOriBaseCoord= endEffectorBaseCoord.getRotation(); - - btVector4 endEffectorOri(endEffectorOriBaseCoord.x(), endEffectorOriBaseCoord.y(), endEffectorOriBaseCoord.z(), endEffectorOriBaseCoord.w()); + endEffectorBaseCoord.getOrigin().serializeDouble(endEffectorWorldPosition); + endEffectorBaseCoord.getRotation().serializeDouble(endEffectorWorldOrientation); - endEffectorBaseCoord.getOrigin().serializeDouble(endEffectorWorldPosition); - endEffectorBaseCoord.getRotation().serializeDouble(endEffectorWorldOrientation); - - btVector3 targetPosWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[0], - clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[1], - clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[2]); + + + + //diff + currentDiff = (endEffectorBaseCoord.getOrigin()-targetBaseCoord.getOrigin()).length(); - btQuaternion targetOrnWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[0], - clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[1], - clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[2], - clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[3]); - btTransform targetWorld; - targetWorld.setOrigin(targetPosWorld); - targetWorld.setRotation(targetOrnWorld); - btTransform targetBaseCoord; - targetBaseCoord = tr.inverse()*targetWorld; - btVector3DoubleData targetPosBaseCoord; - btQuaternionDoubleData targetOrnBaseCoord; - targetBaseCoord.getOrigin().serializeDouble(targetPosBaseCoord); - targetBaseCoord.getRotation().serializeDouble(targetOrnBaseCoord); + btVector3DoubleData targetPosBaseCoord; + btQuaternionDoubleData targetOrnBaseCoord; + targetBaseCoord.getOrigin().serializeDouble(targetPosBaseCoord); + targetBaseCoord.getRotation().serializeDouble(targetOrnBaseCoord); - // Set joint damping coefficents. A small default - // damping constant is added to prevent singularity - // with pseudo inverse. The user can set joint damping - // coefficients differently for each joint. The larger - // the damping coefficient is, the less we rely on - // this joint to achieve the IK target. - btAlignedObjectArray joint_damping; - joint_damping.resize(numDofs,0.5); - if (clientCmd.m_updateFlags& IK_HAS_JOINT_DAMPING) - { - for (int i = 0; i < numDofs; ++i) + // Set joint damping coefficents. A small default + // damping constant is added to prevent singularity + // with pseudo inverse. The user can set joint damping + // coefficients differently for each joint. The larger + // the damping coefficient is, the less we rely on + // this joint to achieve the IK target. + btAlignedObjectArray joint_damping; + joint_damping.resize(numDofs,0.5); + if (clientCmd.m_updateFlags& IK_HAS_JOINT_DAMPING) { - joint_damping[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointDamping[i]; + for (int i = 0; i < numDofs; ++i) + { + joint_damping[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointDamping[i]; + } + } + ikHelperPtr->setDampingCoeff(numDofs, &joint_damping[0]); + + double targetDampCoeff[6]={1.0,1.0,1.0,1.0,1.0,1.0}; + + { + BT_PROFILE("computeIK"); + ikHelperPtr->computeIK(targetPosBaseCoord.m_floats, targetOrnBaseCoord.m_floats, + endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats, + &q_current[0], + numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex, + &q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2, targetDampCoeff); + } + serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId =clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId; + for (int i=0;isetDampingCoeff(numDofs, &joint_damping[0]); - - double targetDampCoeff[6] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 }; - ikHelperPtr->computeIK(targetPosBaseCoord.m_floats, targetOrnBaseCoord.m_floats, - endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats, - &q_current[0], - numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex, - &q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2, targetDampCoeff); - - serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId =clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId; - for (int i=0;im_multiBody) { - //todo: don't activate 'static' colliders/objects - if (!(multiCol->m_multiBody->getNumLinks()==0 && multiCol->m_multiBody->getBaseMass()==0)) - { - m_data->m_prevCanSleep = multiCol->m_multiBody->getCanSleep(); - multiCol->m_multiBody->setCanSleep(false); - btVector3 pivotInA = multiCol->m_multiBody->worldPosToLocal(multiCol->m_link, pickPos); + m_data->m_prevCanSleep = multiCol->m_multiBody->getCanSleep(); + multiCol->m_multiBody->setCanSleep(false); - btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(multiCol->m_multiBody,multiCol->m_link,0,pivotInA,pickPos); - //if you add too much energy to the system, causing high angular velocities, simulation 'explodes' - //see also http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=949 - //so we try to avoid it by clamping the maximum impulse (force) that the mouse pick can apply - //it is not satisfying, hopefully we find a better solution (higher order integrator, using joint friction using a zero-velocity target motor with limited force etc?) - btScalar scaling=1; - p2p->setMaxAppliedImpulse(2*scaling); + btVector3 pivotInA = multiCol->m_multiBody->worldPosToLocal(multiCol->m_link, pickPos); - btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; - world->addMultiBodyConstraint(p2p); - m_data->m_pickingMultiBodyPoint2Point =p2p; - } + btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(multiCol->m_multiBody,multiCol->m_link,0,pivotInA,pickPos); + //if you add too much energy to the system, causing high angular velocities, simulation 'explodes' + //see also http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=949 + //so we try to avoid it by clamping the maximum impulse (force) that the mouse pick can apply + //it is not satisfying, hopefully we find a better solution (higher order integrator, using joint friction using a zero-velocity target motor with limited force etc?) + btScalar scaling=10; + p2p->setMaxAppliedImpulse(2*scaling); + + btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; + world->addMultiBodyConstraint(p2p); + m_data->m_pickingMultiBodyPoint2Point =p2p; } } @@ -9812,4 +9904,4 @@ const btQuaternion& PhysicsServerCommandProcessor::getVRTeleportOrientation() co void PhysicsServerCommandProcessor::setVRTeleportOrientation(const btQuaternion& vrTeleportOrn) { gVRTeleportOrn = vrTeleportOrn; -} +} \ No newline at end of file From f75e22e37d32cd24a2ccc5678b3e3341542d97df Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 1 Jun 2018 22:54:43 -0700 Subject: [PATCH 6/7] bump up PyBullet version to 2.0.3 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 90c0875fe..570c95d9a 100644 --- a/setup.py +++ b/setup.py @@ -450,7 +450,7 @@ print("-----") setup( name = 'pybullet', - version='2.0.2', + version='2.0.3', description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.', url='https://github.com/bulletphysics/bullet3', From 39c9ffa4c358b387868394695f52c48e5850a6f2 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 1 Jun 2018 23:25:47 -0700 Subject: [PATCH 7/7] PyBullet IK: backward compatible changes related to joint damping --- examples/pybullet/pybullet.c | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index d6954e0b8..e6c367935 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -8166,14 +8166,16 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, if (szJointDamping > 0) { - if (szJointDamping != dofCount) + if (szJointDamping < dofCount) { - PyErr_SetString(SpamError, - "calculateInverseKinematics the size of input joint damping values is unequal to the number of degrees of freedom."); - return NULL; + printf("calculateInverseKinematics: the size of input joint damping values should be equal to the number of degrees of freedom, not using joint damping."); } else { + if (szJointDamping != dofCount) + { + printf("calculateInverseKinematics: the size of input joint damping values should be equal to the number of degrees of freedom, ignoring the additonal values."); + } int szInBytes = sizeof(double) * szJointDamping; int i; jointDamping = (double*)malloc(szInBytes);