diff --git a/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp b/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp
index ef3ebf6b8..e432cf438 100644
--- a/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp
+++ b/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp
@@ -27,7 +27,9 @@ int btMultiBodyTreeCreator::createFromBtMultiBody(const btMultiBody *btmb, const
} else {
link.joint_type = FLOATING;
}
- btTransform transform(btmb->getBaseWorldTransform());
+ btTransform transform=(btmb->getBaseWorldTransform());
+ //compute inverse dynamics in body-fixed frame
+ transform.setIdentity();
link.parent_r_parent_body_ref(0) = transform.getOrigin()[0];
link.parent_r_parent_body_ref(1) = transform.getOrigin()[1];
diff --git a/data/pole.urdf b/data/pole.urdf
new file mode 100755
index 000000000..5932f221d
--- /dev/null
+++ b/data/pole.urdf
@@ -0,0 +1,140 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/examples/SharedMemory/IKTrajectoryHelper.cpp b/examples/SharedMemory/IKTrajectoryHelper.cpp
index 0145251bb..ac6faad9f 100644
--- a/examples/SharedMemory/IKTrajectoryHelper.cpp
+++ b/examples/SharedMemory/IKTrajectoryHelper.cpp
@@ -47,7 +47,8 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
const double* q_current, int numQ,int endEffectorIndex,
double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, const double dampIk[6])
{
- bool useAngularPart = (ikMethod==IK2_VEL_DLS_WITH_ORIENTATION || ikMethod==IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE) ? true : false;
+ bool useAngularPart = (ikMethod==IK2_VEL_DLS_WITH_ORIENTATION || ikMethod==IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE
+ || ikMethod==IK2_VEL_SDLS_WITH_ORIENTATION) ? true : false;
Jacobian ikJacobian(useAngularPart,numQ);
@@ -136,8 +137,8 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
ikJacobian.CalcDeltaThetasTranspose(); // Jacobian transpose method
break;
case IK2_DLS:
- case IK2_VEL_DLS:
case IK2_VEL_DLS_WITH_ORIENTATION:
+ case IK2_VEL_DLS:
//ikJacobian.CalcDeltaThetasDLS(); // Damped least squares method
assert(m_data->m_dampingCoeff.GetLength()==numQ);
ikJacobian.CalcDeltaThetasDLS2(m_data->m_dampingCoeff);
@@ -154,6 +155,8 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
ikJacobian.CalcDeltaThetasPseudoinverse(); // Pure pseudoinverse method
break;
case IK2_SDLS:
+ case IK2_VEL_SDLS:
+ case IK2_VEL_SDLS_WITH_ORIENTATION:
ikJacobian.CalcDeltaThetasSDLS(); // Selectively damped least squares method
break;
default:
diff --git a/examples/SharedMemory/IKTrajectoryHelper.h b/examples/SharedMemory/IKTrajectoryHelper.h
index 90f2dc409..10d4e1976 100644
--- a/examples/SharedMemory/IKTrajectoryHelper.h
+++ b/examples/SharedMemory/IKTrajectoryHelper.h
@@ -12,6 +12,8 @@ enum IK2_Method
IK2_VEL_DLS_WITH_ORIENTATION,
IK2_VEL_DLS_WITH_NULLSPACE,
IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE,
+ IK2_VEL_SDLS,
+ IK2_VEL_SDLS_WITH_ORIENTATION,
};
diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp
index 06c8948e5..7b685974f 100644
--- a/examples/SharedMemory/PhysicsClientC_API.cpp
+++ b/examples/SharedMemory/PhysicsClientC_API.cpp
@@ -2128,7 +2128,7 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetAngularDamping(b3SharedMemoryCommandHan
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
- command->m_changeDynamicsInfoArgs.m_linearDamping = angularDamping;
+ command->m_changeDynamicsInfoArgs.m_angularDamping = angularDamping;
command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING;
return 0;
}
@@ -2292,10 +2292,47 @@ B3_SHARED_API int b3InitChangeUserConstraintSetERP(b3SharedMemoryCommandHandle c
}
+B3_SHARED_API b3SharedMemoryCommandHandle b3InitGetUserConstraintStateCommand(b3PhysicsClientHandle physClient, int constraintUniqueId)
+{
+ PhysicsClient* cl = (PhysicsClient*)physClient;
+ b3Assert(cl);
+ b3Assert(cl->canSubmitCommand());
+ struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+ b3Assert(command);
+
+ command->m_type = CMD_USER_CONSTRAINT;
+ command->m_updateFlags = USER_CONSTRAINT_REQUEST_STATE;
+ command->m_userConstraintArguments.m_userConstraintUniqueId = constraintUniqueId;
+ return (b3SharedMemoryCommandHandle)command;
+}
+
+B3_SHARED_API int b3GetStatusUserConstraintState(b3SharedMemoryStatusHandle statusHandle, struct b3UserConstraintState* constraintState)
+{
+ const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
+ if (status)
+ {
+ btAssert(status->m_type == CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED);
+ if (status && status->m_type == CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED)
+ {
+ int i = 0;
+ constraintState->m_numDofs = status->m_userConstraintStateResultArgs.m_numDofs;
+ for (i = 0; i < constraintState->m_numDofs; i++)
+ {
+ constraintState->m_appliedConstraintForces[i] = status->m_userConstraintStateResultArgs.m_appliedConstraintForces[i];
+ }
+ for (; i < 6; i++)
+ {
+ constraintState->m_appliedConstraintForces[i] = 0;
+ }
+ return 1;
+ }
+ }
+ return 0;
+}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId)
{
- PhysicsClient* cl = (PhysicsClient* ) physClient;
+ PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
@@ -2341,6 +2378,7 @@ B3_SHARED_API int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle s
}
+
B3_SHARED_API b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
double rayFromWorldY, double rayFromWorldZ,
double rayToWorldX, double rayToWorldY, double rayToWorldZ)
@@ -3612,6 +3650,12 @@ B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMem
command->m_calculateInverseKinematicsArguments.m_targetPosition[0] = targetPosition[0];
command->m_calculateInverseKinematicsArguments.m_targetPosition[1] = targetPosition[1];
command->m_calculateInverseKinematicsArguments.m_targetPosition[2] = targetPosition[2];
+
+
+ command->m_calculateInverseKinematicsArguments.m_targetOrientation[0] = 0;
+ command->m_calculateInverseKinematicsArguments.m_targetOrientation[1] = 0;
+ command->m_calculateInverseKinematicsArguments.m_targetOrientation[2] = 0;
+ command->m_calculateInverseKinematicsArguments.m_targetOrientation[3] = 1;
}
@@ -3695,6 +3739,14 @@ B3_SHARED_API void b3CalculateInverseKinematicsSetJointDamping(b3SharedMemoryCom
}
}
+B3_SHARED_API void b3CalculateInverseKinematicsSelectSolver(b3SharedMemoryCommandHandle commandHandle, int solver)
+{
+ struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+ b3Assert(command);
+ b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
+ command->m_updateFlags |= solver;
+}
+
B3_SHARED_API int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* dofCount,
diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h
index bfd67ca22..f3bef30e5 100644
--- a/examples/SharedMemory/PhysicsClientC_API.h
+++ b/examples/SharedMemory/PhysicsClientC_API.h
@@ -143,6 +143,11 @@ B3_SHARED_API int b3InitChangeUserConstraintSetERP(b3SharedMemoryCommandHandle c
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId);
B3_SHARED_API int b3GetNumUserConstraints(b3PhysicsClientHandle physClient);
+
+B3_SHARED_API b3SharedMemoryCommandHandle b3InitGetUserConstraintStateCommand(b3PhysicsClientHandle physClient, int constraintUniqueId);
+B3_SHARED_API int b3GetStatusUserConstraintState(b3SharedMemoryStatusHandle statusHandle, struct b3UserConstraintState* constraintState);
+
+
B3_SHARED_API int b3GetUserConstraintInfo(b3PhysicsClientHandle physClient, int constraintUniqueId, struct b3UserConstraint* info);
/// return the user constraint id, given the index in range [0 , b3GetNumUserConstraints() )
B3_SHARED_API int b3GetUserConstraintId(b3PhysicsClientHandle physClient, int serialIndex);
@@ -335,6 +340,7 @@ B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(
B3_SHARED_API void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[/*3*/], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose);
B3_SHARED_API void b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[/*3*/], const double targetOrientation[/*4*/], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose);
B3_SHARED_API void b3CalculateInverseKinematicsSetJointDamping(b3SharedMemoryCommandHandle commandHandle, int numDof, const double* jointDampingCoeff);
+B3_SHARED_API void b3CalculateInverseKinematicsSelectSolver(b3SharedMemoryCommandHandle commandHandle, int solver);
B3_SHARED_API int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* dofCount,
diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp
index 2f2b8aa1f..771d1ba40 100644
--- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp
+++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp
@@ -601,6 +601,10 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
break;
}
+ case CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED:
+ {
+ break;
+ }
case CMD_USER_CONSTRAINT_INFO_COMPLETED:
{
B3_PROFILE("CMD_USER_CONSTRAINT_INFO_COMPLETED");
@@ -649,6 +653,22 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
{
userConstraintPtr->m_maxAppliedForce = serverConstraint->m_maxAppliedForce;
}
+ if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO)
+ {
+ userConstraintPtr->m_gearRatio = serverConstraint->m_gearRatio;
+ }
+ if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET)
+ {
+ userConstraintPtr->m_relativePositionTarget = serverConstraint->m_relativePositionTarget;
+ }
+ if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_ERP)
+ {
+ userConstraintPtr->m_erp = serverConstraint->m_erp;
+ }
+ if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK)
+ {
+ userConstraintPtr->m_gearAuxLink = serverConstraint->m_gearAuxLink;
+ }
}
break;
}
diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp
index 9ad84404e..692024672 100644
--- a/examples/SharedMemory/PhysicsDirect.cpp
+++ b/examples/SharedMemory/PhysicsDirect.cpp
@@ -780,10 +780,29 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
{
userConstraintPtr->m_maxAppliedForce = serverConstraint->m_maxAppliedForce;
}
+ if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO)
+ {
+ userConstraintPtr->m_gearRatio = serverConstraint->m_gearRatio;
+ }
+ if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET)
+ {
+ userConstraintPtr->m_relativePositionTarget = serverConstraint->m_relativePositionTarget;
+ }
+ if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_ERP)
+ {
+ userConstraintPtr->m_erp = serverConstraint->m_erp;
+ }
+ if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK)
+ {
+ userConstraintPtr->m_gearAuxLink = serverConstraint->m_gearAuxLink;
+ }
}
break;
}
-
+ case CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED:
+ {
+ break;
+ }
case CMD_SYNC_BODY_INFO_COMPLETED:
case CMD_MJCF_LOADING_COMPLETED:
case CMD_SDF_LOADING_COMPLETED:
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index 6b624a3dd..f0de991dc 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -5629,13 +5629,22 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
double rollingFriction = clientCmd.m_changeDynamicsInfoArgs.m_rollingFriction;
double restitution = clientCmd.m_changeDynamicsInfoArgs.m_restitution;
btAssert(bodyUniqueId >= 0);
- btAssert(linkIndex >= -1);
InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
if (body && body->m_multiBody)
{
btMultiBody* mb = body->m_multiBody;
+
+ if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING)
+ {
+ mb->setLinearDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping);
+ }
+ if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING)
+ {
+ mb->setAngularDamping(clientCmd.m_changeDynamicsInfoArgs.m_angularDamping);
+ }
+
if (linkIndex == -1)
{
if (mb->getBaseCollider())
@@ -5644,14 +5653,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
mb->getBaseCollider()->setRestitution(restitution);
}
- if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING)
- {
- mb->setLinearDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping);
- }
- if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING)
- {
- mb->setLinearDamping(clientCmd.m_changeDynamicsInfoArgs.m_angularDamping);
- }
+
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING)
{
@@ -7172,6 +7174,29 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_type = CMD_USER_CONSTRAINT_FAILED;
hasStatus = true;
+ if (clientCmd.m_updateFlags & USER_CONSTRAINT_REQUEST_STATE)
+ {
+ int constraintUid = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId;
+ InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(constraintUid);
+ if (userConstraintPtr)
+ {
+ serverCmd.m_userConstraintStateResultArgs.m_numDofs = 0;
+ for (int i = 0; i < 6; i++)
+ {
+ serverCmd.m_userConstraintStateResultArgs.m_appliedConstraintForces[i] = 0;
+ }
+ if (userConstraintPtr->m_mbConstraint)
+ {
+ serverCmd.m_userConstraintStateResultArgs.m_numDofs = userConstraintPtr->m_mbConstraint->getNumRows();
+ for (int i = 0; i < userConstraintPtr->m_mbConstraint->getNumRows(); i++)
+ {
+ serverCmd.m_userConstraintStateResultArgs.m_appliedConstraintForces[i] = userConstraintPtr->m_mbConstraint->getAppliedImpulse(i) / m_data->m_dynamicsWorld->getSolverInfo().m_timeStep;
+ }
+ serverCmd.m_type = CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED;
+ }
+ }
+
+ };
if (clientCmd.m_updateFlags & USER_CONSTRAINT_REQUEST_INFO)
{
int userConstraintUidChange = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId;
@@ -7179,6 +7204,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (userConstraintPtr)
{
serverCmd.m_userConstraintResultArgs = userConstraintPtr->m_userConstraintData;
+
serverCmd.m_type = CMD_USER_CONSTRAINT_INFO_COMPLETED;
}
}
@@ -7544,19 +7570,23 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO)
{
userConstraintPtr->m_mbConstraint->setGearRatio(clientCmd.m_userConstraintArguments.m_gearRatio);
+ userConstraintPtr->m_userConstraintData.m_gearRatio = clientCmd.m_userConstraintArguments.m_gearRatio;
}
if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET)
{
userConstraintPtr->m_mbConstraint->setRelativePositionTarget(clientCmd.m_userConstraintArguments.m_relativePositionTarget);
+ userConstraintPtr->m_userConstraintData.m_relativePositionTarget = clientCmd.m_userConstraintArguments.m_relativePositionTarget;
}
if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_ERP)
{
userConstraintPtr->m_mbConstraint->setErp(clientCmd.m_userConstraintArguments.m_erp);
+ userConstraintPtr->m_userConstraintData.m_erp = clientCmd.m_userConstraintArguments.m_erp;
}
if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK)
{
userConstraintPtr->m_mbConstraint->setGearAuxLink(clientCmd.m_userConstraintArguments.m_gearAuxLink);
+ userConstraintPtr->m_userConstraintData.m_gearAuxLink = clientCmd.m_userConstraintArguments.m_gearAuxLink;
}
}
@@ -7644,7 +7674,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (ikHelperPtr && (endEffectorLinkIndexm_multiBody->getNumLinks()))
{
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;
@@ -7658,11 +7688,13 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
btAlignedObjectArray q_current;
q_current.resize(numDofs);
- if (tree && (numDofs == tree->numDoFs()))
+
+
+ if (tree && ((numDofs+ baseDofs) == tree->numDoFs()))
{
jacSize = jacobian_linear.size();
// Set jacobian value
- int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
+
btInverseDynamics::vecx nu(numDofs+baseDofs), qdot(numDofs + baseDofs), q(numDofs + baseDofs), joint_force(numDofs + baseDofs);
@@ -7682,8 +7714,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force))
{
tree->calculateJacobians(q);
- btInverseDynamics::mat3x jac_t(3, numDofs);
- btInverseDynamics::mat3x jac_r(3,numDofs);
+ 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);
@@ -7691,8 +7723,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
for (int j = 0; j < numDofs; ++j)
{
- jacobian_linear[i*numDofs+j] = jac_t(i,j);
- jacobian_angular[i*numDofs+j] = jac_r(i,j);
+ jacobian_linear[i*numDofs+j] = jac_t(i,(baseDofs+j));
+ jacobian_angular[i*numDofs+j] = jac_r(i,(baseDofs+j));
}
}
}
@@ -7704,19 +7736,35 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
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)
{
- ikMethod = IK2_VEL_DLS_WITH_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;
+ if (clientCmd.m_updateFlags & IK_SDLS)
+ {
+ ikMethod = IK2_VEL_SDLS;
+ }
+ else
+ {
+ ikMethod = IK2_VEL_DLS;;
+ }
}
if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY)
@@ -7742,15 +7790,44 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
btTransform endEffectorTransformWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform * bodyHandle->m_linkLocalInertialFrames[endEffectorLinkIndex].inverse();
btVector3DoubleData endEffectorWorldPosition;
- btVector3DoubleData endEffectorWorldOrientation;
+ btQuaternionDoubleData endEffectorWorldOrientation;
- btVector3 endEffectorPosWorld = endEffectorTransformWorld.getOrigin();
- btQuaternion endEffectorOriWorld = endEffectorTransformWorld.getRotation();
- btVector4 endEffectorOri(endEffectorOriWorld.x(),endEffectorOriWorld.y(),endEffectorOriWorld.z(),endEffectorOriWorld.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());
- endEffectorPosWorld.serializeDouble(endEffectorWorldPosition);
- endEffectorOri.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]);
+
+ 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);
+
// Set joint damping coefficents. A small default
// damping constant is added to prevent singularity
// with pseudo inverse. The user can set joint damping
@@ -7769,7 +7846,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
ikHelperPtr->setDampingCoeff(numDofs, &joint_damping[0]);
double targetDampCoeff[6] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 };
- ikHelperPtr->computeIK(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition, clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation,
+ ikHelperPtr->computeIK(targetPosBaseCoord.m_floats, targetOrnBaseCoord.m_floats,
endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats,
&q_current[0],
numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex,
diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h
index 5455ca9e3..33aac5e47 100644
--- a/examples/SharedMemory/SharedMemoryCommands.h
+++ b/examples/SharedMemory/SharedMemoryCommands.h
@@ -633,15 +633,6 @@ struct CalculateMassMatrixResultArgs
int m_dofCount;
};
-enum EnumCalculateInverseKinematicsFlags
-{
- IK_HAS_TARGET_POSITION=1,
- IK_HAS_TARGET_ORIENTATION=2,
- IK_HAS_NULL_SPACE_VELOCITY=4,
- IK_HAS_JOINT_DAMPING=8,
- //IK_HAS_CURRENT_JOINT_POSITIONS=16,//not used yet
-};
-
struct CalculateInverseKinematicsArgs
{
int m_bodyUniqueId;
@@ -676,7 +667,7 @@ enum EnumUserConstraintFlags
USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK=256,
USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET=512,
USER_CONSTRAINT_CHANGE_ERP=1024,
-
+ USER_CONSTRAINT_REQUEST_STATE=2048,
};
enum EnumBodyChangeFlags
@@ -1039,6 +1030,7 @@ struct SharedMemoryStatus
struct SendVisualShapeDataArgs m_sendVisualShapeArgs;
struct UserDebugDrawResultArgs m_userDebugDrawArgs;
struct b3UserConstraint m_userConstraintResultArgs;
+ struct b3UserConstraintState m_userConstraintStateResultArgs;
struct SendVREvents m_sendVREvents;
struct SendKeyboardEvents m_sendKeyboardEvents;
struct SendRaycastHits m_raycastHits;
diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h
index b3b223ae1..03f3d79d8 100644
--- a/examples/SharedMemory/SharedMemoryPublic.h
+++ b/examples/SharedMemory/SharedMemoryPublic.h
@@ -5,7 +5,8 @@
///increase the SHARED_MEMORY_MAGIC_NUMBER whenever incompatible changes are made in the structures
///my convention is year/month/day/rev
-#define SHARED_MEMORY_MAGIC_NUMBER 201710050
+#define SHARED_MEMORY_MAGIC_NUMBER 201710180
+//#define SHARED_MEMORY_MAGIC_NUMBER 201710050
//#define SHARED_MEMORY_MAGIC_NUMBER 201708270
//#define SHARED_MEMORY_MAGIC_NUMBER 201707140
//#define SHARED_MEMORY_MAGIC_NUMBER 201706015
@@ -143,6 +144,7 @@ enum EnumSharedMemoryServerStatus
CMD_USER_DEBUG_DRAW_FAILED,
CMD_USER_CONSTRAINT_COMPLETED,
CMD_USER_CONSTRAINT_INFO_COMPLETED,
+ CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED,
CMD_REMOVE_USER_CONSTRAINT_COMPLETED,
CMD_CHANGE_USER_CONSTRAINT_COMPLETED,
CMD_REMOVE_USER_CONSTRAINT_FAILED,
@@ -253,7 +255,6 @@ struct b3UserConstraint
int m_gearAuxLink;
double m_relativePositionTarget;
double m_erp;
-
};
struct b3BodyInfo
@@ -331,6 +332,12 @@ struct b3OpenGLVisualizerCameraInfo
float m_target[3];
};
+struct b3UserConstraintState
+{
+ double m_appliedConstraintForces[6];
+ int m_numDofs;
+};
+
enum b3VREventType
{
VR_CONTROLLER_MOVE_EVENT=1,
@@ -565,6 +572,18 @@ enum EnumRenderer
//ER_FIRE_RAYS=(1<<18),
};
+///flags to pick the IK solver and other options
+enum EnumCalculateInverseKinematicsFlags
+{
+ IK_DLS=0,
+ IK_SDLS=1, //TODO: can add other IK solvers
+ IK_HAS_TARGET_POSITION=16,
+ IK_HAS_TARGET_ORIENTATION=32,
+ IK_HAS_NULL_SPACE_VELOCITY=64,
+ IK_HAS_JOINT_DAMPING=128,
+ //IK_HAS_CURRENT_JOINT_POSITIONS=256,//not used yet
+};
+
enum b3ConfigureDebugVisualizerEnum
{
COV_ENABLE_GUI=1,
diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp
index c082bef31..805f21625 100644
--- a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp
+++ b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp
@@ -462,7 +462,8 @@ void Jacobian::CalcDeltaThetasSDLS()
// Calculate response vector dTheta that is the SDLS solution.
// Delta target values are the dS values
int nRows = J.GetNumRows();
- int numEndEffectors = m_tree->GetNumEffector(); // Equals the number of rows of J divided by three
+ // TODO: Modify it to work with multiple end effectors.
+ int numEndEffectors = 1;
int nCols = J.GetNumColumns();
dTheta.SetZero();
diff --git a/examples/ThirdPartyLibs/clsocket/src/StatTimer.h b/examples/ThirdPartyLibs/clsocket/src/StatTimer.h
index 2110add9f..2e049432e 100644
--- a/examples/ThirdPartyLibs/clsocket/src/StatTimer.h
+++ b/examples/ThirdPartyLibs/clsocket/src/StatTimer.h
@@ -46,7 +46,7 @@
#include
#if WIN32
- #include
+ #include
#include
#endif
diff --git a/examples/pybullet/examples/inverse_dynamics.py b/examples/pybullet/examples/inverse_dynamics.py
index e6af3011b..8b86cc3b9 100644
--- a/examples/pybullet/examples/inverse_dynamics.py
+++ b/examples/pybullet/examples/inverse_dynamics.py
@@ -143,7 +143,7 @@ if plot:
ax_tor.set_ylim(-20., 20.)
ax_tor.legend()
- plt.pause(0.01)
+ plt.pause(0.01)
while (1):
diff --git a/examples/pybullet/examples/inverse_kinematics.py b/examples/pybullet/examples/inverse_kinematics.py
index 9aa32afed..928389542 100644
--- a/examples/pybullet/examples/inverse_kinematics.py
+++ b/examples/pybullet/examples/inverse_kinematics.py
@@ -39,8 +39,9 @@ useNullSpace = 0
useOrientation = 1
#If we set useSimulation=0, it sets the arm pose to be the IK result directly without using dynamic control.
#This can be used to test the IK result accuracy.
-useSimulation = 0
+useSimulation = 1
useRealTimeSimulation = 1
+ikSolver = 0
p.setRealTimeSimulation(useRealTimeSimulation)
#trailDuration is duration (in seconds) after debug lines will be removed automatically
#use 0 for no-removal
@@ -68,9 +69,9 @@ while 1:
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,lowerLimits=ll, upperLimits=ul, jointRanges=jr, restPoses=rp)
else:
if (useOrientation==1):
- jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,jointDamping=jd)
+ jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,jointDamping=jd,solver=ikSolver)
else:
- jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos)
+ jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,solver=ikSolver)
if (useSimulation):
for i in range (numJoints):
diff --git a/examples/pybullet/examples/inverse_kinematics_husky_kuka.py b/examples/pybullet/examples/inverse_kinematics_husky_kuka.py
new file mode 100644
index 000000000..eb28102b8
--- /dev/null
+++ b/examples/pybullet/examples/inverse_kinematics_husky_kuka.py
@@ -0,0 +1,169 @@
+import pybullet as p
+import time
+import math
+from datetime import datetime
+from datetime import datetime
+
+clid = p.connect(p.SHARED_MEMORY)
+if (clid<0):
+ p.connect(p.GUI)
+p.loadURDF("plane.urdf",[0,0,-0.3])
+husky = p.loadURDF("husky/husky.urdf",[0.290388,0.329902,-0.310270],[0.002328,-0.000984,0.996491,0.083659])
+for i in range (p.getNumJoints(husky)):
+ print(p.getJointInfo(husky,i))
+kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf", 0.193749,0.345564,0.120208,0.002327,-0.000988,0.996491,0.083659)
+ob = kukaId
+jointPositions=[ 3.559609, 0.411182, 0.862129, 1.744441, 0.077299, -1.129685, 0.006001 ]
+for jointIndex in range (p.getNumJoints(ob)):
+ p.resetJointState(ob,jointIndex,jointPositions[jointIndex])
+
+
+#put kuka on top of husky
+
+cid = p.createConstraint(husky,-1,kukaId,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0.,0.,-.5],[0,0,0,1])
+
+
+baseorn = p.getQuaternionFromEuler([3.1415,0,0.3])
+baseorn = [0,0,0,1]
+#[0, 0, 0.707, 0.707]
+
+#p.resetBasePositionAndOrientation(kukaId,[0,0,0],baseorn)#[0,0,0,1])
+kukaEndEffectorIndex = 6
+numJoints = p.getNumJoints(kukaId)
+if (numJoints!=7):
+ exit()
+
+
+#lower limits for null space
+ll=[-.967,-2 ,-2.96,0.19,-2.96,-2.09,-3.05]
+#upper limits for null space
+ul=[.967,2 ,2.96,2.29,2.96,2.09,3.05]
+#joint ranges for null space
+jr=[5.8,4,5.8,4,5.8,4,6]
+#restposes for null space
+rp=[0,0,0,0.5*math.pi,0,-math.pi*0.5*0.66,0]
+#joint damping coefficents
+jd=[0.1,0.1,0.1,0.1,0.1,0.1,0.1]
+
+for i in range (numJoints):
+ p.resetJointState(kukaId,i,rp[i])
+
+p.setGravity(0,0,-10)
+t=0.
+prevPose=[0,0,0]
+prevPose1=[0,0,0]
+hasPrevPose = 0
+useNullSpace = 0
+
+useOrientation =0
+#If we set useSimulation=0, it sets the arm pose to be the IK result directly without using dynamic control.
+#This can be used to test the IK result accuracy.
+useSimulation = 0
+useRealTimeSimulation = 1
+p.setRealTimeSimulation(useRealTimeSimulation)
+#trailDuration is duration (in seconds) after debug lines will be removed automatically
+#use 0 for no-removal
+trailDuration = 15
+basepos =[0,0,0]
+ang = 0
+ang=0
+
+def accurateCalculateInverseKinematics( kukaId, endEffectorId, targetPos, threshold, maxIter):
+ closeEnough = False
+ iter = 0
+ dist2 = 1e30
+ while (not closeEnough and iter -1:
+ p.resetJointState(sawyerId,i,jointPoses[qIndex-7])
+
+ ls = p.getLinkState(sawyerId,sawyerEndEffectorIndex)
+ if (hasPrevPose):
+ p.addUserDebugLine(prevPose,pos,[0,0,0.3],1,trailDuration)
+ p.addUserDebugLine(prevPose1,ls[4],[1,0,0],1,trailDuration)
+ prevPose=pos
+ prevPose1=ls[4]
+ hasPrevPose = 1
diff --git a/examples/pybullet/examples/jointFrictionDamping.py b/examples/pybullet/examples/jointFrictionDamping.py
new file mode 100644
index 000000000..efe34e12f
--- /dev/null
+++ b/examples/pybullet/examples/jointFrictionDamping.py
@@ -0,0 +1,28 @@
+import pybullet as p
+import time
+
+p.connect(p.GUI)
+p.loadURDF("plane.urdf",[0,0,-0.25])
+minitaur = p.loadURDF("quadruped/minitaur_single_motor.urdf",useFixedBase=True)
+print(p.getNumJoints(minitaur))
+p.resetDebugVisualizerCamera(cameraDistance=1, cameraYaw=23.2, cameraPitch=-6.6,cameraTargetPosition=[-0.064,.621,-0.2])
+motorJointId = 1
+p.setJointMotorControl2(minitaur,motorJointId,p.VELOCITY_CONTROL,targetVelocity=100000,force=0)
+
+p.resetJointState(minitaur,motorJointId,targetValue=0, targetVelocity=1)
+angularDampingSlider = p.addUserDebugParameter("angularDamping", 0,1,0)
+jointFrictionForceSlider = p.addUserDebugParameter("jointFrictionForce", 0,0.1,0)
+
+textId = p.addUserDebugText("jointVelocity=0",[0,0,-0.2])
+p.setRealTimeSimulation(1)
+while (1):
+ frictionForce = p.readUserDebugParameter(jointFrictionForceSlider)
+ angularDamping = p.readUserDebugParameter(angularDampingSlider)
+ p.setJointMotorControl2(minitaur,motorJointId,p.VELOCITY_CONTROL,targetVelocity=0,force=frictionForce)
+ p.changeDynamics(minitaur,motorJointId,linearDamping=0, angularDamping=angularDamping)
+
+ time.sleep(0.01)
+ txt = "jointVelocity="+str(p.getJointState(minitaur,motorJointId)[1])
+ prevTextId = textId
+ textId = p.addUserDebugText(txt,[0,0,-0.2])
+ p.removeUserDebugItem(prevTextId)
diff --git a/examples/pybullet/examples/testrender.py b/examples/pybullet/examples/testrender.py
index 72fa9e27e..55eecd07c 100644
--- a/examples/pybullet/examples/testrender.py
+++ b/examples/pybullet/examples/testrender.py
@@ -3,6 +3,7 @@ import matplotlib.pyplot as plt
import pybullet
pybullet.connect(pybullet.GUI)
+pybullet.loadURDF("plane.urdf")
pybullet.loadURDF("r2d2.urdf")
camTargetPos = [0.,0.,0.]
@@ -18,8 +19,8 @@ pixelWidth = 320
pixelHeight = 240
nearPlane = 0.01
farPlane = 1000
-lightDirection = [0,1,0]
-lightColor = [1,1,1]#optional argument
+lightDirection = [0.4,0.4,0]
+lightColor = [.3,.3,.3]#1,1,1]#optional argument
fov = 60
#img_arr = pybullet.renderImage(pixelWidth, pixelHeight)
@@ -30,7 +31,7 @@ for pitch in range (0,360,10) :
aspect = pixelWidth / pixelHeight;
projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane);
#getCameraImage can also use renderer=pybullet.ER_BULLET_HARDWARE_OPENGL
- img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,projectionMatrix, lightDirection,lightColor,renderer=pybullet.ER_TINY_RENDERER)
+ img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,projectionMatrix, lightAmbientCoeff = 0.3, lightDiffuseCoeff = 0.4, shadow=1, renderer=pybullet.ER_TINY_RENDERER)
w=img_arr[0]
h=img_arr[1]
rgb=img_arr[2]
diff --git a/examples/pybullet/gym/pybullet_envs/agents/configs.py b/examples/pybullet/gym/pybullet_envs/agents/configs.py
index 9b2cb2da2..6e787d129 100644
--- a/examples/pybullet/gym/pybullet_envs/agents/configs.py
+++ b/examples/pybullet/gym/pybullet_envs/agents/configs.py
@@ -36,7 +36,7 @@ def default():
eval_episodes = 25
use_gpu = False
# Network
- network = networks.ForwardGaussianPolicy
+ network = networks.feed_forward_gaussian
weight_summaries = dict(
all=r'.*',
policy=r'.*/policy/.*',
diff --git a/examples/pybullet/gym/pybullet_envs/agents/train_ppo.py b/examples/pybullet/gym/pybullet_envs/agents/train_ppo.py
index 6b6a49766..561de7628 100644
--- a/examples/pybullet/gym/pybullet_envs/agents/train_ppo.py
+++ b/examples/pybullet/gym/pybullet_envs/agents/train_ppo.py
@@ -101,8 +101,6 @@ def train(config, env_processes):
"""
tf.reset_default_graph()
with config.unlocked:
- config.network = functools.partial(
- utility.define_network, config.network, config)
config.policy_optimizer = getattr(tf.train, config.policy_optimizer)
config.value_optimizer = getattr(tf.train, config.value_optimizer)
if config.update_every % config.num_agents:
diff --git a/examples/pybullet/gym/pybullet_envs/agents/visualize_ppo.py b/examples/pybullet/gym/pybullet_envs/agents/visualize_ppo.py
index f6673dc1d..f7a67c7ea 100644
--- a/examples/pybullet/gym/pybullet_envs/agents/visualize_ppo.py
+++ b/examples/pybullet/gym/pybullet_envs/agents/visualize_ppo.py
@@ -98,8 +98,6 @@ def visualize(
"""
config = utility.load_config(logdir)
with config.unlocked:
- config.network = functools.partial(
- utility.define_network, config.network, config)
config.policy_optimizer = getattr(tf.train, config.policy_optimizer)
config.value_optimizer = getattr(tf.train, config.value_optimizer)
with tf.device('/cpu:0'):
diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c
index fd578f7d1..2ea7e23c1 100644
--- a/examples/pybullet/pybullet.c
+++ b/examples/pybullet/pybullet.c
@@ -2470,7 +2470,7 @@ static PyObject* pybullet_getConstraintInfo(PyObject* self, PyObject* args, PyOb
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
- static char* kwlist[] = {"constraintUniqueId", "physicsClientId", NULL};
+ static char* kwlist[] = { "constraintUniqueId", "physicsClientId", NULL };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &constraintUniqueId, &physicsClientId))
{
return NULL;
@@ -2487,7 +2487,7 @@ static PyObject* pybullet_getConstraintInfo(PyObject* self, PyObject* args, PyOb
if (b3GetUserConstraintInfo(sm, constraintUniqueId, &constraintInfo))
{
- PyObject* pyListConstraintInfo = PyTuple_New(11);
+ PyObject* pyListConstraintInfo = PyTuple_New(15);
PyTuple_SetItem(pyListConstraintInfo, 0, PyLong_FromLong(constraintInfo.m_parentBodyIndex));
PyTuple_SetItem(pyListConstraintInfo, 1, PyLong_FromLong(constraintInfo.m_parentJointIndex));
@@ -2533,16 +2533,71 @@ static PyObject* pybullet_getConstraintInfo(PyObject* self, PyObject* args, PyOb
PyTuple_SetItem(pyListConstraintInfo, 9, childFrameOrientation);
}
PyTuple_SetItem(pyListConstraintInfo, 10, PyFloat_FromDouble(constraintInfo.m_maxAppliedForce));
+ PyTuple_SetItem(pyListConstraintInfo, 11, PyFloat_FromDouble(constraintInfo.m_gearRatio));
+ PyTuple_SetItem(pyListConstraintInfo, 12, PyLong_FromLong(constraintInfo.m_gearAuxLink));
+ PyTuple_SetItem(pyListConstraintInfo, 13, PyFloat_FromDouble(constraintInfo.m_relativePositionTarget));
+ PyTuple_SetItem(pyListConstraintInfo, 14, PyFloat_FromDouble(constraintInfo.m_erp));
return pyListConstraintInfo;
}
}
}
+
PyErr_SetString(SpamError, "Couldn't get user constraint info");
return NULL;
}
+static PyObject* pybullet_getConstraintState(PyObject* self, PyObject* args, PyObject* keywds)
+{
+ {
+ int constraintUniqueId = -1;
+ b3PhysicsClientHandle sm = 0;
+
+ int physicsClientId = 0;
+ static char* kwlist[] = { "constraintUniqueId", "physicsClientId", NULL };
+ if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &constraintUniqueId, &physicsClientId))
+ {
+ return NULL;
+ }
+ sm = getPhysicsClient(physicsClientId);
+ if (sm == 0)
+ {
+ PyErr_SetString(SpamError, "Not connected to physics server.");
+ return NULL;
+ }
+
+ {
+ b3SharedMemoryCommandHandle cmd_handle;
+ b3SharedMemoryStatusHandle statusHandle;
+ int statusType;
+ if (b3CanSubmitCommand(sm))
+ {
+ struct b3UserConstraintState constraintState;
+ cmd_handle = b3InitGetUserConstraintStateCommand(sm, constraintUniqueId);
+ statusHandle = b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
+ statusType = b3GetStatusType(statusHandle);
+
+ if (b3GetStatusUserConstraintState(statusHandle, &constraintState))
+ {
+ if (constraintState.m_numDofs)
+ {
+ PyObject* appliedConstraintForces = PyTuple_New(constraintState.m_numDofs);
+ int i = 0;
+ for (i = 0; i < constraintState.m_numDofs; i++)
+ {
+ PyTuple_SetItem(appliedConstraintForces, i, PyFloat_FromDouble(constraintState.m_appliedConstraintForces[i]));
+ }
+ return appliedConstraintForces;
+ }
+ }
+ }
+ }
+ }
+ PyErr_SetString(SpamError, "Couldn't getConstraintState.");
+ return NULL;
+}
+
static PyObject* pybullet_getConstraintUniqueId(PyObject* self, PyObject* args, PyObject* keywds)
{
int physicsClientId = 0;
@@ -7004,7 +7059,8 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
PyObject* targetPosObj = 0;
PyObject* targetOrnObj = 0;
-
+
+ int solver = 0; // the default IK solver is DLS
int physicsClientId = 0;
b3PhysicsClientHandle sm = 0;
PyObject* lowerLimitsObj = 0;
@@ -7013,8 +7069,8 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
PyObject* restPosesObj = 0;
PyObject* jointDampingObj = 0;
- static char* kwlist[] = {"bodyUniqueId", "endEffectorLinkIndex", "targetPosition", "targetOrientation", "lowerLimits", "upperLimits", "jointRanges", "restPoses", "jointDamping", "physicsClientId", NULL};
- if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiO|OOOOOOi", kwlist, &bodyUniqueId, &endEffectorLinkIndex, &targetPosObj, &targetOrnObj, &lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &jointDampingObj, &physicsClientId))
+ static char* kwlist[] = {"bodyUniqueId", "endEffectorLinkIndex", "targetPosition", "targetOrientation", "lowerLimits", "upperLimits", "jointRanges", "restPoses", "jointDamping", "solver", "physicsClientId", NULL};
+ if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiO|OOOOOOii", kwlist, &bodyUniqueId, &endEffectorLinkIndex, &targetPosObj, &targetOrnObj, &lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &jointDampingObj, &solver, &physicsClientId))
{
//backward compatibility bodyIndex -> bodyUniqueId. don't update keywords, people need to migrate to bodyUniqueId version
static char* kwlist2[] = {"bodyIndex", "endEffectorLinkIndex", "targetPosition", "targetOrientation", "lowerLimits", "upperLimits", "jointRanges", "restPoses", "jointDamping", "physicsClientId", NULL};
@@ -7112,6 +7168,7 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
int result;
b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm, bodyUniqueId);
+ b3CalculateInverseKinematicsSelectSolver(command, solver);
if (hasNullSpace)
{
@@ -7705,6 +7762,9 @@ static PyMethodDef SpamMethods[] = {
{"getConstraintInfo", (PyCFunction)pybullet_getConstraintInfo, METH_VARARGS | METH_KEYWORDS,
"Get the user-created constraint info, given a constraint unique id."},
+ { "getConstraintState", (PyCFunction)pybullet_getConstraintState, METH_VARARGS | METH_KEYWORDS,
+ "Get the user-created constraint state (applied forces), given a constraint unique id." },
+
{"getConstraintUniqueId", (PyCFunction)pybullet_getConstraintUniqueId, METH_VARARGS | METH_KEYWORDS,
"Get the unique id of the constraint, given a integer index in range [0.. number of constraints)."},
@@ -8139,6 +8199,13 @@ initpybullet(void)
PyModule_AddIntConstant(m, "ER_TINY_RENDERER", ER_TINY_RENDERER);
PyModule_AddIntConstant(m, "ER_BULLET_HARDWARE_OPENGL", ER_BULLET_HARDWARE_OPENGL);
+
+ PyModule_AddIntConstant(m, "IK_DLS", IK_DLS);
+ PyModule_AddIntConstant(m, "IK_SDLS", IK_SDLS);
+ PyModule_AddIntConstant(m, "IK_HAS_TARGET_POSITION", IK_HAS_TARGET_POSITION);
+ PyModule_AddIntConstant(m, "IK_HAS_TARGET_ORIENTATION", IK_HAS_TARGET_ORIENTATION);
+ PyModule_AddIntConstant(m, "IK_HAS_NULL_SPACE_VELOCITY", IK_HAS_NULL_SPACE_VELOCITY);
+ PyModule_AddIntConstant(m, "IK_HAS_JOINT_DAMPING", IK_HAS_JOINT_DAMPING);
PyModule_AddIntConstant(m, "URDF_USE_INERTIA_FROM_FILE", URDF_USE_INERTIA_FROM_FILE);
PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION", URDF_USE_SELF_COLLISION);
diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
index c893b60d3..c3e912fdc 100644
--- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
+++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
@@ -21,6 +21,7 @@ subject to the following restrictions:
#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
#include "BulletCollision/CollisionShapes/btSphereShape.h" //for raycasting
#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" //for raycasting
+#include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h" //for raycasting
#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
#include "BulletCollision/CollisionShapes/btCompoundShape.h"
#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
@@ -407,6 +408,22 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con
rcb.m_hitFraction = resultCallback.m_closestHitFraction;
triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal);
}
+ else if (collisionShape->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE)
+ {
+ ///optimized version for btScaledBvhTriangleMeshShape
+ btScaledBvhTriangleMeshShape* scaledTriangleMesh = (btScaledBvhTriangleMeshShape*)collisionShape;
+ btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)scaledTriangleMesh->getChildShape();
+
+ //scale the ray positions
+ btVector3 scale = scaledTriangleMesh->getLocalScaling();
+ btVector3 rayFromLocalScaled = rayFromLocal / scale;
+ btVector3 rayToLocalScaled = rayToLocal / scale;
+
+ //perform raycast in the underlying btBvhTriangleMeshShape
+ BridgeTriangleRaycastCallback rcb(rayFromLocalScaled, rayToLocalScaled, &resultCallback, collisionObjectWrap->getCollisionObject(), triangleMesh, colObjWorldTransform);
+ rcb.m_hitFraction = resultCallback.m_closestHitFraction;
+ triangleMesh->performRaycast(&rcb, rayFromLocalScaled, rayToLocalScaled);
+ }
else
{
//generic (slower) case
diff --git a/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp b/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp
index aa3d159f8..23c73c882 100644
--- a/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp
+++ b/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp
@@ -22,7 +22,12 @@ subject to the following restrictions:
///This is to allow MaterialCombiner/Custom Friction/Restitution values
ContactAddedCallback gContactAddedCallback=0;
-
+CalculateCombinedCallback gCalculateCombinedRestitutionCallback = &btManifoldResult::calculateCombinedRestitution;
+CalculateCombinedCallback gCalculateCombinedFrictionCallback = &btManifoldResult::calculateCombinedFriction;
+CalculateCombinedCallback gCalculateCombinedRollingFrictionCallback = &btManifoldResult::calculateCombinedRollingFriction;
+CalculateCombinedCallback gCalculateCombinedSpinningFrictionCallback = &btManifoldResult::calculateCombinedSpinningFriction;
+CalculateCombinedCallback gCalculateCombinedContactDampingCallback = &btManifoldResult::calculateCombinedContactDamping;
+CalculateCombinedCallback gCalculateCombinedContactStiffnessCallback = &btManifoldResult::calculateCombinedContactStiffness;
btScalar btManifoldResult::calculateCombinedRollingFriction(const btCollisionObject* body0,const btCollisionObject* body1)
{
@@ -134,16 +139,16 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b
int insertIndex = m_manifoldPtr->getCacheEntry(newPt);
- newPt.m_combinedFriction = calculateCombinedFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
- newPt.m_combinedRestitution = calculateCombinedRestitution(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
- newPt.m_combinedRollingFriction = calculateCombinedRollingFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
- newPt.m_combinedSpinningFriction = calculateCombinedSpinningFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
+ newPt.m_combinedFriction = gCalculateCombinedFrictionCallback(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
+ newPt.m_combinedRestitution = gCalculateCombinedRestitutionCallback(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
+ newPt.m_combinedRollingFriction = gCalculateCombinedRollingFrictionCallback(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
+ newPt.m_combinedSpinningFriction = gCalculateCombinedSpinningFrictionCallback(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
if ( (m_body0Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING) ||
(m_body1Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING))
{
- newPt.m_combinedContactDamping1 = calculateCombinedContactDamping(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
- newPt.m_combinedContactStiffness1 = calculateCombinedContactStiffness(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
+ newPt.m_combinedContactDamping1 = gCalculateCombinedContactDampingCallback(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
+ newPt.m_combinedContactStiffness1 = gCalculateCombinedContactStiffnessCallback(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
newPt.m_contactPointFlags |= BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING;
}
diff --git a/src/BulletCollision/CollisionDispatch/btManifoldResult.h b/src/BulletCollision/CollisionDispatch/btManifoldResult.h
index 86bbc3f72..12cdafd1b 100644
--- a/src/BulletCollision/CollisionDispatch/btManifoldResult.h
+++ b/src/BulletCollision/CollisionDispatch/btManifoldResult.h
@@ -34,6 +34,15 @@ extern ContactAddedCallback gContactAddedCallback;
//#define DEBUG_PART_INDEX 1
+/// These callbacks are used to customize the algorith that combine restitution, friction, damping, Stiffness
+typedef btScalar (*CalculateCombinedCallback)(const btCollisionObject* body0,const btCollisionObject* body1);
+
+extern CalculateCombinedCallback gCalculateCombinedRestitutionCallback;
+extern CalculateCombinedCallback gCalculateCombinedFrictionCallback;
+extern CalculateCombinedCallback gCalculateCombinedRollingFrictionCallback;
+extern CalculateCombinedCallback gCalculateCombinedSpinningFrictionCallback;
+extern CalculateCombinedCallback gCalculateCombinedContactDampingCallback;
+extern CalculateCombinedCallback gCalculateCombinedContactStiffnessCallback;
///btManifoldResult is a helper class to manage contact results.
class btManifoldResult : public btDiscreteCollisionDetectorInterface::Result
diff --git a/src/BulletDynamics/Character/btCharacterControllerInterface.h b/src/BulletDynamics/Character/btCharacterControllerInterface.h
index c3a3ac6c8..abe24b5ca 100644
--- a/src/BulletDynamics/Character/btCharacterControllerInterface.h
+++ b/src/BulletDynamics/Character/btCharacterControllerInterface.h
@@ -37,7 +37,7 @@ public:
virtual void preStep ( btCollisionWorld* collisionWorld) = 0;
virtual void playerStep (btCollisionWorld* collisionWorld, btScalar dt) = 0;
virtual bool canJump () const = 0;
- virtual void jump(const btVector3& dir = btVector3()) = 0;
+ virtual void jump(const btVector3& dir = btVector3(0, 0, 0)) = 0;
virtual bool onGround () const = 0;
virtual void setUpInterpolate (bool value) = 0;
diff --git a/src/BulletDynamics/Character/btKinematicCharacterController.h b/src/BulletDynamics/Character/btKinematicCharacterController.h
index 3d677e647..00c59c024 100644
--- a/src/BulletDynamics/Character/btKinematicCharacterController.h
+++ b/src/BulletDynamics/Character/btKinematicCharacterController.h
@@ -176,7 +176,7 @@ public:
void setMaxJumpHeight (btScalar maxJumpHeight);
bool canJump () const;
- void jump(const btVector3& v = btVector3());
+ void jump(const btVector3& v = btVector3(0, 0, 0));
void applyImpulse(const btVector3& v) { jump(v); }
diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
index fc85b4f86..a196d4522 100644
--- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
+++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
@@ -952,7 +952,7 @@ void btDiscreteDynamicsWorld::createPredictiveContactsInternal( btRigidBody** bo
int index = manifold->addManifoldPoint(newPoint, isPredictive);
btManifoldPoint& pt = manifold->getContactPoint(index);
pt.m_combinedRestitution = 0;
- pt.m_combinedFriction = btManifoldResult::calculateCombinedFriction(body,sweepResults.m_hitCollisionObject);
+ pt.m_combinedFriction = gCalculateCombinedFrictionCallback(body,sweepResults.m_hitCollisionObject);
pt.m_positionWorldOnA = body->getWorldTransform().getOrigin();
pt.m_positionWorldOnB = worldPointB;
@@ -1113,7 +1113,7 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
for (int p=0;pgetNumContacts();p++)
{
const btManifoldPoint& pt = manifold->getContactPoint(p);
- btScalar combinedRestitution = btManifoldResult::calculateCombinedRestitution(body0, body1);
+ btScalar combinedRestitution = gCalculateCombinedRestitutionCallback(body0, body1);
if (combinedRestitution>0 && pt.m_appliedImpulse != 0.f)
//if (pt.getDistance()>0 && combinedRestitution>0 && pt.m_appliedImpulse != 0.f)
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp
index 0f0d9f67b..339b3800c 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp
@@ -156,7 +156,7 @@ void btMultiBodyFixedConstraint::createConstraintRows(btMultiBodyConstraintArray
btVector3 constraintNormalAng(0,0,0);
btScalar posError = 0.0;
if (i < 3) {
- constraintNormalLin[i] = -1;
+ constraintNormalLin[i] = 1;
posError = (pivotAworld-pivotBworld).dot(constraintNormalLin);
fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
constraintNormalLin, pivotAworld, pivotBworld,