PyBullet.changeDynamics: expose jointDamping
PyBullet: Implement a few more APIs of PhysX backend, resetJointState and setJointMotorControl2 allow useMaximalCoordinate=True for PhysX loadURDF (only for single rigid bodies, articulations require reduced coordinates at the moment)
This commit is contained in:
@@ -2831,6 +2831,18 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetAngularDamping(b3SharedMemoryCommandHan
|
||||
return 0;
|
||||
}
|
||||
|
||||
B3_SHARED_API int b3ChangeDynamicsInfoSetJointDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double jointDamping)
|
||||
{
|
||||
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
|
||||
b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
|
||||
command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
|
||||
command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
|
||||
command->m_changeDynamicsInfoArgs.m_jointDamping = jointDamping;
|
||||
command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_JOINT_DAMPING;
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
B3_SHARED_API int b3ChangeDynamicsInfoSetContactStiffnessAndDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double contactStiffness, double contactDamping)
|
||||
{
|
||||
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
|
||||
|
||||
@@ -155,6 +155,8 @@ extern "C"
|
||||
B3_SHARED_API int b3ChangeDynamicsInfoSetRestitution(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double restitution);
|
||||
B3_SHARED_API int b3ChangeDynamicsInfoSetLinearDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, double linearDamping);
|
||||
B3_SHARED_API int b3ChangeDynamicsInfoSetAngularDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, double angularDamping);
|
||||
B3_SHARED_API int b3ChangeDynamicsInfoSetJointDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double jointDamping);
|
||||
|
||||
B3_SHARED_API int b3ChangeDynamicsInfoSetContactStiffnessAndDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double contactStiffness, double contactDamping);
|
||||
B3_SHARED_API int b3ChangeDynamicsInfoSetFrictionAnchor(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, int frictionAnchor);
|
||||
B3_SHARED_API int b3ChangeDynamicsInfoSetCcdSweptSphereRadius(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double ccdSweptSphereRadius);
|
||||
|
||||
@@ -7737,6 +7737,12 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
|
||||
mb->getLinkCollider(linkIndex)->setContactStiffnessAndDamping(clientCmd.m_changeDynamicsInfoArgs.m_contactStiffness, clientCmd.m_changeDynamicsInfoArgs.m_contactDamping);
|
||||
}
|
||||
}
|
||||
|
||||
if (clientCmd.m_updateFlags &CHANGE_DYNAMICS_INFO_SET_JOINT_DAMPING)
|
||||
{
|
||||
mb->getLink(linkIndex).m_jointDamping = clientCmd.m_changeDynamicsInfoArgs.m_jointDamping;
|
||||
}
|
||||
|
||||
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS)
|
||||
{
|
||||
mb->getLink(linkIndex).m_mass = mass;
|
||||
@@ -8057,6 +8063,7 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
|
||||
|
||||
BT_PROFILE("CMD_SEND_PHYSICS_SIMULATION_PARAMETERS");
|
||||
|
||||
|
||||
if (clientCmd.m_updateFlags & SIM_PARAM_ENABLE_CONE_FRICTION)
|
||||
{
|
||||
if (clientCmd.m_physSimParamArgs.m_enableConeFriction)
|
||||
|
||||
@@ -164,6 +164,7 @@ enum EnumChangeDynamicsInfoFlags
|
||||
CHANGE_DYNAMICS_INFO_SET_CCD_SWEPT_SPHERE_RADIUS = 2048,
|
||||
CHANGE_DYNAMICS_INFO_SET_CONTACT_PROCESSING_THRESHOLD = 4096,
|
||||
CHANGE_DYNAMICS_INFO_SET_ACTIVATION_STATE = 8192,
|
||||
CHANGE_DYNAMICS_INFO_SET_JOINT_DAMPING = 16384,
|
||||
};
|
||||
|
||||
struct ChangeDynamicsInfoArgs
|
||||
@@ -185,6 +186,7 @@ struct ChangeDynamicsInfoArgs
|
||||
double m_ccdSweptSphereRadius;
|
||||
double m_contactProcessingThreshold;
|
||||
int m_activationState;
|
||||
double m_jointDamping;
|
||||
};
|
||||
|
||||
struct GetDynamicsInfoArgs
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -16,6 +16,11 @@ class PhysXServerCommandProcessor : public PhysicsCommandProcessorInterface
|
||||
bool processSendPhysicsParametersCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
|
||||
bool processRequestActualStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
|
||||
bool processResetSimulationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
|
||||
bool processInitPoseCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
|
||||
bool processSendDesiredStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
|
||||
bool processChangeDynamicsInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
|
||||
bool processRequestPhysicsSimulationParametersCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
|
||||
|
||||
|
||||
bool processCustomCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
|
||||
|
||||
|
||||
@@ -16,11 +16,15 @@
|
||||
#include "PxMaterial.h"
|
||||
#include "PxCooking.h"
|
||||
#include "PxScene.h"
|
||||
#include "PxRigidStatic.h"
|
||||
#include "PxRigidDynamic.h"
|
||||
#include "PxActor.h"
|
||||
#include "PxAggregate.h"
|
||||
|
||||
#include "Bullet3Common/b3FileUtils.h"
|
||||
#include "PhysXUserData.h"
|
||||
#include "../../CommonInterfaces/CommonFileIOInterface.h"
|
||||
|
||||
#include "../../Importers/ImportObjDemo/LoadMeshFromObj.h"
|
||||
#include "../../Importers/ImportSTLDemo/LoadMeshFromSTL.h"
|
||||
#include "Importers/ImportURDFDemo/UrdfParser.h"
|
||||
|
||||
|
||||
@@ -31,6 +35,8 @@ struct URDF2PhysXCachedData
|
||||
URDF2PhysXCachedData()
|
||||
: m_currentMultiBodyLinkIndex(-1),
|
||||
m_articulation(0),
|
||||
m_rigidStatic(0),
|
||||
m_rigidDynamic(0),
|
||||
m_totalNumJoints1(0)
|
||||
{
|
||||
}
|
||||
@@ -44,6 +50,8 @@ struct URDF2PhysXCachedData
|
||||
int m_currentMultiBodyLinkIndex;
|
||||
|
||||
physx::PxArticulationReducedCoordinate* m_articulation;
|
||||
physx::PxRigidStatic* m_rigidStatic;
|
||||
physx::PxRigidDynamic* m_rigidDynamic;
|
||||
|
||||
btAlignedObjectArray<physx::PxTransform> m_linkTransWorldSpace;
|
||||
btAlignedObjectArray<int> m_urdfLinkIndex;
|
||||
@@ -237,7 +245,7 @@ static physx::PxConvexMesh* createPhysXConvex(physx::PxU32 numVerts, const physx
|
||||
|
||||
|
||||
int convertLinkPhysXShapes(const URDFImporterInterface& u2b, URDF2PhysXCachedData& cache, ArticulationCreationInterface& creation, int urdfLinkIndex, const char* pathPrefix, const btTransform& localInertiaFrame,
|
||||
physx::PxArticulationReducedCoordinate* articulation, int mbLinkIndex, physx::PxArticulationLink* linkPtr)
|
||||
physx::PxArticulationReducedCoordinate* articulation, int mbLinkIndex, physx::PxRigidActor* linkPtr)
|
||||
{
|
||||
int numShapes = 0;
|
||||
|
||||
@@ -362,9 +370,61 @@ int convertLinkPhysXShapes(const URDFImporterInterface& u2b, URDF2PhysXCachedDat
|
||||
numShapes++;
|
||||
break;
|
||||
}
|
||||
case URDF_GEOM_MESH:
|
||||
{
|
||||
btAlignedObjectArray<physx::PxVec3> vertices;
|
||||
GLInstanceGraphicsShape* glmesh = 0;
|
||||
switch (collision->m_geometry.m_meshFileType)
|
||||
{
|
||||
case UrdfGeometry::FILE_OBJ:
|
||||
{
|
||||
char relativeFileName[1024];
|
||||
char pathPrefix[1024];
|
||||
pathPrefix[0] = 0;
|
||||
if (creation.m_fileIO->findResourcePath(collision->m_geometry.m_meshFileName.c_str(), relativeFileName, 1024))
|
||||
{
|
||||
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
|
||||
}
|
||||
glmesh = LoadMeshFromObj(collision->m_geometry.m_meshFileName.c_str(), pathPrefix, creation.m_fileIO);
|
||||
|
||||
break;
|
||||
}
|
||||
case UrdfGeometry::FILE_STL:
|
||||
{
|
||||
glmesh = LoadMeshFromSTL(collision->m_geometry.m_meshFileName.c_str(), creation.m_fileIO);
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
|
||||
}
|
||||
}
|
||||
if (glmesh && glmesh->m_numvertices)
|
||||
{
|
||||
for (int i = 0; i < glmesh->m_numvertices; i++)
|
||||
{
|
||||
physx::PxVec3 vert(
|
||||
glmesh->m_vertices->at(i).xyzw[0] * collision->m_geometry.m_meshScale[0],
|
||||
glmesh->m_vertices->at(i).xyzw[1] * collision->m_geometry.m_meshScale[1],
|
||||
glmesh->m_vertices->at(i).xyzw[2] * collision->m_geometry.m_meshScale[2]);
|
||||
vertices.push_back(vert);
|
||||
}
|
||||
|
||||
physx::PxConvexMesh* convexMesh = createPhysXConvex(vertices.size(), &vertices[0], creation);
|
||||
|
||||
shape = physx::PxRigidActorExt::createExclusiveShape(*linkPtr,
|
||||
physx::PxConvexMeshGeometry(convexMesh), *material);
|
||||
|
||||
shape->setLocalPose(tr);
|
||||
cache.m_geomLocalPoses.push_back(tr);
|
||||
numShapes++;
|
||||
}
|
||||
break;
|
||||
|
||||
}
|
||||
default:
|
||||
{
|
||||
printf("unknown physx shape\n");
|
||||
}
|
||||
}
|
||||
|
||||
@@ -390,7 +450,7 @@ btTransform ConvertURDF2PhysXInternal(
|
||||
ArticulationCreationInterface& creation,
|
||||
URDF2PhysXCachedData& cache, int urdfLinkIndex,
|
||||
const btTransform& parentTransformInWorldSpace,
|
||||
bool createMultiBody, const char* pathPrefix,
|
||||
bool createActiculation, const char* pathPrefix,
|
||||
int flags,
|
||||
UrdfVisualShapeCache2* cachedLinkGraphicsShapesIn,
|
||||
UrdfVisualShapeCache2* cachedLinkGraphicsShapesOut,
|
||||
@@ -562,17 +622,44 @@ btTransform ConvertURDF2PhysXInternal(
|
||||
btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace * localInertialFrame;
|
||||
bool canSleep = (flags & CUF_ENABLE_SLEEPING) != 0;
|
||||
|
||||
physx::PxArticulationLink* linkPtr = 0;
|
||||
physx::PxRigidActor* linkPtr = 0;
|
||||
|
||||
physx::PxRigidBody* rbLinkPtr = 0;
|
||||
|
||||
if (!createMultiBody)
|
||||
{
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
physx::PxTransform tr;
|
||||
tr.p = physx::PxVec3(linkTransformInWorldSpace.getOrigin().x(), linkTransformInWorldSpace.getOrigin().y(), linkTransformInWorldSpace.getOrigin().z());
|
||||
tr.q = physx::PxQuat(linkTransformInWorldSpace.getRotation().x(), linkTransformInWorldSpace.getRotation().y(), linkTransformInWorldSpace.getRotation().z(), linkTransformInWorldSpace.getRotation().w());
|
||||
bool isFixedBase = (mass == 0); //todo: figure out when base is fixed
|
||||
|
||||
|
||||
if (!createActiculation)
|
||||
{
|
||||
|
||||
if (isFixedBase)
|
||||
{
|
||||
physx::PxRigidStatic* s = creation.m_physics->createRigidStatic(tr);
|
||||
if ((cache.m_rigidStatic == 0) && (cache.m_rigidDynamic == 0))
|
||||
{
|
||||
cache.m_rigidStatic = s;
|
||||
}
|
||||
linkPtr = s;
|
||||
}
|
||||
else
|
||||
{
|
||||
|
||||
physx::PxRigidDynamic* d = creation.m_physics->createRigidDynamic(tr);
|
||||
linkPtr = d;
|
||||
if ((cache.m_rigidStatic == 0) && (cache.m_rigidDynamic == 0))
|
||||
{
|
||||
cache.m_rigidDynamic = d;
|
||||
}
|
||||
rbLinkPtr = d;
|
||||
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
|
||||
cache.m_linkTransWorldSpace.push_back(tr);
|
||||
cache.m_urdfLinkIndex.push_back(urdfLinkIndex);
|
||||
@@ -582,7 +669,7 @@ btTransform ConvertURDF2PhysXInternal(
|
||||
if (cache.m_articulation == 0)
|
||||
{
|
||||
|
||||
bool isFixedBase = (mass == 0); //todo: figure out when base is fixed
|
||||
|
||||
|
||||
cache.m_articulation = creation.m_physics->createArticulationReducedCoordinate();
|
||||
|
||||
@@ -596,6 +683,7 @@ btTransform ConvertURDF2PhysXInternal(
|
||||
physx::PxArticulationLink* base = cache.m_articulation->createLink(NULL,tr);
|
||||
|
||||
linkPtr = base;
|
||||
rbLinkPtr = base;
|
||||
|
||||
|
||||
//physx::PxRigidActorExt::createExclusiveShape(*base, PxBoxGeometry(0.5f, 0.25f, 1.5f), *gMaterial);
|
||||
@@ -628,14 +716,19 @@ btTransform ConvertURDF2PhysXInternal(
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
cache.registerMultiBody(urdfLinkIndex, base, inertialFrameInWorldSpace, mass, localInertiaDiagonal, localInertialFrame);
|
||||
}
|
||||
else
|
||||
{
|
||||
|
||||
physx::PxArticulationLink* parentLink = cache.getPhysxLinkFromLink(urdfParentIndex);
|
||||
linkPtr = cache.m_articulation->createLink(parentLink, tr);
|
||||
|
||||
physx::PxArticulationJointReducedCoordinate* joint = static_cast<physx::PxArticulationJointReducedCoordinate*>(linkPtr->getInboundJoint());
|
||||
physx::PxArticulationLink* childLink = cache.m_articulation->createLink(parentLink, tr);
|
||||
linkPtr = childLink;
|
||||
rbLinkPtr = childLink;
|
||||
|
||||
physx::PxArticulationJointReducedCoordinate* joint = static_cast<physx::PxArticulationJointReducedCoordinate*>(childLink->getInboundJoint());
|
||||
|
||||
switch (jointType)
|
||||
{
|
||||
@@ -691,8 +784,13 @@ btTransform ConvertURDF2PhysXInternal(
|
||||
joint->setParentPose(parentPose);
|
||||
joint->setChildPose(childPose);
|
||||
|
||||
cache.registerMultiBody(urdfLinkIndex, childLink, inertialFrameInWorldSpace, mass, localInertiaDiagonal, localInertialFrame);
|
||||
}
|
||||
cache.registerMultiBody(urdfLinkIndex, linkPtr, inertialFrameInWorldSpace, mass, localInertiaDiagonal, localInertialFrame);
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
if (linkPtr)
|
||||
{
|
||||
//todo: mem leaks
|
||||
@@ -700,8 +798,6 @@ btTransform ConvertURDF2PhysXInternal(
|
||||
userData->m_graphicsUniqueId = graphicsIndex;
|
||||
linkPtr->userData = userData;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//create collision shapes
|
||||
|
||||
@@ -709,7 +805,7 @@ btTransform ConvertURDF2PhysXInternal(
|
||||
convertLinkPhysXShapes(u2b, cache, creation, urdfLinkIndex, pathPrefix, localInertialFrame, cache.m_articulation, mbLinkIndex, linkPtr);
|
||||
|
||||
|
||||
physx::PxRigidBodyExt::updateMassAndInertia(*linkPtr, mass);
|
||||
physx::PxRigidBodyExt::updateMassAndInertia(*rbLinkPtr, mass);
|
||||
|
||||
|
||||
//base->setMass(massOut);
|
||||
@@ -727,7 +823,7 @@ btTransform ConvertURDF2PhysXInternal(
|
||||
|
||||
bool disableParentCollision = true;
|
||||
|
||||
if (createMultiBody && cache.m_articulation)
|
||||
if (createActiculation && cache.m_articulation)
|
||||
{
|
||||
#if 0
|
||||
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointDamping = jointDamping;
|
||||
@@ -741,7 +837,7 @@ btTransform ConvertURDF2PhysXInternal(
|
||||
}
|
||||
|
||||
|
||||
if (createMultiBody)
|
||||
if (createActiculation)
|
||||
{
|
||||
|
||||
}
|
||||
@@ -762,7 +858,7 @@ btTransform ConvertURDF2PhysXInternal(
|
||||
{
|
||||
int urdfChildLinkIndex = urdfChildIndices[i];
|
||||
|
||||
ConvertURDF2PhysXInternal(u2b, creation, cache, urdfChildLinkIndex, linkTransformInWorldSpace, createMultiBody, pathPrefix, flags, cachedLinkGraphicsShapesIn, cachedLinkGraphicsShapesOut, recursive);
|
||||
ConvertURDF2PhysXInternal(u2b, creation, cache, urdfChildLinkIndex, linkTransformInWorldSpace, createActiculation, pathPrefix, flags, cachedLinkGraphicsShapesIn, cachedLinkGraphicsShapesOut, recursive);
|
||||
}
|
||||
}
|
||||
return linkTransformInWorldSpace;
|
||||
@@ -774,7 +870,7 @@ btTransform ConvertURDF2PhysXInternal(
|
||||
|
||||
|
||||
|
||||
physx::PxArticulationReducedCoordinate* URDF2PhysX(physx::PxFoundation* foundation, physx::PxPhysics* physics, physx::PxCooking* cooking, physx::PxScene* scene, class PhysXURDFImporter& u2p, int flags, const char* pathPrefix, const btTransform& rootTransformInWorldSpace, struct CommonFileIOInterface* fileIO)
|
||||
physx::PxBase* URDF2PhysX(physx::PxFoundation* foundation, physx::PxPhysics* physics, physx::PxCooking* cooking, physx::PxScene* scene, class PhysXURDFImporter& u2p, int flags, const char* pathPrefix, const btTransform& rootTransformInWorldSpace, struct CommonFileIOInterface* fileIO, bool createActiculation)
|
||||
{
|
||||
URDF2PhysXCachedData cache;
|
||||
InitURDF2BulletCache(u2p, cache, flags);
|
||||
@@ -792,13 +888,13 @@ physx::PxArticulationReducedCoordinate* URDF2PhysX(physx::PxFoundation* foundati
|
||||
creation.m_scene = scene;
|
||||
creation.m_fileIO = fileIO;
|
||||
|
||||
bool createMultiBody = true;
|
||||
|
||||
|
||||
bool recursive = (flags & CUF_MAINTAIN_LINK_ORDER) == 0;
|
||||
|
||||
if (recursive)
|
||||
{
|
||||
ConvertURDF2PhysXInternal(u2p, creation, cache, urdfLinkIndex, rootTransformInWorldSpace, createMultiBody, pathPrefix, flags, &cachedLinkGraphicsShapes, &cachedLinkGraphicsShapesOut, recursive);
|
||||
ConvertURDF2PhysXInternal(u2p, creation, cache, urdfLinkIndex, rootTransformInWorldSpace, createActiculation, pathPrefix, flags, &cachedLinkGraphicsShapes, &cachedLinkGraphicsShapesOut, recursive);
|
||||
|
||||
}
|
||||
else
|
||||
@@ -820,7 +916,7 @@ physx::PxArticulationReducedCoordinate* URDF2PhysX(physx::PxFoundation* foundati
|
||||
int urdfLinkIndex = allIndices[i].m_index;
|
||||
int parentIndex = allIndices[i].m_parentIndex;
|
||||
btTransform parentTr = parentIndex >= 0 ? parentTransforms[parentIndex] : rootTransformInWorldSpace;
|
||||
btTransform tr = ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex, parentTr, world1, createMultiBody, pathPrefix, flags, cachedLinkGraphicsShapes, &cachedLinkGraphicsShapesOut, recursive);
|
||||
btTransform tr = ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex, parentTr, world1, createActiculation, pathPrefix, flags, cachedLinkGraphicsShapes, &cachedLinkGraphicsShapesOut, recursive);
|
||||
if ((urdfLinkIndex + 1) >= parentTransforms.size())
|
||||
{
|
||||
parentTransforms.resize(urdfLinkIndex + 1);
|
||||
@@ -838,7 +934,7 @@ physx::PxArticulationReducedCoordinate* URDF2PhysX(physx::PxFoundation* foundati
|
||||
#endif
|
||||
|
||||
|
||||
if (scene && cache.m_articulation)
|
||||
if (cache.m_articulation)
|
||||
{
|
||||
#ifdef DEBUG_ARTICULATIONS
|
||||
printf("\n-----------------\n");
|
||||
@@ -902,7 +998,19 @@ physx::PxArticulationReducedCoordinate* URDF2PhysX(physx::PxFoundation* foundati
|
||||
scene->addArticulation(*cache.m_articulation);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
return cache.m_articulation;
|
||||
}
|
||||
|
||||
if (cache.m_rigidStatic)
|
||||
{
|
||||
scene->addActor(*cache.m_rigidStatic);
|
||||
return cache.m_rigidStatic;
|
||||
}
|
||||
if (cache.m_rigidDynamic)
|
||||
{
|
||||
scene->addActor(*cache.m_rigidDynamic);
|
||||
return cache.m_rigidDynamic;
|
||||
}
|
||||
return NULL;
|
||||
|
||||
}
|
||||
|
||||
@@ -6,6 +6,7 @@
|
||||
|
||||
namespace physx
|
||||
{
|
||||
class PxBase;
|
||||
class PxFoundation;
|
||||
class PxPhysics;
|
||||
class PxDefaultCpuDispatcher;
|
||||
@@ -20,6 +21,6 @@ struct UrdfVisualShapeCache2
|
||||
b3AlignedObjectArray<int> m_cachedUrdfLinkVisualShapeIndices;
|
||||
};
|
||||
|
||||
physx::PxArticulationReducedCoordinate* URDF2PhysX(physx::PxFoundation* foundation, physx::PxPhysics* physics, physx::PxCooking* cooking, physx::PxScene* scene, class PhysXURDFImporter& u2p, int flags, const char* pathPrefix, const class btTransform& rootTransformInWorldSpace,struct CommonFileIOInterface* fileIO);
|
||||
physx::PxBase* URDF2PhysX(physx::PxFoundation* foundation, physx::PxPhysics* physics, physx::PxCooking* cooking, physx::PxScene* scene, class PhysXURDFImporter& u2p, int flags, const char* pathPrefix, const class btTransform& rootTransformInWorldSpace,struct CommonFileIOInterface* fileIO, bool createActiculation);
|
||||
|
||||
#endif //URDF2PHYSX_H
|
||||
@@ -1,14 +1,43 @@
|
||||
import pybullet as p
|
||||
import time
|
||||
import math
|
||||
|
||||
p.connect(p.PhysX)
|
||||
p.connect(p.PhysX)#GUI)
|
||||
p.loadPlugin("eglRendererPlugin")
|
||||
|
||||
p.loadURDF("plane.urdf")
|
||||
for i in range (50):
|
||||
p.loadURDF("r2d2.urdf",[0,0,1+i*2])
|
||||
|
||||
for i in range (10):
|
||||
sphere = p.loadURDF("marble_cube.urdf",[0,-1,1+i*1], useMaximalCoordinates=False)
|
||||
p.changeDynamics(sphere ,-1, mass=1000)
|
||||
|
||||
door = p.loadURDF("door.urdf",[0,0,1])
|
||||
p.changeDynamics(door ,1, linearDamping=0, angularDamping=0, jointDamping=0, mass=1)
|
||||
print("numJoints = ", p.getNumJoints(door))
|
||||
|
||||
|
||||
p.setGravity(0,0,-10)
|
||||
position_control = True
|
||||
angle = math.pi*0.25
|
||||
p.resetJointState(door,1,angle)
|
||||
|
||||
prevTime = time.time()
|
||||
|
||||
angle = math.pi*0.5
|
||||
|
||||
while (1):
|
||||
|
||||
curTime = time.time()
|
||||
|
||||
diff = curTime - prevTime
|
||||
#every second, swap target angle
|
||||
if (diff>1):
|
||||
angle = - angle
|
||||
prevTime = curTime
|
||||
|
||||
if position_control:
|
||||
p.setJointMotorControl2(door,1,p.POSITION_CONTROL, targetPosition = angle, positionGain=10.1, velocityGain=1, force=11.001)
|
||||
else:
|
||||
p.setJointMotorControl2(door,1,p.VELOCITY_CONTROL, targetVelocity=1, force=1011)
|
||||
p.stepSimulation()
|
||||
time.sleep(1./240.)
|
||||
@@ -1233,20 +1233,21 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO
|
||||
double restitution = -1;
|
||||
double linearDamping = -1;
|
||||
double angularDamping = -1;
|
||||
|
||||
double contactStiffness = -1;
|
||||
double contactDamping = -1;
|
||||
double ccdSweptSphereRadius = -1;
|
||||
int frictionAnchor = -1;
|
||||
double contactProcessingThreshold = -1;
|
||||
int activationState = -1;
|
||||
|
||||
double jointDamping = -1;
|
||||
PyObject* localInertiaDiagonalObj = 0;
|
||||
|
||||
b3PhysicsClientHandle sm = 0;
|
||||
|
||||
int physicsClientId = 0;
|
||||
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction", "restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "localInertiaDiagonal", "ccdSweptSphereRadius", "contactProcessingThreshold", "activationState", "physicsClientId", NULL};
|
||||
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddiOddii", kwlist, &bodyUniqueId, &linkIndex, &mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution, &linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &localInertiaDiagonalObj, &ccdSweptSphereRadius, &contactProcessingThreshold, &activationState, &physicsClientId))
|
||||
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction", "restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "localInertiaDiagonal", "ccdSweptSphereRadius", "contactProcessingThreshold", "activationState", "jointDamping", "physicsClientId", NULL};
|
||||
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddiOddidi", kwlist, &bodyUniqueId, &linkIndex, &mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution, &linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &localInertiaDiagonalObj, &ccdSweptSphereRadius, &contactProcessingThreshold, &activationState, &jointDamping, &physicsClientId))
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
@@ -1300,6 +1301,10 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO
|
||||
b3ChangeDynamicsInfoSetAngularDamping(command, bodyUniqueId, angularDamping);
|
||||
}
|
||||
|
||||
if (jointDamping >= 0)
|
||||
{
|
||||
b3ChangeDynamicsInfoSetJointDamping(command, bodyUniqueId, linkIndex, jointDamping);
|
||||
}
|
||||
if (restitution >= 0)
|
||||
{
|
||||
b3ChangeDynamicsInfoSetRestitution(command, bodyUniqueId, linkIndex, restitution);
|
||||
|
||||
Reference in New Issue
Block a user