Merge remote-tracking branch 'bp/master'
This commit is contained in:
@@ -1208,46 +1208,71 @@ int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyId)
|
||||
return cl->getNumJoints(bodyId);
|
||||
}
|
||||
|
||||
|
||||
int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info)
|
||||
{
|
||||
PhysicsClient* cl = (PhysicsClient* ) physClient;
|
||||
return cl->getJointInfo(bodyIndex, jointIndex, *info);
|
||||
}
|
||||
|
||||
b3SharedMemoryCommandHandle b3InitResetDynamicInfo(b3PhysicsClientHandle physClient)
|
||||
b3SharedMemoryCommandHandle b3GetDynamicsInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex)
|
||||
{
|
||||
PhysicsClient* cl = (PhysicsClient* ) physClient;
|
||||
b3Assert(cl);
|
||||
b3Assert(cl->canSubmitCommand());
|
||||
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
|
||||
b3Assert(command);
|
||||
command->m_type = CMD_RESET_DYNAMIC_INFO;
|
||||
command->m_type = CMD_GET_DYNAMICS_INFO;
|
||||
command->m_getDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
|
||||
command->m_getDynamicsInfoArgs.m_linkIndex = linkIndex;
|
||||
return (b3SharedMemoryCommandHandle) command;
|
||||
}
|
||||
|
||||
int b3GetDynamicsInfo(b3SharedMemoryStatusHandle statusHandle, struct b3DynamicsInfo* info)
|
||||
{
|
||||
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
|
||||
const b3DynamicsInfo &dynamicsInfo = status->m_dynamicsInfo;
|
||||
btAssert(status->m_type == CMD_GET_DYNAMICS_INFO);
|
||||
if (status->m_type != CMD_GET_DYNAMICS_INFO_COMPLETED)
|
||||
return false;
|
||||
|
||||
info->m_mass = dynamicsInfo.m_mass;
|
||||
info->m_lateralFrictionCoeff = dynamicsInfo.m_lateralFrictionCoeff;
|
||||
return true;
|
||||
}
|
||||
|
||||
b3SharedMemoryCommandHandle b3InitChangeDynamicsInfo(b3PhysicsClientHandle physClient)
|
||||
{
|
||||
PhysicsClient* cl = (PhysicsClient* ) physClient;
|
||||
b3Assert(cl);
|
||||
b3Assert(cl->canSubmitCommand());
|
||||
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
|
||||
b3Assert(command);
|
||||
command->m_type = CMD_CHANGE_DYNAMICS_INFO;
|
||||
command->m_updateFlags = 0;
|
||||
|
||||
return (b3SharedMemoryCommandHandle) command;
|
||||
}
|
||||
|
||||
int b3ResetDynamicInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass)
|
||||
int b3ChangeDynamicsInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass)
|
||||
{
|
||||
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
|
||||
b3Assert(command->m_type == CMD_RESET_DYNAMIC_INFO);
|
||||
b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
|
||||
b3Assert(mass > 0);
|
||||
command->m_resetDynamicInfoArgs.m_bodyUniqueId = bodyUniqueId;
|
||||
command->m_resetDynamicInfoArgs.m_linkIndex = linkIndex;
|
||||
command->m_resetDynamicInfoArgs.m_mass = mass;
|
||||
command->m_updateFlags |= RESET_DYNAMIC_INFO_SET_MASS;
|
||||
command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
|
||||
command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
|
||||
command->m_changeDynamicsInfoArgs.m_mass = mass;
|
||||
command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_MASS;
|
||||
return 0;
|
||||
}
|
||||
|
||||
int b3ResetDynamicInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction)
|
||||
int b3ChangeDynamicsInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction)
|
||||
{
|
||||
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
|
||||
b3Assert(command->m_type == CMD_RESET_DYNAMIC_INFO);
|
||||
command->m_resetDynamicInfoArgs.m_bodyUniqueId = bodyUniqueId;
|
||||
command->m_resetDynamicInfoArgs.m_linkIndex = linkIndex;
|
||||
command->m_resetDynamicInfoArgs.m_lateralFriction = lateralFriction;
|
||||
command->m_updateFlags |= RESET_DYNAMIC_INFO_SET_LATERAL_FRICTION;
|
||||
b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
|
||||
command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
|
||||
command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
|
||||
command->m_changeDynamicsInfoArgs.m_lateralFriction = lateralFriction;
|
||||
command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION;
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
@@ -77,12 +77,13 @@ int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex);
|
||||
///given a body and joint index, return the joint information. See b3JointInfo in SharedMemoryPublic.h
|
||||
int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info);
|
||||
|
||||
///given a body unique id and link index, return the dynamic information. See b3DynamicInfo in SharedMemoryPublic.h
|
||||
int b3GetDynamicInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, struct b3DynamicInfo* info);
|
||||
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
|
||||
int b3GetDynamicsInfo(b3SharedMemoryStatusHandle statusHandle, struct b3DynamicsInfo* info);
|
||||
|
||||
b3SharedMemoryCommandHandle b3InitResetDynamicInfo(b3PhysicsClientHandle physClient);
|
||||
int b3ResetDynamicInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass);
|
||||
int b3ResetDynamicInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction);
|
||||
b3SharedMemoryCommandHandle b3InitChangeDynamicsInfo(b3PhysicsClientHandle physClient);
|
||||
int b3ChangeDynamicsInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass);
|
||||
int b3ChangeDynamicsInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction);
|
||||
|
||||
b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info);
|
||||
|
||||
|
||||
@@ -54,6 +54,9 @@ protected:
|
||||
int m_canvasDepthIndex;
|
||||
int m_canvasSegMaskIndex;
|
||||
|
||||
btScalar m_lightPos[3];
|
||||
btScalar m_specularCoeff;
|
||||
|
||||
void createButton(const char* name, int id, bool isTrigger );
|
||||
|
||||
void createButtons();
|
||||
@@ -278,6 +281,12 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
|
||||
|
||||
b3RequestCameraImageSetCameraMatrices(commandHandle, viewMatrix,projectionMatrix);
|
||||
b3RequestCameraImageSetPixelResolution(commandHandle, camVisualizerWidth,camVisualizerHeight);
|
||||
float lightPos[3];
|
||||
lightPos[0] = m_lightPos[0];
|
||||
lightPos[1] = m_lightPos[1];
|
||||
lightPos[2] = m_lightPos[2];
|
||||
b3RequestCameraImageSetLightDirection(commandHandle, lightPos);
|
||||
b3RequestCameraImageSetLightSpecularCoeff(commandHandle, m_specularCoeff);
|
||||
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
|
||||
break;
|
||||
}
|
||||
@@ -522,6 +531,7 @@ m_canvas(0),
|
||||
m_canvasRGBIndex(-1),
|
||||
m_canvasDepthIndex(-1),
|
||||
m_canvasSegMaskIndex(-1),
|
||||
m_specularCoeff(1.0),
|
||||
m_numMotors(0),
|
||||
m_options(options),
|
||||
m_isOptionalServerConnected(false)
|
||||
@@ -660,6 +670,29 @@ void PhysicsClientExample::createButtons()
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
SliderParams sliderLightPosX("light source position x",&m_lightPos[0]);
|
||||
SliderParams sliderLightPosY("light source position y",&m_lightPos[1]);
|
||||
SliderParams sliderLightPosZ("light source position z",&m_lightPos[2]);
|
||||
SliderParams sliderSpecularCoeff("specular coefficient",&m_specularCoeff);
|
||||
sliderLightPosX.m_minVal=-1.5;
|
||||
sliderLightPosX.m_maxVal=1.5;
|
||||
sliderLightPosY.m_minVal=-1.5;
|
||||
sliderLightPosY.m_maxVal=1.5;
|
||||
sliderLightPosZ.m_minVal=-1.5;
|
||||
sliderLightPosZ.m_maxVal=1.5;
|
||||
sliderSpecularCoeff.m_minVal=0;
|
||||
sliderSpecularCoeff.m_maxVal=5.0;
|
||||
if (m_guiHelper && m_guiHelper->getParameterInterface())
|
||||
{
|
||||
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(sliderLightPosX);
|
||||
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(sliderLightPosY);
|
||||
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(sliderLightPosZ);
|
||||
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(sliderSpecularCoeff);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -685,6 +718,10 @@ void PhysicsClientExample::initPhysics()
|
||||
m_selectedBody = -1;
|
||||
m_prevSelectedBody = -1;
|
||||
|
||||
m_lightPos[0] = 1.0;
|
||||
m_lightPos[1] = 1.0;
|
||||
m_lightPos[2] = 1.0;
|
||||
|
||||
{
|
||||
m_canvas = m_guiHelper->get2dCanvasInterface();
|
||||
if (m_canvas)
|
||||
|
||||
@@ -1039,6 +1039,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
|
||||
b3Warning("Removing body failed");
|
||||
break;
|
||||
}
|
||||
case CMD_GET_DYNAMICS_INFO_COMPLETED:
|
||||
{
|
||||
break;
|
||||
}
|
||||
case CMD_GET_DYNAMICS_INFO_FAILED:
|
||||
{
|
||||
b3Warning("Request dynamics info failed");
|
||||
break;
|
||||
}
|
||||
default: {
|
||||
b3Error("Unknown server status %d\n", serverCmd.m_type);
|
||||
btAssert(0);
|
||||
|
||||
@@ -3898,15 +3898,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
|
||||
break;
|
||||
};
|
||||
case CMD_RESET_DYNAMIC_INFO:
|
||||
case CMD_CHANGE_DYNAMICS_INFO:
|
||||
{
|
||||
BT_PROFILE("CMD_RESET_DYNAMIC_INFO");
|
||||
BT_PROFILE("CMD_CHANGE_DYNAMICS_INFO");
|
||||
|
||||
if (clientCmd.m_updateFlags & RESET_DYNAMIC_INFO_SET_MASS)
|
||||
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS)
|
||||
{
|
||||
int bodyUniqueId = clientCmd.m_resetDynamicInfoArgs.m_bodyUniqueId;
|
||||
int linkIndex = clientCmd.m_resetDynamicInfoArgs.m_linkIndex;
|
||||
double mass = clientCmd.m_resetDynamicInfoArgs.m_mass;
|
||||
int bodyUniqueId = clientCmd.m_changeDynamicsInfoArgs.m_bodyUniqueId;
|
||||
int linkIndex = clientCmd.m_changeDynamicsInfoArgs.m_linkIndex;
|
||||
double mass = clientCmd.m_changeDynamicsInfoArgs.m_mass;
|
||||
btAssert(bodyUniqueId >= 0);
|
||||
btAssert(linkIndex >= -1);
|
||||
|
||||
@@ -3925,11 +3925,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
}
|
||||
}
|
||||
|
||||
if (clientCmd.m_updateFlags & RESET_DYNAMIC_INFO_SET_LATERAL_FRICTION)
|
||||
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION)
|
||||
{
|
||||
int bodyUniqueId = clientCmd.m_resetDynamicInfoArgs.m_bodyUniqueId;
|
||||
int linkIndex = clientCmd.m_resetDynamicInfoArgs.m_linkIndex;
|
||||
double lateralFriction = clientCmd.m_resetDynamicInfoArgs.m_lateralFriction;
|
||||
int bodyUniqueId = clientCmd.m_changeDynamicsInfoArgs.m_bodyUniqueId;
|
||||
int linkIndex = clientCmd.m_changeDynamicsInfoArgs.m_linkIndex;
|
||||
double lateralFriction = clientCmd.m_changeDynamicsInfoArgs.m_lateralFriction;
|
||||
btAssert(bodyUniqueId >= 0);
|
||||
btAssert(linkIndex >= -1);
|
||||
|
||||
@@ -3955,6 +3955,38 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
|
||||
break;
|
||||
};
|
||||
case CMD_GET_DYNAMICS_INFO:
|
||||
{
|
||||
int bodyUniqueId = clientCmd.m_getDynamicsInfoArgs.m_bodyUniqueId;
|
||||
int linkIndex = clientCmd.m_getDynamicsInfoArgs.m_linkIndex;
|
||||
InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
|
||||
if (body && body->m_multiBody)
|
||||
{
|
||||
SharedMemoryStatus& serverCmd = serverStatusOut;
|
||||
serverCmd.m_type = CMD_GET_DYNAMICS_INFO_COMPLETED;
|
||||
|
||||
btMultiBody* mb = body->m_multiBody;
|
||||
if (linkIndex == -1)
|
||||
{
|
||||
serverCmd.m_dynamicsInfo.m_mass = mb->getBaseMass();
|
||||
serverCmd.m_dynamicsInfo.m_lateralFrictionCoeff = mb->getBaseCollider()->getFriction();
|
||||
}
|
||||
else
|
||||
{
|
||||
serverCmd.m_dynamicsInfo.m_mass = mb->getLinkMass(linkIndex);
|
||||
serverCmd.m_dynamicsInfo.m_lateralFrictionCoeff = mb->getLinkCollider(linkIndex)->getFriction();
|
||||
}
|
||||
hasStatus = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
b3Warning("The dynamic info requested is not available");
|
||||
SharedMemoryStatus& serverCmd = serverStatusOut;
|
||||
serverCmd.m_type = CMD_GET_DYNAMICS_INFO_FAILED;
|
||||
hasStatus = true;
|
||||
}
|
||||
break;
|
||||
}
|
||||
case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS:
|
||||
{
|
||||
BT_PROFILE("CMD_SEND_PHYSICS_SIMULATION_PARAMETERS");
|
||||
|
||||
@@ -106,14 +106,14 @@ struct BulletDataStreamArgs
|
||||
char m_bodyName[MAX_FILENAME_LENGTH];
|
||||
};
|
||||
|
||||
enum EnumResetDynamicInfoFlags
|
||||
enum EnumChangeDynamicsInfoFlags
|
||||
{
|
||||
RESET_DYNAMIC_INFO_SET_MASS=1,
|
||||
RESET_DYNAMIC_INFO_SET_COM=2,
|
||||
RESET_DYNAMIC_INFO_SET_LATERAL_FRICTION=4,
|
||||
CHANGE_DYNAMICS_INFO_SET_MASS=1,
|
||||
CHANGE_DYNAMICS_INFO_SET_COM=2,
|
||||
CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION=4,
|
||||
};
|
||||
|
||||
struct ResetDynamicInfoArgs
|
||||
struct ChangeDynamicsInfoArgs
|
||||
{
|
||||
int m_bodyUniqueId;
|
||||
int m_linkIndex;
|
||||
@@ -122,6 +122,12 @@ struct ResetDynamicInfoArgs
|
||||
double m_lateralFriction;
|
||||
};
|
||||
|
||||
struct GetDynamicsInfoArgs
|
||||
{
|
||||
int m_bodyUniqueId;
|
||||
int m_linkIndex;
|
||||
};
|
||||
|
||||
struct SetJointFeedbackArgs
|
||||
{
|
||||
int m_bodyUniqueId;
|
||||
@@ -738,7 +744,8 @@ struct SharedMemoryCommand
|
||||
struct MjcfArgs m_mjcfArguments;
|
||||
struct FileArgs m_fileArguments;
|
||||
struct SdfRequestInfoArgs m_sdfRequestInfoArgs;
|
||||
struct ResetDynamicInfoArgs m_resetDynamicInfoArgs;
|
||||
struct ChangeDynamicsInfoArgs m_changeDynamicsInfoArgs;
|
||||
struct GetDynamicsInfoArgs m_getDynamicsInfoArgs;
|
||||
struct InitPoseArgs m_initPoseArgs;
|
||||
struct SendPhysicsSimulationParameters m_physSimParamArgs;
|
||||
struct BulletDataStreamArgs m_dataStreamArguments;
|
||||
@@ -832,6 +839,7 @@ struct SharedMemoryStatus
|
||||
struct StateLoggingResultArgs m_stateLoggingResultArgs;
|
||||
struct b3OpenGLVisualizerCameraInfo m_visualizerCameraResultArgs;
|
||||
struct b3ObjectArgs m_removeObjectArgs;
|
||||
struct b3DynamicsInfo m_dynamicsInfo;
|
||||
};
|
||||
};
|
||||
|
||||
|
||||
@@ -56,7 +56,8 @@ enum EnumSharedMemoryClientCommand
|
||||
CMD_REQUEST_KEYBOARD_EVENTS_DATA,
|
||||
CMD_REQUEST_OPENGL_VISUALIZER_CAMERA,
|
||||
CMD_REMOVE_BODY,
|
||||
CMD_RESET_DYNAMIC_INFO,
|
||||
CMD_CHANGE_DYNAMICS_INFO,
|
||||
CMD_GET_DYNAMICS_INFO,
|
||||
CMD_PROFILE_TIMING,
|
||||
//don't go beyond this command!
|
||||
CMD_MAX_CLIENT_COMMANDS,
|
||||
@@ -141,6 +142,8 @@ enum EnumSharedMemoryServerStatus
|
||||
CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_COMPLETED,
|
||||
CMD_REMOVE_BODY_COMPLETED,
|
||||
CMD_REMOVE_BODY_FAILED,
|
||||
CMD_GET_DYNAMICS_INFO_COMPLETED,
|
||||
CMD_GET_DYNAMICS_INFO_FAILED,
|
||||
//don't go beyond 'CMD_MAX_SERVER_COMMANDS!
|
||||
CMD_MAX_SERVER_COMMANDS
|
||||
};
|
||||
@@ -220,10 +223,11 @@ struct b3BodyInfo
|
||||
const char* m_bodyName; // for btRigidBody, it does not have a base, but can still have a body name from urdf
|
||||
};
|
||||
|
||||
struct b3DynamicInfo
|
||||
struct b3DynamicsInfo
|
||||
{
|
||||
double m_mass;
|
||||
double m_localInertialPosition[3];
|
||||
double m_lateralFrictionCoeff;
|
||||
};
|
||||
|
||||
// copied from btMultiBodyLink.h
|
||||
|
||||
@@ -167,8 +167,12 @@ Vec2f Model::uv(int iface, int nthvert) {
|
||||
}
|
||||
|
||||
float Model::specular(Vec2f uvf) {
|
||||
Vec2i uv(uvf[0]*specularmap_.get_width(), uvf[1]*specularmap_.get_height());
|
||||
return specularmap_.get(uv[0], uv[1])[0]/1.f;
|
||||
if (specularmap_.get_width() && specularmap_.get_height())
|
||||
{
|
||||
Vec2i uv(uvf[0]*specularmap_.get_width(), uvf[1]*specularmap_.get_height());
|
||||
return specularmap_.get(uv[0], uv[1])[0]/1.f;
|
||||
}
|
||||
return 2.0;
|
||||
}
|
||||
|
||||
Vec3f Model::normal(int iface, int nthvert) {
|
||||
|
||||
@@ -3,18 +3,21 @@ import time
|
||||
import math
|
||||
|
||||
p.connect(p.GUI)
|
||||
|
||||
p.loadURDF(fileName="plane.urdf",baseOrientation=[0.25882,0,0,0.96593])
|
||||
planeId = p.loadURDF(fileName="plane.urdf",baseOrientation=[0.25882,0,0,0.96593])
|
||||
p.loadURDF(fileName="cube.urdf",baseOrientation=[0.25882,0,0,0.96593],basePosition=[0,0,2])
|
||||
p.loadURDF(fileName="cube.urdf",baseOrientation=[0,0,0,1],basePosition=[0,0,4])
|
||||
p.resetDynamicInfo(bodyUniqueId=2,linkIndex=-1,mass=0.1)
|
||||
#p.resetDynamicInfo(bodyUniqueId=2,linkIndex=-1,mass=100.0)
|
||||
cubeId = p.loadURDF(fileName="cube.urdf",baseOrientation=[0,0,0,1],basePosition=[0,0,4])
|
||||
p.changeDynamicsInfo(bodyUniqueId=2,linkIndex=-1,mass=0.1)
|
||||
#p.changeDynamicsInfo(bodyUniqueId=2,linkIndex=-1,mass=100.0)
|
||||
p.setGravity(0,0,-10)
|
||||
p.setRealTimeSimulation(0)
|
||||
t=0
|
||||
while 1:
|
||||
t=t+1
|
||||
if t > 400:
|
||||
p.resetDynamicInfo(bodyUniqueId=0,linkIndex=-1,lateralFriction=0.01)
|
||||
p.changeDynamicsInfo(bodyUniqueId=0,linkIndex=-1,lateralFriction=0.01)
|
||||
mass1,frictionCoeff1=p.getDynamicsInfo(bodyUniqueId=planeId,linkIndex=-1)
|
||||
mass2,frictionCoeff2=p.getDynamicsInfo(bodyUniqueId=cubeId,linkIndex=-1)
|
||||
print mass1,frictionCoeff1
|
||||
print mass2,frictionCoeff2
|
||||
time.sleep(.01)
|
||||
p.stepSimulation()
|
||||
|
||||
@@ -604,7 +604,7 @@ static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args, PyObject* key
|
||||
return pylist;
|
||||
}
|
||||
|
||||
static PyObject* pybullet_resetDynamicInfo(PyObject* self, PyObject* args, PyObject* keywds)
|
||||
static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyObject* keywds)
|
||||
{
|
||||
int bodyUniqueId = -1;
|
||||
int linkIndex = -2;
|
||||
@@ -627,17 +627,17 @@ static PyObject* pybullet_resetDynamicInfo(PyObject* self, PyObject* args, PyObj
|
||||
}
|
||||
|
||||
{
|
||||
b3SharedMemoryCommandHandle command = b3InitResetDynamicInfo(sm);
|
||||
b3SharedMemoryCommandHandle command = b3InitChangeDynamicsInfo(sm);
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
|
||||
if (mass >= 0)
|
||||
{
|
||||
b3ResetDynamicInfoSetMass(command, bodyUniqueId, linkIndex, mass);
|
||||
b3ChangeDynamicsInfoSetMass(command, bodyUniqueId, linkIndex, mass);
|
||||
}
|
||||
|
||||
if (lateralFriction >= 0)
|
||||
{
|
||||
b3ResetDynamicInfoSetLateralFriction(command, bodyUniqueId, linkIndex, lateralFriction);
|
||||
b3ChangeDynamicsInfoSetLateralFriction(command, bodyUniqueId, linkIndex, lateralFriction);
|
||||
}
|
||||
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
||||
@@ -647,6 +647,63 @@ static PyObject* pybullet_resetDynamicInfo(PyObject* self, PyObject* args, PyObj
|
||||
return Py_None;
|
||||
}
|
||||
|
||||
static PyObject* pybullet_getDynamicsInfo(PyObject* self, PyObject* args, PyObject* keywds)
|
||||
{
|
||||
{
|
||||
int bodyUniqueId = -1;
|
||||
int linkIndex = -2;
|
||||
b3PhysicsClientHandle sm = 0;
|
||||
|
||||
int physicsClientId = 0;
|
||||
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "physicsClientId", NULL};
|
||||
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist, &bodyUniqueId, &linkIndex, &physicsClientId))
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
{
|
||||
int status_type = 0;
|
||||
b3SharedMemoryCommandHandle cmd_handle;
|
||||
b3SharedMemoryStatusHandle status_handle;
|
||||
|
||||
if (bodyUniqueId < 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "getDynamicsInfo failed; invalid bodyUniqueId");
|
||||
return NULL;
|
||||
}
|
||||
if (linkIndex < -1)
|
||||
{
|
||||
PyErr_SetString(SpamError, "getDynamicsInfo failed; invalid linkIndex");
|
||||
return NULL;
|
||||
}
|
||||
cmd_handle = b3GetDynamicsInfoCommandInit(sm, bodyUniqueId, linkIndex);
|
||||
status_handle = b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
|
||||
status_type = b3GetStatusType(status_handle);
|
||||
if (status_type != CMD_GET_DYNAMICS_INFO_COMPLETED)
|
||||
{
|
||||
PyErr_SetString(SpamError, "getDynamicsInfo failed; invalid return status");
|
||||
return NULL;
|
||||
}
|
||||
struct b3DynamicsInfo info;
|
||||
if (b3GetDynamicsInfo(status_handle, &info))
|
||||
{
|
||||
PyObject* pyDynamicsInfo = PyTuple_New(2);
|
||||
PyTuple_SetItem(pyDynamicsInfo, 0, PyFloat_FromDouble(info.m_mass));
|
||||
PyTuple_SetItem(pyDynamicsInfo, 1, PyFloat_FromDouble(info.m_lateralFrictionCoeff));
|
||||
return pyDynamicsInfo;
|
||||
}
|
||||
}
|
||||
}
|
||||
PyErr_SetString(SpamError, "Couldn't get dynamics info");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
|
||||
static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* args, PyObject* keywds)
|
||||
{
|
||||
double fixedTimeStep = -1;
|
||||
@@ -5850,8 +5907,11 @@ static PyMethodDef SpamMethods[] = {
|
||||
"Reset the state (position, velocity etc) for a joint on a body "
|
||||
"instantaneously, not through physics simulation."},
|
||||
|
||||
{"resetDynamicInfo", (PyCFunction)pybullet_resetDynamicInfo, METH_VARARGS | METH_KEYWORDS,
|
||||
"Reset dynamic information such as mass, lateral friction coefficient."},
|
||||
{"changeDynamicsInfo", (PyCFunction)pybullet_changeDynamicsInfo, METH_VARARGS | METH_KEYWORDS,
|
||||
"change dynamics information such as mass, lateral friction coefficient."},
|
||||
|
||||
{"getDynamicsInfo", (PyCFunction)pybullet_getDynamicsInfo, METH_VARARGS | METH_KEYWORDS,
|
||||
"Get dynamics information such as mass, lateral friction coefficient."},
|
||||
|
||||
{"setJointMotorControl", (PyCFunction)pybullet_setJointMotorControl, METH_VARARGS,
|
||||
"This (obsolete) method cannot select non-zero physicsClientId, use setJointMotorControl2 instead."
|
||||
|
||||
Reference in New Issue
Block a user