mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-14 05:40:05 +00:00
Merge branch 'master' of https://github.com/erwincoumans/bullet3
# Conflicts: # examples/SharedMemory/PhysicsServerCommandProcessor.cpp
This commit is contained in:
erwincoumans
commit
92579f9196
@ -25,6 +25,8 @@ m_guiHelper(guiHelper)
|
||||
m_mb2urdfLink.resize(totalNumJoints+1,-2);
|
||||
|
||||
m_bulletMultiBody = new btMultiBody(totalNumJoints,mass,localInertiaDiagonal,isFixedBase,canSleep);
|
||||
//if (canSleep)
|
||||
// m_bulletMultiBody->goToSleep();
|
||||
return m_bulletMultiBody;
|
||||
}
|
||||
|
||||
|
||||
@ -595,6 +595,11 @@ void ConvertURDF2BulletInternal(
|
||||
}
|
||||
} else
|
||||
{
|
||||
//todo: fix the crash it can cause
|
||||
//if (cache.m_bulletMultiBody->getBaseMass()==0)
|
||||
//{
|
||||
// col->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT);//:CF_STATIC_OBJECT);
|
||||
//}
|
||||
cache.m_bulletMultiBody->setBaseCollider(col);
|
||||
}
|
||||
}
|
||||
|
||||
@ -1270,6 +1270,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getDynamicsInfo(int bodyUniqueId, int link
|
||||
status_handle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, cmd_handle);
|
||||
status_type = b3GetStatusType(status_handle);
|
||||
if (status_type == CMD_GET_DYNAMICS_INFO_COMPLETED) {
|
||||
b3GetDynamicsInfo(status_handle, dynamicsInfo);
|
||||
return true;
|
||||
} else {
|
||||
b3Warning("getDynamicsInfo did not complete");
|
||||
@ -1396,7 +1397,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::removeUserDebugItem(int itemUniqueId) {
|
||||
}
|
||||
|
||||
|
||||
int b3RobotSimulatorClientAPI_NoGUI::addUserDebugText(char *text, double *posXYZ, struct b3RobotSimulatorAddUserDebugTextArgs & args)
|
||||
int b3RobotSimulatorClientAPI_NoGUI::addUserDebugText(char *text, double *textPosition, struct b3RobotSimulatorAddUserDebugTextArgs & args)
|
||||
{
|
||||
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
|
||||
if (sm == 0) {
|
||||
@ -1407,7 +1408,7 @@ int b3RobotSimulatorClientAPI_NoGUI::addUserDebugText(char *text, double *posXYZ
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
|
||||
commandHandle = b3InitUserDebugDrawAddText3D(sm, text, posXYZ, &args.m_colorRGB[0], args.m_size, args.m_lifeTime);
|
||||
commandHandle = b3InitUserDebugDrawAddText3D(sm, text, textPosition, &args.m_colorRGB[0], args.m_size, args.m_lifeTime);
|
||||
|
||||
if (args.m_parentObjectUniqueId>=0) {
|
||||
b3UserDebugItemSetParentObject(commandHandle, args.m_parentObjectUniqueId, args.m_parentLinkIndex);
|
||||
@ -1428,12 +1429,12 @@ int b3RobotSimulatorClientAPI_NoGUI::addUserDebugText(char *text, double *posXYZ
|
||||
return -1;
|
||||
}
|
||||
|
||||
int b3RobotSimulatorClientAPI_NoGUI::addUserDebugText(char *text, btVector3 &posXYZ, struct b3RobotSimulatorAddUserDebugTextArgs & args)
|
||||
int b3RobotSimulatorClientAPI_NoGUI::addUserDebugText(char *text, btVector3 &textPosition, struct b3RobotSimulatorAddUserDebugTextArgs & args)
|
||||
{
|
||||
double dposXYZ[3];
|
||||
dposXYZ[0] = posXYZ.x();
|
||||
dposXYZ[1] = posXYZ.y();
|
||||
dposXYZ[2] = posXYZ.z();
|
||||
dposXYZ[0] = textPosition.x();
|
||||
dposXYZ[1] = textPosition.y();
|
||||
dposXYZ[2] = textPosition.z();
|
||||
|
||||
return addUserDebugText(text, &dposXYZ[0], args);
|
||||
}
|
||||
@ -1626,6 +1627,10 @@ bool b3RobotSimulatorClientAPI_NoGUI::setPhysicsEngineParameter(struct b3RobotSi
|
||||
b3PhysicsParamSetDefaultFrictionERP(command,args.m_frictionERP);
|
||||
}
|
||||
|
||||
if (args.m_solverResidualThreshold >= 0) {
|
||||
b3PhysicsParamSetSolverResidualThreshold(command, args.m_solverResidualThreshold);
|
||||
}
|
||||
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
||||
return true;
|
||||
}
|
||||
@ -2060,4 +2065,53 @@ void b3RobotSimulatorClientAPI_NoGUI::setGuiHelper(struct GUIHelperInterface* gu
|
||||
struct GUIHelperInterface* b3RobotSimulatorClientAPI_NoGUI::getGuiHelper()
|
||||
{
|
||||
return m_data->m_guiHelper;
|
||||
}
|
||||
}
|
||||
|
||||
bool b3RobotSimulatorClientAPI_NoGUI::getCollisionShapeData(int bodyUniqueId, int linkIndex,
|
||||
b3CollisionShapeInformation &collisionShapeInfo)
|
||||
{
|
||||
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
|
||||
if (sm == 0) {
|
||||
b3Warning("Not connected");
|
||||
return false;
|
||||
}
|
||||
b3SharedMemoryCommandHandle command;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
|
||||
{
|
||||
command = b3InitRequestCollisionShapeInformation(sm, bodyUniqueId, linkIndex);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
}
|
||||
|
||||
btAssert(statusType == CMD_COLLISION_SHAPE_INFO_COMPLETED);
|
||||
if (statusType == CMD_COLLISION_SHAPE_INFO_COMPLETED) {
|
||||
b3GetCollisionShapeInformation(sm, &collisionShapeInfo);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool b3RobotSimulatorClientAPI_NoGUI::getVisualShapeData(int bodyUniqueId, b3VisualShapeInformation &visualShapeInfo)
|
||||
{
|
||||
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
|
||||
if (sm == 0) {
|
||||
b3Warning("Not connected");
|
||||
return false;
|
||||
}
|
||||
b3SharedMemoryCommandHandle commandHandle;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
|
||||
{
|
||||
commandHandle = b3InitRequestVisualShapeInformation(sm, bodyUniqueId);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
|
||||
btAssert(statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED);
|
||||
if (statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED) {
|
||||
b3GetVisualShapeInformation(sm, &visualShapeInfo);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
@ -152,7 +152,14 @@ struct b3RobotSimulatorJointMotorArrayArgs
|
||||
double *m_forces;
|
||||
|
||||
b3RobotSimulatorJointMotorArrayArgs(int controlMode, int numControlledDofs)
|
||||
: m_controlMode(controlMode), m_numControlledDofs(numControlledDofs)
|
||||
: m_controlMode(controlMode),
|
||||
m_numControlledDofs(numControlledDofs),
|
||||
m_jointIndices(NULL),
|
||||
m_targetPositions(NULL),
|
||||
m_kps(NULL),
|
||||
m_targetVelocities(NULL),
|
||||
m_kds(NULL),
|
||||
m_forces(NULL)
|
||||
{
|
||||
}
|
||||
};
|
||||
@ -204,6 +211,7 @@ struct b3RobotSimulatorSetPhysicsEngineParameters
|
||||
double m_erp;
|
||||
double m_contactERP;
|
||||
double m_frictionERP;
|
||||
double m_solverResidualThreshold;
|
||||
|
||||
b3RobotSimulatorSetPhysicsEngineParameters()
|
||||
: m_fixedTimeStep(-1),
|
||||
@ -218,7 +226,8 @@ struct b3RobotSimulatorSetPhysicsEngineParameters
|
||||
m_restitutionVelocityThreshold(-1),
|
||||
m_erp(-1),
|
||||
m_contactERP(-1),
|
||||
m_frictionERP(-1)
|
||||
m_frictionERP(-1),
|
||||
m_solverResidualThreshold(-1)
|
||||
{}
|
||||
};
|
||||
|
||||
@ -566,6 +575,10 @@ public:
|
||||
|
||||
virtual void setGuiHelper(struct GUIHelperInterface* guiHelper);
|
||||
virtual struct GUIHelperInterface* getGuiHelper();
|
||||
|
||||
bool getCollisionShapeData(int bodyUniqueId, int linkIndex, b3CollisionShapeInformation &collisionShapeInfo);
|
||||
|
||||
bool getVisualShapeData(int bodyUniqueId, b3VisualShapeInformation &visualShapeInfo);
|
||||
|
||||
};
|
||||
|
||||
|
||||
@ -1516,7 +1516,7 @@ B3_SHARED_API int b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHan
|
||||
return 0;
|
||||
}
|
||||
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex)
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId)
|
||||
{
|
||||
|
||||
PhysicsClient* cl = (PhysicsClient* ) physClient;
|
||||
@ -1526,7 +1526,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClien
|
||||
b3Assert(command);
|
||||
command->m_type = CMD_INIT_POSE;
|
||||
command->m_updateFlags =0;
|
||||
command->m_initPoseArgs.m_bodyUniqueId = bodyIndex;
|
||||
command->m_initPoseArgs.m_bodyUniqueId = bodyUniqueId;
|
||||
//a bit slow, initialing the full range to zero...
|
||||
for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
|
||||
{
|
||||
@ -2037,16 +2037,54 @@ B3_SHARED_API int b3GetBodyInfo(b3PhysicsClientHandle physClient, int bodyUnique
|
||||
|
||||
|
||||
|
||||
B3_SHARED_API int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyId)
|
||||
B3_SHARED_API int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyUniqueId)
|
||||
{
|
||||
PhysicsClient* cl = (PhysicsClient* ) physClient;
|
||||
return cl->getNumJoints(bodyId);
|
||||
return cl->getNumJoints(bodyUniqueId);
|
||||
}
|
||||
|
||||
B3_SHARED_API int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info)
|
||||
B3_SHARED_API int b3ComputeDofCount(b3PhysicsClientHandle physClient, int bodyUniqueId)
|
||||
{
|
||||
int nj = b3GetNumJoints(physClient, bodyUniqueId);
|
||||
int j=0;
|
||||
int dofCountOrg = 0;
|
||||
for (j=0;j<nj;j++)
|
||||
{
|
||||
struct b3JointInfo info;
|
||||
b3GetJointInfo(physClient, bodyUniqueId, j, &info);
|
||||
switch (info.m_jointType)
|
||||
{
|
||||
case eRevoluteType:
|
||||
{
|
||||
dofCountOrg+=1;
|
||||
break;
|
||||
}
|
||||
case ePrismaticType:
|
||||
{
|
||||
dofCountOrg+=1;
|
||||
break;
|
||||
}
|
||||
case eSphericalType:
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
case ePlanarType:
|
||||
{
|
||||
return -2;
|
||||
}
|
||||
default:
|
||||
{
|
||||
//fixed joint has 0-dof and at the moment, we don't deal with planar, spherical etc
|
||||
}
|
||||
}
|
||||
}
|
||||
return dofCountOrg;
|
||||
}
|
||||
|
||||
B3_SHARED_API int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, struct b3JointInfo* info)
|
||||
{
|
||||
PhysicsClient* cl = (PhysicsClient* ) physClient;
|
||||
return cl->getJointInfo(bodyIndex, jointIndex, *info);
|
||||
return cl->getJointInfo(bodyUniqueId, jointIndex, *info);
|
||||
}
|
||||
|
||||
|
||||
@ -2389,7 +2427,7 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetContactProcessingThreshold(b3SharedMemo
|
||||
|
||||
|
||||
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info)
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyUniqueId, int parentJointIndex, int childBodyUniqueId, int childJointIndex, struct b3JointInfo* info)
|
||||
{
|
||||
PhysicsClient* cl = (PhysicsClient* ) physClient;
|
||||
b3Assert(cl);
|
||||
@ -2400,9 +2438,9 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3Ph
|
||||
command->m_type = CMD_USER_CONSTRAINT;
|
||||
command->m_updateFlags = USER_CONSTRAINT_ADD_CONSTRAINT;
|
||||
|
||||
command->m_userConstraintArguments.m_parentBodyIndex = parentBodyIndex;
|
||||
command->m_userConstraintArguments.m_parentBodyIndex = parentBodyUniqueId;
|
||||
command->m_userConstraintArguments.m_parentJointIndex = parentJointIndex;
|
||||
command->m_userConstraintArguments.m_childBodyIndex = childBodyIndex;
|
||||
command->m_userConstraintArguments.m_childBodyIndex = childBodyUniqueId;
|
||||
command->m_userConstraintArguments.m_childJointIndex = childJointIndex;
|
||||
for (int i = 0; i < 7; ++i) {
|
||||
command->m_userConstraintArguments.m_parentFrame[i] = info->m_parentFrame[i];
|
||||
@ -3742,7 +3780,7 @@ B3_SHARED_API void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHand
|
||||
|
||||
|
||||
///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex,
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId,
|
||||
const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations)
|
||||
{
|
||||
PhysicsClient* cl = (PhysicsClient*)physClient;
|
||||
@ -3753,8 +3791,8 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(
|
||||
|
||||
command->m_type = CMD_CALCULATE_INVERSE_DYNAMICS;
|
||||
command->m_updateFlags = 0;
|
||||
command->m_calculateInverseDynamicsArguments.m_bodyUniqueId = bodyIndex;
|
||||
int numJoints = cl->getNumJoints(bodyIndex);
|
||||
command->m_calculateInverseDynamicsArguments.m_bodyUniqueId = bodyUniqueId;
|
||||
int numJoints = cl->getNumJoints(bodyUniqueId);
|
||||
for (int i = 0; i < numJoints;i++)
|
||||
{
|
||||
command->m_calculateInverseDynamicsArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
|
||||
@ -3799,7 +3837,7 @@ B3_SHARED_API int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHand
|
||||
return true;
|
||||
}
|
||||
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, const double* localPosition, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations)
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations)
|
||||
{
|
||||
PhysicsClient* cl = (PhysicsClient*)physClient;
|
||||
b3Assert(cl);
|
||||
@ -3809,12 +3847,12 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3Physi
|
||||
|
||||
command->m_type = CMD_CALCULATE_JACOBIAN;
|
||||
command->m_updateFlags = 0;
|
||||
command->m_calculateJacobianArguments.m_bodyUniqueId = bodyIndex;
|
||||
command->m_calculateJacobianArguments.m_bodyUniqueId = bodyUniqueId;
|
||||
command->m_calculateJacobianArguments.m_linkIndex = linkIndex;
|
||||
command->m_calculateJacobianArguments.m_localPosition[0] = localPosition[0];
|
||||
command->m_calculateJacobianArguments.m_localPosition[1] = localPosition[1];
|
||||
command->m_calculateJacobianArguments.m_localPosition[2] = localPosition[2];
|
||||
int numJoints = cl->getNumJoints(bodyIndex);
|
||||
int numJoints = cl->getNumJoints(bodyUniqueId);
|
||||
for (int i = 0; i < numJoints;i++)
|
||||
{
|
||||
command->m_calculateJacobianArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
|
||||
@ -3859,7 +3897,7 @@ B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, i
|
||||
return true;
|
||||
}
|
||||
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, const double* jointPositionsQ)
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, const double* jointPositionsQ)
|
||||
{
|
||||
PhysicsClient* cl = (PhysicsClient*)physClient;
|
||||
b3Assert(cl);
|
||||
@ -3869,7 +3907,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3Phy
|
||||
|
||||
command->m_type = CMD_CALCULATE_MASS_MATRIX;
|
||||
command->m_updateFlags = 0;
|
||||
int numJoints = cl->getNumJoints(bodyIndex);
|
||||
int numJoints = cl->getNumJoints(bodyUniqueId);
|
||||
for (int i = 0; i < numJoints; i++)
|
||||
{
|
||||
command->m_calculateMassMatrixArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
|
||||
@ -3905,7 +3943,7 @@ B3_SHARED_API int b3GetStatusMassMatrix(b3PhysicsClientHandle physClient, b3Shar
|
||||
}
|
||||
|
||||
///compute the joint positions to move the end effector to a desired target using inverse kinematics
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex)
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId)
|
||||
{
|
||||
PhysicsClient* cl = (PhysicsClient*)physClient;
|
||||
b3Assert(cl);
|
||||
@ -3915,7 +3953,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandIni
|
||||
|
||||
command->m_type = CMD_CALCULATE_INVERSE_KINEMATICS;
|
||||
command->m_updateFlags = 0;
|
||||
command->m_calculateInverseKinematicsArguments.m_bodyUniqueId = bodyIndex;
|
||||
command->m_calculateInverseKinematicsArguments.m_bodyUniqueId = bodyUniqueId;
|
||||
|
||||
return (b3SharedMemoryCommandHandle)command;
|
||||
|
||||
@ -4008,6 +4046,36 @@ B3_SHARED_API void b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(b3SharedMe
|
||||
|
||||
}
|
||||
|
||||
B3_SHARED_API void b3CalculateInverseKinematicsSetCurrentPositions(b3SharedMemoryCommandHandle commandHandle, int numDof, const double* currentJointPositions)
|
||||
{
|
||||
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
|
||||
b3Assert(command);
|
||||
b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
|
||||
command->m_updateFlags |= IK_HAS_CURRENT_JOINT_POSITIONS;
|
||||
for (int i = 0; i < numDof; ++i)
|
||||
{
|
||||
command->m_calculateInverseKinematicsArguments.m_currentPositions[i] = currentJointPositions[i];
|
||||
}
|
||||
}
|
||||
|
||||
B3_SHARED_API void b3CalculateInverseKinematicsSetMaxNumIterations(b3SharedMemoryCommandHandle commandHandle, int maxNumIterations)
|
||||
{
|
||||
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
|
||||
b3Assert(command);
|
||||
b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
|
||||
command->m_updateFlags |= IK_HAS_MAX_ITERATIONS;
|
||||
command->m_calculateInverseKinematicsArguments.m_maxNumIterations = maxNumIterations;
|
||||
}
|
||||
|
||||
B3_SHARED_API void b3CalculateInverseKinematicsSetResidualThreshold(b3SharedMemoryCommandHandle commandHandle, double residualThreshold)
|
||||
{
|
||||
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
|
||||
b3Assert(command);
|
||||
b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
|
||||
command->m_updateFlags |= IK_HAS_RESIDUAL_THRESHOLD;
|
||||
command->m_calculateInverseKinematicsArguments.m_residualThreshold = residualThreshold;
|
||||
}
|
||||
|
||||
B3_SHARED_API void b3CalculateInverseKinematicsSetJointDamping(b3SharedMemoryCommandHandle commandHandle, int numDof, const double* jointDampingCoeff)
|
||||
{
|
||||
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
|
||||
|
||||
@ -104,10 +104,14 @@ B3_SHARED_API int b3GetBodyUniqueId(b3PhysicsClientHandle physClient, int serial
|
||||
B3_SHARED_API int b3GetBodyInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, struct b3BodyInfo* info);
|
||||
|
||||
///give a unique body index (after loading the body) return the number of joints.
|
||||
B3_SHARED_API int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyId);
|
||||
B3_SHARED_API int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyUniqueId);
|
||||
|
||||
///compute the number of degrees of freedom for this body.
|
||||
///Return -1 for unsupported spherical joint, -2 for unsupported planar joint.
|
||||
B3_SHARED_API int b3ComputeDofCount(b3PhysicsClientHandle physClient, int bodyUniqueId);
|
||||
|
||||
///given a body and joint index, return the joint information. See b3JointInfo in SharedMemoryPublic.h
|
||||
B3_SHARED_API int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info);
|
||||
B3_SHARED_API int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, struct b3JointInfo* info);
|
||||
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3GetDynamicsInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex);
|
||||
///given a body unique id and link index, return the dynamics information. See b3DynamicsInfo in SharedMemoryPublic.h
|
||||
@ -128,7 +132,7 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetFrictionAnchor(b3SharedMemoryCommandHan
|
||||
B3_SHARED_API int b3ChangeDynamicsInfoSetCcdSweptSphereRadius(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex, double ccdSweptSphereRadius);
|
||||
B3_SHARED_API int b3ChangeDynamicsInfoSetContactProcessingThreshold(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double contactProcessingThreshold);
|
||||
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info);
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyUniqueId, int parentJointIndex, int childBodyUniqueId, int childJointIndex, struct b3JointInfo* info);
|
||||
|
||||
///return a unique id for the user constraint, after successful creation, or -1 for an invalid constraint id
|
||||
B3_SHARED_API int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle);
|
||||
@ -350,26 +354,26 @@ B3_SHARED_API void b3LoadMJCFCommandSetFlags(b3SharedMemoryCommandHandle command
|
||||
|
||||
|
||||
///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex,
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId,
|
||||
const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations);
|
||||
B3_SHARED_API int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandle,
|
||||
int* bodyUniqueId,
|
||||
int* dofCount,
|
||||
double* jointForces);
|
||||
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, const double* localPosition, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations);
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations);
|
||||
B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle,
|
||||
int* dofCount,
|
||||
double* linearJacobian,
|
||||
double* angularJacobian);
|
||||
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, const double* jointPositionsQ);
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, const double* jointPositionsQ);
|
||||
///the mass matrix is stored in column-major layout of size dofCount*dofCount
|
||||
B3_SHARED_API int b3GetStatusMassMatrix(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int* dofCount, double* massMatrix);
|
||||
|
||||
|
||||
///compute the joint positions to move the end effector to a desired target using inverse kinematics
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex);
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId);
|
||||
B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[/*3*/]);
|
||||
B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[/*3*/], const double targetOrientation[/*4*/]);
|
||||
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);
|
||||
@ -381,6 +385,11 @@ B3_SHARED_API int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatu
|
||||
int* dofCount,
|
||||
double* jointPositions);
|
||||
|
||||
B3_SHARED_API void b3CalculateInverseKinematicsSetCurrentPositions(b3SharedMemoryCommandHandle commandHandle, int numDof, const double* currentJointPositions);
|
||||
B3_SHARED_API void b3CalculateInverseKinematicsSetMaxNumIterations(b3SharedMemoryCommandHandle commandHandle, int maxNumIterations);
|
||||
B3_SHARED_API void b3CalculateInverseKinematicsSetResidualThreshold(b3SharedMemoryCommandHandle commandHandle, double residualThreshold);
|
||||
|
||||
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
|
||||
B3_SHARED_API int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
|
||||
B3_SHARED_API int b3LoadSdfCommandSetUseGlobalScaling(b3SharedMemoryCommandHandle commandHandle, double globalScaling);
|
||||
@ -476,7 +485,7 @@ B3_SHARED_API int b3CreateBoxCommandSetColorRGBA(b3SharedMemoryCommandHandle com
|
||||
///b3CreatePoseCommandInit will initialize (teleport) the pose of a body/robot. You can individually set the base position,
|
||||
///base orientation and joint angles. This will set all velocities of base and joints to zero.
|
||||
///This is not a robot control command using actuators/joint motors, but manual repositioning the robot.
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex);
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId);
|
||||
B3_SHARED_API int b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ);
|
||||
B3_SHARED_API int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
|
||||
B3_SHARED_API int b3CreatePoseCommandSetBaseLinearVelocity(b3SharedMemoryCommandHandle commandHandle, double linVel[/*3*/]);
|
||||
|
||||
@ -695,6 +695,9 @@ struct CalculateInverseKinematicsArgs
|
||||
double m_jointRange[MAX_DEGREE_OF_FREEDOM];
|
||||
double m_restPose[MAX_DEGREE_OF_FREEDOM];
|
||||
double m_jointDamping[MAX_DEGREE_OF_FREEDOM];
|
||||
double m_currentPositions[MAX_DEGREE_OF_FREEDOM];
|
||||
int m_maxNumIterations;
|
||||
double m_residualThreshold;
|
||||
};
|
||||
|
||||
struct CalculateInverseKinematicsResultArgs
|
||||
|
||||
@ -628,7 +628,9 @@ enum EnumCalculateInverseKinematicsFlags
|
||||
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
|
||||
IK_HAS_CURRENT_JOINT_POSITIONS=256,
|
||||
IK_HAS_MAX_ITERATIONS=512,
|
||||
IK_HAS_RESIDUAL_THRESHOLD = 1024,
|
||||
};
|
||||
|
||||
enum b3ConfigureDebugVisualizerEnum
|
||||
|
||||
@ -34,12 +34,12 @@ t=0.
|
||||
prevPose=[0,0,0]
|
||||
prevPose1=[0,0,0]
|
||||
hasPrevPose = 0
|
||||
useNullSpace = 0
|
||||
useNullSpace = 1
|
||||
|
||||
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 = 1
|
||||
useSimulation = 0
|
||||
useRealTimeSimulation = 1
|
||||
ikSolver = 0
|
||||
p.setRealTimeSimulation(useRealTimeSimulation)
|
||||
@ -69,7 +69,7 @@ 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,solver=ikSolver)
|
||||
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,jointDamping=jd,solver=ikSolver, maxNumIterations=100, residualThreshold=.01)
|
||||
else:
|
||||
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,solver=ikSolver)
|
||||
|
||||
|
||||
@ -24,8 +24,8 @@ ground = p.loadURDF("plane.urdf",[0,0,0], flags=p.URDF_ENABLE_CACHED_GRAPHICS_SH
|
||||
p.setPhysicsEngineParameter(solverResidualThreshold=1e-2)
|
||||
|
||||
index = 0
|
||||
numX = 3
|
||||
numY = 3
|
||||
numX = 10
|
||||
numY = 10
|
||||
|
||||
for i in range (numX):
|
||||
for j in range (numY):
|
||||
|
||||
@ -1,4 +1,4 @@
|
||||
import pybullet
|
||||
import pybullet
|
||||
import gym, gym.spaces, gym.utils
|
||||
import numpy as np
|
||||
import os, inspect
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
from robot_bases import XmlBasedRobot, MJCFBasedRobot, URDFBasedRobot
|
||||
import numpy as np
|
||||
import pybullet
|
||||
import pybullet
|
||||
import os
|
||||
import pybullet_data
|
||||
from robot_bases import BodyPart
|
||||
|
||||
@ -3,7 +3,7 @@ from __future__ import absolute_import
|
||||
from __future__ import division
|
||||
import functools
|
||||
import inspect
|
||||
import pybullet
|
||||
import pybullet
|
||||
|
||||
|
||||
class BulletClient(object):
|
||||
|
||||
@ -8055,9 +8055,6 @@ static PyObject* pybullet_executePluginCommand(PyObject* self,
|
||||
return PyInt_FromLong(statusType);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
///Inverse Kinematics binding
|
||||
static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
|
||||
PyObject* args, PyObject* keywds)
|
||||
@ -8077,9 +8074,12 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
|
||||
PyObject* jointRangesObj = 0;
|
||||
PyObject* restPosesObj = 0;
|
||||
PyObject* jointDampingObj = 0;
|
||||
PyObject* currentPositionsObj = 0;
|
||||
int maxNumIterations = -1;
|
||||
double residualThreshold=-1;
|
||||
|
||||
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))
|
||||
static char* kwlist[] = {"bodyUniqueId", "endEffectorLinkIndex", "targetPosition", "targetOrientation", "lowerLimits", "upperLimits", "jointRanges", "restPoses", "jointDamping", "solver", "currentPositions", "maxNumIterations", "residualThreshold", "physicsClientId", NULL};
|
||||
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiO|OOOOOOiOidi", kwlist, &bodyUniqueId, &endEffectorLinkIndex, &targetPosObj, &targetOrnObj, &lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &jointDampingObj, &solver, ¤tPositionsObj, &maxNumIterations, &residualThreshold, &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};
|
||||
@ -8107,53 +8107,69 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
|
||||
int szRestPoses = restPosesObj ? PySequence_Size(restPosesObj) : 0;
|
||||
int szJointDamping = jointDampingObj ? PySequence_Size(jointDampingObj) : 0;
|
||||
|
||||
int szCurrentPositions = currentPositionsObj ? PySequence_Size(currentPositionsObj) : 0;
|
||||
|
||||
|
||||
int numJoints = b3GetNumJoints(sm, bodyUniqueId);
|
||||
int dofCount = b3ComputeDofCount(sm, bodyUniqueId);
|
||||
|
||||
int hasNullSpace = 0;
|
||||
int hasJointDamping = 0;
|
||||
int hasCurrentPositions = 0;
|
||||
double* lowerLimits = 0;
|
||||
double* upperLimits = 0;
|
||||
double* jointRanges = 0;
|
||||
double* restPoses = 0;
|
||||
double* jointDamping = 0;
|
||||
|
||||
if (numJoints && (szLowerLimits == numJoints) && (szUpperLimits == numJoints) &&
|
||||
(szJointRanges == numJoints) && (szRestPoses == numJoints))
|
||||
double* currentPositions = 0;
|
||||
|
||||
if (dofCount && (szLowerLimits == dofCount) && (szUpperLimits == dofCount) &&
|
||||
(szJointRanges == dofCount) && (szRestPoses == dofCount))
|
||||
{
|
||||
int szInBytes = sizeof(double) * numJoints;
|
||||
int szInBytes = sizeof(double) * dofCount;
|
||||
int i;
|
||||
lowerLimits = (double*)malloc(szInBytes);
|
||||
upperLimits = (double*)malloc(szInBytes);
|
||||
jointRanges = (double*)malloc(szInBytes);
|
||||
restPoses = (double*)malloc(szInBytes);
|
||||
|
||||
for (i = 0; i < numJoints; i++)
|
||||
for (i = 0; i < dofCount; i++)
|
||||
{
|
||||
lowerLimits[i] =
|
||||
pybullet_internalGetFloatFromSequence(lowerLimitsObj, i);
|
||||
upperLimits[i] =
|
||||
pybullet_internalGetFloatFromSequence(upperLimitsObj, i);
|
||||
jointRanges[i] =
|
||||
pybullet_internalGetFloatFromSequence(jointRangesObj, i);
|
||||
restPoses[i] =
|
||||
pybullet_internalGetFloatFromSequence(restPosesObj, i);
|
||||
lowerLimits[i] = pybullet_internalGetFloatFromSequence(lowerLimitsObj, i);
|
||||
upperLimits[i] = pybullet_internalGetFloatFromSequence(upperLimitsObj, i);
|
||||
jointRanges[i] = pybullet_internalGetFloatFromSequence(jointRangesObj, i);
|
||||
restPoses[i] = pybullet_internalGetFloatFromSequence(restPosesObj, i);
|
||||
}
|
||||
hasNullSpace = 1;
|
||||
}
|
||||
|
||||
if (szJointDamping > 0)
|
||||
if (szCurrentPositions > 0)
|
||||
{
|
||||
// We allow the number of joint damping values to be larger than
|
||||
// the number of degrees of freedom (DOFs). On the server side, it does
|
||||
// the check and only sets joint damping for all DOFs.
|
||||
// We can use the number of DOFs here when that is exposed. Since the
|
||||
// number of joints is larger than the number of DOFs (we assume the
|
||||
// joint is either fixed joint or one DOF joint), it is safe to use
|
||||
// number of joints here.
|
||||
if (szJointDamping < numJoints)
|
||||
if (szCurrentPositions != dofCount)
|
||||
{
|
||||
PyErr_SetString(SpamError,
|
||||
"calculateInverseKinematics the size of input joint damping values is smaller than the number of joints.");
|
||||
"calculateInverseKinematics the size of input current positions needs to be equal to the number of degrees of freedom.");
|
||||
return NULL;
|
||||
}
|
||||
else
|
||||
{
|
||||
int szInBytes = sizeof(double) * szCurrentPositions;
|
||||
int i;
|
||||
currentPositions = (double*)malloc(szInBytes);
|
||||
for (i = 0; i < szCurrentPositions; i++)
|
||||
{
|
||||
currentPositions[i] = pybullet_internalGetFloatFromSequence(currentPositionsObj, i);
|
||||
}
|
||||
hasCurrentPositions = 1;
|
||||
}
|
||||
}
|
||||
|
||||
if (szJointDamping > 0)
|
||||
{
|
||||
if (szJointDamping != dofCount)
|
||||
{
|
||||
PyErr_SetString(SpamError,
|
||||
"calculateInverseKinematics the size of input joint damping values is unequal to the number of degrees of freedom.");
|
||||
return NULL;
|
||||
}
|
||||
else
|
||||
@ -8179,15 +8195,28 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
|
||||
b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm, bodyUniqueId);
|
||||
b3CalculateInverseKinematicsSelectSolver(command, solver);
|
||||
|
||||
if (hasCurrentPositions)
|
||||
{
|
||||
b3CalculateInverseKinematicsSetCurrentPositions(command, dofCount, currentPositions);
|
||||
}
|
||||
if (maxNumIterations>0)
|
||||
{
|
||||
b3CalculateInverseKinematicsSetMaxNumIterations(command,maxNumIterations);
|
||||
}
|
||||
if (residualThreshold>=0)
|
||||
{
|
||||
b3CalculateInverseKinematicsSetResidualThreshold(command,residualThreshold);
|
||||
}
|
||||
|
||||
if (hasNullSpace)
|
||||
{
|
||||
if (hasOrn)
|
||||
{
|
||||
b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, numJoints, endEffectorLinkIndex, pos, ori, lowerLimits, upperLimits, jointRanges, restPoses);
|
||||
b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, dofCount, endEffectorLinkIndex, pos, ori, lowerLimits, upperLimits, jointRanges, restPoses);
|
||||
}
|
||||
else
|
||||
{
|
||||
b3CalculateInverseKinematicsPosWithNullSpaceVel(command, numJoints, endEffectorLinkIndex, pos, lowerLimits, upperLimits, jointRanges, restPoses);
|
||||
b3CalculateInverseKinematicsPosWithNullSpaceVel(command, dofCount, endEffectorLinkIndex, pos, lowerLimits, upperLimits, jointRanges, restPoses);
|
||||
}
|
||||
}
|
||||
else
|
||||
@ -8204,8 +8233,9 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
|
||||
|
||||
if (hasJointDamping)
|
||||
{
|
||||
b3CalculateInverseKinematicsSetJointDamping(command, numJoints, jointDamping);
|
||||
b3CalculateInverseKinematicsSetJointDamping(command, dofCount, jointDamping);
|
||||
}
|
||||
free(currentPositions);
|
||||
free(jointDamping);
|
||||
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
||||
@ -8294,44 +8324,8 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
|
||||
int szObPos = PySequence_Size(objPositionsQ);
|
||||
int szObVel = PySequence_Size(objVelocitiesQdot);
|
||||
int szObAcc = PySequence_Size(objAccelerations);
|
||||
int nj = b3GetNumJoints(sm, bodyUniqueId);
|
||||
int j=0;
|
||||
int dofCountOrg = 0;
|
||||
for (j=0;j<nj;j++)
|
||||
{
|
||||
struct b3JointInfo info;
|
||||
b3GetJointInfo(sm, bodyUniqueId, j, &info);
|
||||
switch (info.m_jointType)
|
||||
{
|
||||
case eRevoluteType:
|
||||
{
|
||||
dofCountOrg+=1;
|
||||
break;
|
||||
}
|
||||
case ePrismaticType:
|
||||
{
|
||||
dofCountOrg+=1;
|
||||
break;
|
||||
}
|
||||
case eSphericalType:
|
||||
{
|
||||
PyErr_SetString(SpamError,
|
||||
"Spherirical joints are not supported in the pybullet binding");
|
||||
return NULL;
|
||||
}
|
||||
case ePlanarType:
|
||||
{
|
||||
PyErr_SetString(SpamError,
|
||||
"Planar joints are not supported in the pybullet binding");
|
||||
return NULL;
|
||||
}
|
||||
default:
|
||||
{
|
||||
//fixed joint has 0-dof and at the moment, we don't deal with planar, spherical etc
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
int dofCountOrg = b3ComputeDofCount(sm, bodyUniqueId);
|
||||
|
||||
if (dofCountOrg && (szObPos == dofCountOrg) && (szObVel == dofCountOrg) &&
|
||||
(szObAcc == dofCountOrg))
|
||||
|
||||
2
setup.py
2
setup.py
@ -450,7 +450,7 @@ print("-----")
|
||||
|
||||
setup(
|
||||
name = 'pybullet',
|
||||
version='2.0.1',
|
||||
version='2.0.2',
|
||||
description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning',
|
||||
long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.',
|
||||
url='https://github.com/bulletphysics/bullet3',
|
||||
|
||||
@ -37,7 +37,7 @@ bool gJointFeedbackInJointFrame = false;
|
||||
|
||||
namespace {
|
||||
const btScalar SLEEP_EPSILON = btScalar(0.05); // this is a squared velocity (m^2 s^-2)
|
||||
const btScalar SLEEP_TIMEOUT = btScalar(2); // in seconds
|
||||
const btScalar SLEEP_TIMEOUT = btScalar(0.3); // in seconds
|
||||
}
|
||||
|
||||
namespace {
|
||||
|
||||
@ -485,7 +485,7 @@ void addJointTorque(int i, btScalar Q);
|
||||
}
|
||||
void setCompanionId(int id)
|
||||
{
|
||||
//printf("for %p setCompanionId(%d)\n",this, id);
|
||||
printf("for %p setCompanionId(%d)\n",this, id);
|
||||
m_companionId = id;
|
||||
}
|
||||
|
||||
|
||||
@ -26,6 +26,7 @@ subject to the following restrictions:
|
||||
|
||||
btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
|
||||
{
|
||||
printf("btSequentialImpulseConstraintSolver::solveSingleIteration\n");
|
||||
btScalar leastSquaredResidual = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
|
||||
|
||||
//solve featherstone non-contact constraints
|
||||
@ -38,6 +39,7 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
|
||||
|
||||
btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[index];
|
||||
|
||||
printf("m_multiBodyNonContactConstraints resolveSingleConstraintRowGeneric\n");
|
||||
btScalar residual = resolveSingleConstraintRowGeneric(constraint);
|
||||
leastSquaredResidual += residual*residual;
|
||||
|
||||
@ -46,7 +48,8 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
|
||||
if(constraint.m_multiBodyB)
|
||||
constraint.m_multiBodyB->setPosUpdated(false);
|
||||
}
|
||||
|
||||
printf("featherstone normal contact\n");
|
||||
printf("numContact=%d\n", m_multiBodyNormalContactConstraints.size());
|
||||
//solve featherstone normal contact
|
||||
for (int j0=0;j0<m_multiBodyNormalContactConstraints.size();j0++)
|
||||
{
|
||||
@ -57,6 +60,7 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
|
||||
|
||||
if (iteration < infoGlobal.m_numIterations)
|
||||
{
|
||||
printf("featherstone normal contact resolveSingleConstraintRowGeneric\n");
|
||||
residual = resolveSingleConstraintRowGeneric(constraint);
|
||||
}
|
||||
|
||||
@ -68,6 +72,7 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
|
||||
constraint.m_multiBodyB->setPosUpdated(false);
|
||||
}
|
||||
|
||||
printf("featherstone frictional contact\n");
|
||||
//solve featherstone frictional contact
|
||||
if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode&SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) == 0))
|
||||
{
|
||||
@ -158,6 +163,7 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
|
||||
}
|
||||
}
|
||||
}
|
||||
printf("end featjerstp\n");
|
||||
return leastSquaredResidual;
|
||||
}
|
||||
|
||||
@ -203,6 +209,19 @@ btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const bt
|
||||
int ndofA = 0;
|
||||
int ndofB = 0;
|
||||
|
||||
printf("c.m_solverBodyIdA=%d\n", c.m_solverBodyIdA);
|
||||
printf("c.m_solverBodyIdB=%d\n", c.m_solverBodyIdB);
|
||||
|
||||
printf("c.m_multiBodyA=%p\n", c.m_multiBodyA);
|
||||
printf("c.m_multiBodyB=%p\n", c.m_multiBodyB);
|
||||
printf("c.m_jacAindex=%d\n",c.m_jacAindex);
|
||||
printf("c.m_jacBindex=%d\n",c.m_jacBindex);
|
||||
printf("c.m_deltaVelAindex=%d\n",c.m_deltaVelAindex);
|
||||
printf("c.m_deltaVelBindex=%d\n",c.m_deltaVelBindex);
|
||||
if (c.m_deltaVelAindex>500)
|
||||
{
|
||||
printf("???\n");
|
||||
}
|
||||
if (c.m_multiBodyA)
|
||||
{
|
||||
ndofA = c.m_multiBodyA->getNumDofs() + 6;
|
||||
@ -539,7 +558,13 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
|
||||
}
|
||||
const int ndofA = multiBodyA->getNumDofs() + 6;
|
||||
|
||||
|
||||
solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
|
||||
if (solverConstraint.m_deltaVelAindex>10000)
|
||||
{
|
||||
printf("????????????????????????????????????????\n");
|
||||
printf("solverConstraint.m_deltaVelAindex==%d\n", solverConstraint.m_deltaVelAindex);
|
||||
}
|
||||
|
||||
if (solverConstraint.m_deltaVelAindex <0)
|
||||
{
|
||||
@ -1657,7 +1682,9 @@ void btMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject** bodie
|
||||
//printf("solveMultiBodyGroup start\n");
|
||||
m_tmpMultiBodyConstraints = multiBodyConstraints;
|
||||
m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
|
||||
|
||||
|
||||
printf("m_tmpNumMultiBodyConstraints =%d\n",m_tmpNumMultiBodyConstraints );
|
||||
printf("solveGroup\n");
|
||||
btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
|
||||
|
||||
m_tmpMultiBodyConstraints = 0;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user