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

Merge branch 'master' of https://github.com/erwincoumans/bullet3

Browse Source
This commit is contained in:
erwincoumans
2017-10-25 08:15:14 -07:00
parent ed8de36ffa c1ba04a580
commit 7524e37124
30 changed files with 759 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
Extras/InverseDynamics/btMultiBodyTreeCreator.cpp
View File

@@ -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];

140
data/pole.urdf Executable file
View File

@@ -0,0 +1,140 @@
<?xml version="1.0"?>
<robot name="physics">
<link name="slideBar">
<visual>
<geometry>
<box size="30 0.05 0.05"/>
</geometry>
<origin xyz="0 0 0"/>
<material name="green">
<color rgba="0 0.8 .8 1"/>
</material>
</visual>
<inertial>
<mass value="0"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<link name="pole">
<visual>
<geometry>
<box size="0.05 0.05 1.0"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<inertial>
<origin xyz="0 0 0"/>
<mass value="10"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
<collision>
<geometry>
<box size="0.05 0.05 1.0"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
</collision>
</link>
<joint name="cart_to_pole" type="prismatic">
<axis xyz="1 0 0"/>
<origin xyz="0.0 0.0 0.5"/>
<parent link="slideBar"/>
<child link="pole"/>
<limit effort="1000.0" lower="-5" upper="5" velocity="0.5"/>
</joint>
<link name="pole2">
<visual>
<geometry>
<box size="0.05 0.05 1.0"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.5"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<inertial>
<origin xyz="0 0 0.5"/>
<mass value="10"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
<collision>
<geometry>
<box size="0.05 0.05 1.0"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.5"/>
</collision>
</link>
<joint name="pole_to_pole2" type="continuous">
<axis xyz="1 0 0"/>
<origin xyz="0.0 0.0 0.5"/>
<parent link="pole"/>
<child link="pole2"/>
</joint>
<link name="pole3">
<visual>
<geometry>
<box size="0.05 0.05 1.0"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.5"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<inertial>
<origin xyz="0 0 0.5"/>
<mass value="10"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
<collision>
<geometry>
<box size="0.05 0.05 1.0"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.5"/>
</collision>
</link>
<joint name="pole2_to_pole3" type="continuous">
<axis xyz="0 1 0"/>
<origin xyz="0.0 0.0 1"/>
<parent link="pole2"/>
<child link="pole3"/>
</joint>
<link name="endeffector">
<visual>
<geometry>
<box size="0.06 0.06 .06"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
<material name="red">
<color rgba="1 0 0 1"/>
</material>
</visual>
<inertial>
<origin xyz="0 0 0"/>
<mass value="10"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
<collision>
<geometry>
<box size="0.06 0.06 .06"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
</collision>
</link>
<joint name="pole2_to_endeffector" type="fixed">
<origin xyz="0.0 0.0 1"/>
<parent link="pole3"/>
<child link="endeffector"/>
</joint>
</robot>

7
examples/SharedMemory/IKTrajectoryHelper.cpp
View File

@@ -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:

2
examples/SharedMemory/IKTrajectoryHelper.h
View File

@@ -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,
};

56
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -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)
@@ -3614,6 +3652,12 @@ B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMem
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;
}
B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3], const double targetOrientation[4])
{
@@ -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,

6
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -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,

20
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -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;
}

21
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -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:

127
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -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 && (endEffectorLinkIndex<bodyHandle->m_multiBody->getNumLinks()))
{
const int numDofs = bodyHandle->m_multiBody->getNumDofs();
int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
b3AlignedObjectArray<double> jacobian_linear;
jacobian_linear.resize(3*numDofs);
b3AlignedObjectArray<double> jacobian_angular;
@@ -7658,11 +7688,13 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
btAlignedObjectArray<double> 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,14 +7790,43 @@ 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);
endEffectorPosWorld.serializeDouble(endEffectorWorldPosition);
endEffectorOri.serializeDouble(endEffectorWorldOrientation);
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);
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
@@ -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,

12
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -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;

23
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -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,

3
examples/ThirdPartyLibs/BussIK/Jacobian.cpp
View File

@@ -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();

2
examples/ThirdPartyLibs/clsocket/src/StatTimer.h
View File

@@ -46,7 +46,7 @@
#include <string.h>
#if WIN32
#include <Winsock2.h>
#include <winsock2.h>
#include <time.h>
#endif

2
examples/pybullet/examples/inverse_dynamics.py
View File

@@ -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):

7
examples/pybullet/examples/inverse_kinematics.py
View File

@@ -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):

169
examples/pybullet/examples/inverse_kinematics_husky_kuka.py Normal file
View File

@@ -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<maxIter):
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,targetPos)
for i in range (numJoints):
p.resetJointState(kukaId,i,jointPoses[i])
ls = p.getLinkState(kukaId,kukaEndEffectorIndex)
newPos = ls[4]
diff = [targetPos[0]-newPos[0],targetPos[1]-newPos[1],targetPos[2]-newPos[2]]
dist2 = (diff[0]*diff[0] + diff[1]*diff[1] + diff[2]*diff[2])
closeEnough = (dist2 < threshold)
iter=iter+1
#print ("Num iter: "+str(iter) + "threshold: "+str(dist2))
return jointPoses
wheels=[2,3,4,5]
#(2, b'front_left_wheel', 0, 7, 6, 1, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'front_left_wheel_link')
#(3, b'front_right_wheel', 0, 8, 7, 1, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'front_right_wheel_link')
#(4, b'rear_left_wheel', 0, 9, 8, 1, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'rear_left_wheel_link')
#(5, b'rear_right_wheel', 0, 10, 9, 1, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'rear_right_wheel_link')
wheelVelocities=[0,0,0,0]
wheelDeltasTurn=[1,-1,1,-1]
wheelDeltasFwd=[1,1,1,1]
while 1:
keys = p.getKeyboardEvents()
shift = 0.01
wheelVelocities=[0,0,0,0]
speed = 1.0
for k in keys:
if ord('s') in keys:
p.saveWorld("state.py")
if ord('a') in keys:
basepos = basepos=[basepos[0],basepos[1]-shift,basepos[2]]
if ord('d') in keys:
basepos = basepos=[basepos[0],basepos[1]+shift,basepos[2]]
if p.B3G_LEFT_ARROW in keys:
for i in range(len(wheels)):
wheelVelocities[i] = wheelVelocities[i] - speed*wheelDeltasTurn[i]
if p.B3G_RIGHT_ARROW in keys:
for i in range(len(wheels)):
wheelVelocities[i] = wheelVelocities[i] +speed*wheelDeltasTurn[i]
if p.B3G_UP_ARROW in keys:
for i in range(len(wheels)):
wheelVelocities[i] = wheelVelocities[i] + speed*wheelDeltasFwd[i]
if p.B3G_DOWN_ARROW in keys:
for i in range(len(wheels)):
wheelVelocities[i] = wheelVelocities[i] -speed*wheelDeltasFwd[i]
baseorn = p.getQuaternionFromEuler([0,0,ang])
for i in range(len(wheels)):
p.setJointMotorControl2(husky,wheels[i],p.VELOCITY_CONTROL,targetVelocity=wheelVelocities[i], force=1000)
#p.resetBasePositionAndOrientation(kukaId,basepos,baseorn)#[0,0,0,1])
if (useRealTimeSimulation):
t = time.time()#(dt, micro) = datetime.utcnow().strftime('%Y-%m-%d %H:%M:%S.%f').split('.')
#t = (dt.second/60.)*2.*math.pi
else:
t=t+0.001
if (useSimulation and useRealTimeSimulation==0):
p.stepSimulation()
for i in range (1):
#pos = [-0.4,0.2*math.cos(t),0.+0.2*math.sin(t)]
pos = [0.2*math.cos(t),0,0.+0.2*math.sin(t)+0.7]
#end effector points down, not up (in case useOrientation==1)
orn = p.getQuaternionFromEuler([0,-math.pi,0])
if (useNullSpace==1):
if (useOrientation==1):
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,ll,ul,jr,rp)
else:
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)
else:
threshold =0.001
maxIter = 100
jointPoses = accurateCalculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos, threshold, maxIter)
if (useSimulation):
for i in range (numJoints):
p.setJointMotorControl2(bodyIndex=kukaId,jointIndex=i,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[i],targetVelocity=0,force=500,positionGain=1,velocityGain=0.1)
else:
#reset the joint state (ignoring all dynamics, not recommended to use during simulation)
for i in range (numJoints):
p.resetJointState(kukaId,i,jointPoses[i])
ls = p.getLinkState(kukaId,kukaEndEffectorIndex)
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

59
examples/pybullet/examples/inverse_kinematics_pole.py Executable file
View File

@@ -0,0 +1,59 @@
import pybullet as p
import time
import math
from datetime import datetime
clid = p.connect(p.SHARED_MEMORY)
if (clid<0):
p.connect(p.GUI)
p.loadURDF("plane.urdf",[0,0,-1.3])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
sawyerId = p.loadURDF("pole.urdf",[0,0,0])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
p.resetBasePositionAndOrientation(sawyerId,[0,0,0],[0,0,0,1])
sawyerEndEffectorIndex = 3
numJoints = p.getNumJoints(sawyerId)
#joint damping coefficents
jd=[0.01,0.01,0.01,0.01]
p.setGravity(0,0,0)
t=0.
prevPose=[0,0,0]
prevPose1=[0,0,0]
hasPrevPose = 0
ikSolver = 0
useRealTimeSimulation = 0
p.setRealTimeSimulation(useRealTimeSimulation)
#trailDuration is duration (in seconds) after debug lines will be removed automatically
#use 0 for no-removal
trailDuration = 1
while 1:
if (useRealTimeSimulation):
dt = datetime.now()
t = (dt.second/60.)*2.*math.pi
else:
t=t+0.01
time.sleep(0.01)
for i in range (1):
pos = [2.*math.cos(t),2.*math.cos(t),0.+2.*math.sin(t)]
jointPoses = p.calculateInverseKinematics(sawyerId,sawyerEndEffectorIndex,pos,jointDamping=jd,solver=ikSolver)
#reset the joint state (ignoring all dynamics, not recommended to use during simulation)
for i in range (numJoints):
jointInfo = p.getJointInfo(sawyerId, i)
qIndex = jointInfo[3]
if qIndex > -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

28
examples/pybullet/examples/jointFrictionDamping.py Normal file
View File

@@ -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)

7
examples/pybullet/examples/testrender.py
View File

@@ -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]

2
examples/pybullet/gym/pybullet_envs/agents/configs.py
View File

@@ -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/.*',

2
examples/pybullet/gym/pybullet_envs/agents/train_ppo.py
View File

@@ -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:

2
examples/pybullet/gym/pybullet_envs/agents/visualize_ppo.py
View File

@@ -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'):

75
examples/pybullet/pybullet.c
View File

@@ -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;
@@ -7005,6 +7060,7 @@ 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)."},
@@ -8140,6 +8200,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);
PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION_EXCLUDE_PARENT", URDF_USE_SELF_COLLISION_EXCLUDE_PARENT);

17
src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
View File

@@ -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

19
src/BulletCollision/CollisionDispatch/btManifoldResult.cpp
View File

@@ -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;
}

9
src/BulletCollision/CollisionDispatch/btManifoldResult.h
View File

@@ -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

2
src/BulletDynamics/Character/btCharacterControllerInterface.h
View File

@@ -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;

2
src/BulletDynamics/Character/btKinematicCharacterController.h
View File

@@ -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); }

4
src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
View File

@@ -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;p<manifold->getNumContacts();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)

2
src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp
View File

@@ -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,