mirror of
https://github.com/bulletphysics/bullet3.git
synced 2026-11-18 21:56:08 +00:00
Merge pull request #2626 from erwincoumans/master
unsupported: expose collisionMargin to changeDynamics/getDynamicsInfo
This commit is contained in:
@@ -0,0 +1,32 @@
|
||||
<?xml version="1.0" ?>
|
||||
<robot name="cube">
|
||||
<link name="baseLink">
|
||||
<contact>
|
||||
<lateral_friction value="1.0"/>
|
||||
<rolling_friction value="0.0"/>
|
||||
<contact_cfm value="0.0"/>
|
||||
<contact_erp value="1.0"/>
|
||||
</contact>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value="1.0"/>
|
||||
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="cube.obj" scale="1 1 1"/>
|
||||
</geometry>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="cube.obj" scale="1 1 1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
</robot>
|
||||
|
||||
@@ -3256,6 +3256,19 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetMaxJointVelocity(b3SharedMemoryCommandH
|
||||
return 0;
|
||||
}
|
||||
|
||||
B3_SHARED_API int b3ChangeDynamicsInfoSetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, double collisionMargin)
|
||||
{
|
||||
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 = -1;
|
||||
command->m_changeDynamicsInfoArgs.m_collisionMargin = collisionMargin;
|
||||
command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_COLLISION_MARGIN;
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
B3_SHARED_API int b3ChangeDynamicsInfoSetActivationState(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int activationState)
|
||||
|
||||
@@ -168,6 +168,8 @@ extern "C"
|
||||
B3_SHARED_API int b3ChangeDynamicsInfoSetContactProcessingThreshold(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double contactProcessingThreshold);
|
||||
B3_SHARED_API int b3ChangeDynamicsInfoSetActivationState(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int activationState);
|
||||
B3_SHARED_API int b3ChangeDynamicsInfoSetMaxJointVelocity(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, double maxJointVelocity);
|
||||
B3_SHARED_API int b3ChangeDynamicsInfoSetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, double collisionMargin);
|
||||
|
||||
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyUniqueId, int parentJointIndex, int childBodyUniqueId, int childJointIndex, struct b3JointInfo* info);
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand2(b3SharedMemoryCommandHandle commandHandle, int parentBodyUniqueId, int parentJointIndex, int childBodyUniqueId, int childJointIndex, struct b3JointInfo* info);
|
||||
|
||||
@@ -8927,6 +8927,18 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
|
||||
{
|
||||
mb->setMaxCoordinateVelocity(clientCmd.m_changeDynamicsInfoArgs.m_maxJointVelocity);
|
||||
}
|
||||
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_COLLISION_MARGIN)
|
||||
{
|
||||
mb->getBaseCollider()->getCollisionShape()->setMargin(clientCmd.m_changeDynamicsInfoArgs.m_collisionMargin);
|
||||
if (mb->getBaseCollider()->getCollisionShape()->isCompound())
|
||||
{
|
||||
btCompoundShape* compound = (btCompoundShape*)mb->getBaseCollider()->getCollisionShape();
|
||||
for (int s = 0; s < compound->getNumChildShapes(); s++)
|
||||
{
|
||||
compound->getChildShape(s)->setMargin(clientCmd.m_changeDynamicsInfoArgs.m_collisionMargin);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
@@ -8968,6 +8980,10 @@ 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_COLLISION_MARGIN)
|
||||
{
|
||||
mb->getLinkCollider(linkIndex)->getCollisionShape()->setMargin(clientCmd.m_changeDynamicsInfoArgs.m_collisionMargin);
|
||||
}
|
||||
}
|
||||
|
||||
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_JOINT_DAMPING)
|
||||
@@ -9115,6 +9131,10 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
|
||||
//for a given sphere radius, use a motion threshold of half the radius, before the ccd algorithm is enabled
|
||||
rb->setCcdMotionThreshold(clientCmd.m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius / 2.);
|
||||
}
|
||||
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_COLLISION_MARGIN)
|
||||
{
|
||||
rb->getCollisionShape()->setMargin(clientCmd.m_changeDynamicsInfoArgs.m_collisionMargin);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -9165,6 +9185,7 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S
|
||||
serverCmd.m_dynamicsInfo.m_contactProcessingThreshold = mb->getBaseCollider()->getContactProcessingThreshold();
|
||||
serverCmd.m_dynamicsInfo.m_ccdSweptSphereRadius = mb->getBaseCollider()->getCcdSweptSphereRadius();
|
||||
serverCmd.m_dynamicsInfo.m_frictionAnchor = mb->getBaseCollider()->getCollisionFlags() & btCollisionObject::CF_HAS_FRICTION_ANCHOR;
|
||||
serverCmd.m_dynamicsInfo.m_collisionMargin = mb->getBaseCollider()->getCollisionShape()->getMargin();
|
||||
}
|
||||
else
|
||||
{
|
||||
@@ -9172,6 +9193,7 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S
|
||||
serverCmd.m_dynamicsInfo.m_contactProcessingThreshold = 0;
|
||||
serverCmd.m_dynamicsInfo.m_ccdSweptSphereRadius = 0;
|
||||
serverCmd.m_dynamicsInfo.m_frictionAnchor = 0;
|
||||
serverCmd.m_dynamicsInfo.m_collisionMargin = 0;
|
||||
}
|
||||
serverCmd.m_dynamicsInfo.m_localInertialDiagonal[0] = mb->getBaseInertia()[0];
|
||||
serverCmd.m_dynamicsInfo.m_localInertialDiagonal[1] = mb->getBaseInertia()[1];
|
||||
@@ -9214,6 +9236,7 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S
|
||||
serverCmd.m_dynamicsInfo.m_contactProcessingThreshold = mb->getLinkCollider(linkIndex)->getContactProcessingThreshold();
|
||||
serverCmd.m_dynamicsInfo.m_ccdSweptSphereRadius = mb->getLinkCollider(linkIndex)->getCcdSweptSphereRadius();
|
||||
serverCmd.m_dynamicsInfo.m_frictionAnchor = mb->getLinkCollider(linkIndex)->getCollisionFlags() & btCollisionObject::CF_HAS_FRICTION_ANCHOR;
|
||||
serverCmd.m_dynamicsInfo.m_collisionMargin = mb->getLinkCollider(linkIndex)->getCollisionShape()->getMargin();
|
||||
}
|
||||
else
|
||||
{
|
||||
@@ -9221,6 +9244,7 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S
|
||||
serverCmd.m_dynamicsInfo.m_contactProcessingThreshold = 0;
|
||||
serverCmd.m_dynamicsInfo.m_ccdSweptSphereRadius = 0;
|
||||
serverCmd.m_dynamicsInfo.m_frictionAnchor = 0;
|
||||
serverCmd.m_dynamicsInfo.m_collisionMargin = 0;
|
||||
}
|
||||
|
||||
serverCmd.m_dynamicsInfo.m_localInertialDiagonal[0] = mb->getLinkInertia(linkIndex)[0];
|
||||
@@ -9277,12 +9301,14 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S
|
||||
serverCmd.m_dynamicsInfo.m_angularDamping = rb->getAngularDamping();
|
||||
serverCmd.m_dynamicsInfo.m_linearDamping = rb->getLinearDamping();
|
||||
serverCmd.m_dynamicsInfo.m_mass = rb->getMass();
|
||||
serverCmd.m_dynamicsInfo.m_collisionMargin = rb->getCollisionShape() ? rb->getCollisionShape()->getMargin() : 0;
|
||||
}
|
||||
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
|
||||
else if (body && body->m_softBody){
|
||||
SharedMemoryStatus& serverCmd = serverStatusOut;
|
||||
serverCmd.m_type = CMD_GET_DYNAMICS_INFO_COMPLETED;
|
||||
serverCmd.m_dynamicsInfo.m_bodyType = BT_SOFT_BODY;
|
||||
serverCmd.m_dynamicsInfo.m_collisionMargin = 0;
|
||||
}
|
||||
#endif
|
||||
return hasStatus;
|
||||
|
||||
@@ -166,6 +166,7 @@ enum EnumChangeDynamicsInfoFlags
|
||||
CHANGE_DYNAMICS_INFO_SET_JOINT_DAMPING = 16384,
|
||||
CHANGE_DYNAMICS_INFO_SET_ANISOTROPIC_FRICTION = 32768,
|
||||
CHANGE_DYNAMICS_INFO_SET_MAX_JOINT_VELOCITY = 1<<16,
|
||||
CHANGE_DYNAMICS_INFO_SET_COLLISION_MARGIN = 1 << 17,
|
||||
};
|
||||
|
||||
struct ChangeDynamicsInfoArgs
|
||||
@@ -190,6 +191,7 @@ struct ChangeDynamicsInfoArgs
|
||||
double m_jointDamping;
|
||||
double m_anisotropicFriction[3];
|
||||
double m_maxJointVelocity;
|
||||
double m_collisionMargin;
|
||||
};
|
||||
|
||||
struct GetDynamicsInfoArgs
|
||||
|
||||
@@ -388,6 +388,7 @@ struct b3DynamicsInfo
|
||||
double m_ccdSweptSphereRadius;
|
||||
double m_contactProcessingThreshold;
|
||||
int m_frictionAnchor;
|
||||
double m_collisionMargin;
|
||||
};
|
||||
|
||||
// copied from btMultiBodyLink.h
|
||||
|
||||
@@ -988,6 +988,45 @@ bool b3RobotSimulatorClientAPI_NoDirect::calculateInverseKinematics(const struct
|
||||
return result;
|
||||
}
|
||||
|
||||
int b3RobotSimulatorClientAPI_NoDirect::computeDofCount(int bodyUniqueId) const
|
||||
{
|
||||
if (!isConnected())
|
||||
{
|
||||
b3Warning("Not connected");
|
||||
return 0;
|
||||
}
|
||||
return b3ComputeDofCount(m_data->m_physicsClientHandle, bodyUniqueId);
|
||||
}
|
||||
|
||||
int b3RobotSimulatorClientAPI_NoDirect::calculateMassMatrix(int bodyUniqueId, const double* jointPositions, int numJointPositions, double* massMatrix, int flags)
|
||||
{
|
||||
if (!isConnected())
|
||||
{
|
||||
b3Warning("Not connected");
|
||||
return 0;
|
||||
}
|
||||
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
b3SharedMemoryCommandHandle commandHandle =
|
||||
b3CalculateMassMatrixCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, jointPositions, numJointPositions);
|
||||
b3CalculateMassMatrixSetFlags(commandHandle, flags);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
if (statusType == CMD_CALCULATED_MASS_MATRIX_COMPLETED)
|
||||
{
|
||||
int dofCount;
|
||||
b3GetStatusMassMatrix(m_data->m_physicsClientHandle, statusHandle, &dofCount, NULL);
|
||||
if (dofCount)
|
||||
{
|
||||
b3GetStatusMassMatrix(m_data->m_physicsClientHandle, statusHandle, NULL, massMatrix);
|
||||
return dofCount;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
bool b3RobotSimulatorClientAPI_NoDirect::getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian)
|
||||
{
|
||||
if (!isConnected())
|
||||
|
||||
@@ -783,8 +783,12 @@ public:
|
||||
void setNumSolverIterations(int numIterations);
|
||||
void setContactBreakingThreshold(double threshold);
|
||||
|
||||
int computeDofCount(int bodyUniqueId) const;
|
||||
|
||||
bool calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs &args, struct b3RobotSimulatorInverseKinematicsResults &results);
|
||||
|
||||
int calculateMassMatrix(int bodyUniqueId, const double* jointPositions, int numJointPositions, double* massMatrix, int flags);
|
||||
|
||||
bool getBodyJacobian(int bodyUniqueId, int linkIndex, const double *localPosition, const double *jointPositions, const double *jointVelocities, const double *jointAccelerations, double *linearJacobian, double *angularJacobian);
|
||||
|
||||
void configureDebugVisualizer(enum b3ConfigureDebugVisualizerEnum flag, int enable);
|
||||
|
||||
@@ -0,0 +1,148 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<robot name="pendulum">
|
||||
<!--
|
||||
Author: Erwin Coumans <erwincoumans@google.com>
|
||||
Contributers:
|
||||
-->
|
||||
|
||||
<link name="world"/>
|
||||
<material name="Blue">
|
||||
<color rgba="0.28 0.52 0.92 1.0"/>
|
||||
</material>
|
||||
<link name="link0">
|
||||
<visual>
|
||||
<geometry>
|
||||
<sphere radius="0.05"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0 0.5 0"/>
|
||||
<material name="Blue"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<sphere radius="0.05"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0 0.5 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.0 0.5 0.0"/>
|
||||
<mass value="1."/>
|
||||
<inertia ixx="0.00494875" ixy="0" ixz="0" iyy="0.00494174" iyz="0" izz="0.002219"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="joint0" type="continuous">
|
||||
<parent link="world"/>
|
||||
<child link="link0"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<dynamics damping="1.0" friction="1.0"/>
|
||||
</joint>
|
||||
<link name="link1">
|
||||
<visual>
|
||||
<geometry>
|
||||
<sphere radius="0.05"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0 0.5 0"/>
|
||||
<material name="Red"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<sphere radius="0.05"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0 0.5 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0.5 0"/>
|
||||
<mass value="2.16"/>
|
||||
<inertia ixx="0.00539427" ixy="0" ixz="0" iyy="0.0048979" iyz="0" izz="0.00311573"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="joint1" type="continuous">
|
||||
<parent link="link0"/>
|
||||
<child link="link1"/>
|
||||
<origin rpy="0 0 0" xyz="0 0.5 0"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<dynamics damping="1.0" friction="1.0"/>
|
||||
</joint>
|
||||
<link name="link2">
|
||||
<visual>
|
||||
<geometry>
|
||||
<sphere radius="0.05"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0 0.5 0"/>
|
||||
<material name="Red"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<sphere radius="0.05"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0 0.5 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0.5 0"/>
|
||||
<mass value="2.16"/>
|
||||
<inertia ixx="0.00539427" ixy="0" ixz="0" iyy="0.0048979" iyz="0" izz="0.00311573"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="joint2" type="continuous">
|
||||
<parent link="link1"/>
|
||||
<child link="link2"/>
|
||||
<origin rpy="0 0 0" xyz="0 0.5 0"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<dynamics damping="1.0" friction="1.0"/>
|
||||
</joint>
|
||||
<link name="link3">
|
||||
<visual>
|
||||
<geometry>
|
||||
<sphere radius="0.05"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0 0.5 0"/>
|
||||
<material name="Red"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<sphere radius="0.05"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0 0.5 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0.5 0"/>
|
||||
<mass value="2.16"/>
|
||||
<inertia ixx="0.00539427" ixy="0" ixz="0" iyy="0.0048979" iyz="0" izz="0.00311573"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="joint3" type="continuous">
|
||||
<parent link="link2"/>
|
||||
<child link="link3"/>
|
||||
<origin rpy="0 0 0" xyz="0 0.5 0"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<dynamics damping="1.0" friction="1.0"/>
|
||||
</joint>
|
||||
<link name="link4">
|
||||
<visual>
|
||||
<geometry>
|
||||
<sphere radius="0.05"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0 0.5 0"/>
|
||||
<material name="Red"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<sphere radius="0.05"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0 0.5 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0.5 0"/>
|
||||
<mass value="2.16"/>
|
||||
<inertia ixx="0.00539427" ixy="0" ixz="0" iyy="0.0048979" iyz="0" izz="0.00311573"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="joint4" type="continuous">
|
||||
<parent link="link3"/>
|
||||
<child link="link4"/>
|
||||
<origin rpy="0 0 0" xyz="0 0.5 0"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<dynamics damping="1.0" friction="1.0"/>
|
||||
</joint>
|
||||
</robot>
|
||||
|
||||
@@ -0,0 +1,63 @@
|
||||
<?xml version="0.0" ?>
|
||||
<robot name="urdf_robot">
|
||||
<link name="baseLink">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value="10.0"/>
|
||||
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="cube.obj" scale="1 1 1"/>
|
||||
</geometry>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
<specular rgb="11 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0.4 0.4 0.4"/>
|
||||
<geometry>
|
||||
<sphere radius="0.1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="-0.4 0.4 0.4"/>
|
||||
<geometry>
|
||||
<sphere radius="0.1"/>
|
||||
</geometry>
|
||||
</collision><collision>
|
||||
<origin rpy="0 0 0" xyz="0.4 -0.4 0.4"/>
|
||||
<geometry>
|
||||
<sphere radius="0.1"/>
|
||||
</geometry>
|
||||
</collision><collision>
|
||||
<origin rpy="0 0 0" xyz="-0.4 -0.4 0.4"/>
|
||||
<geometry>
|
||||
<sphere radius="0.1"/>
|
||||
</geometry>
|
||||
</collision><collision>
|
||||
<origin rpy="0 0 0" xyz="0.4 0.4 -0.4"/>
|
||||
<geometry>
|
||||
<sphere radius="0.1"/>
|
||||
</geometry>
|
||||
</collision><collision>
|
||||
<origin rpy="0 0 0" xyz="-0.4 0.4 -0.4"/>
|
||||
<geometry>
|
||||
<sphere radius="0.1"/>
|
||||
</geometry>
|
||||
</collision><collision>
|
||||
<origin rpy="0 0 0" xyz="0.4 -0.4 -0.4"/>
|
||||
<geometry>
|
||||
<sphere radius="0.1"/>
|
||||
</geometry>
|
||||
</collision><collision>
|
||||
<origin rpy="0 0 0" xyz="-0.4 -0.4 -0.4"/>
|
||||
<geometry>
|
||||
<sphere radius="0.1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
</robot>
|
||||
|
||||
@@ -1324,6 +1324,7 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO
|
||||
double contactStiffness = -1;
|
||||
double contactDamping = -1;
|
||||
double ccdSweptSphereRadius = -1;
|
||||
double collisionMargin = -1;
|
||||
int frictionAnchor = -1;
|
||||
double contactProcessingThreshold = -1;
|
||||
int activationState = -1;
|
||||
@@ -1335,8 +1336,8 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO
|
||||
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", "jointDamping", "anisotropicFriction", "maxJointVelocity", "physicsClientId", NULL};
|
||||
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddiOddidOdi", kwlist, &bodyUniqueId, &linkIndex, &mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution, &linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &localInertiaDiagonalObj, &ccdSweptSphereRadius, &contactProcessingThreshold, &activationState, &jointDamping, &anisotropicFrictionObj, &maxJointVelocity, &physicsClientId))
|
||||
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction", "restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "localInertiaDiagonal", "ccdSweptSphereRadius", "contactProcessingThreshold", "activationState", "jointDamping", "anisotropicFriction", "maxJointVelocity", "collisionMargin", "physicsClientId", NULL};
|
||||
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddiOddidOddi", kwlist, &bodyUniqueId, &linkIndex, &mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution, &linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &localInertiaDiagonalObj, &ccdSweptSphereRadius, &contactProcessingThreshold, &activationState, &jointDamping, &anisotropicFrictionObj, &maxJointVelocity, &collisionMargin , &physicsClientId))
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
@@ -1428,6 +1429,11 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO
|
||||
{
|
||||
b3ChangeDynamicsInfoSetMaxJointVelocity(command, bodyUniqueId, maxJointVelocity);
|
||||
}
|
||||
|
||||
if (collisionMargin >= 0)
|
||||
{
|
||||
b3ChangeDynamicsInfoSetCollisionMargin(command, bodyUniqueId, collisionMargin);
|
||||
}
|
||||
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
||||
}
|
||||
@@ -1483,7 +1489,7 @@ static PyObject* pybullet_getDynamicsInfo(PyObject* self, PyObject* args, PyObje
|
||||
|
||||
if (b3GetDynamicsInfo(status_handle, &info))
|
||||
{
|
||||
int numFields = 11;
|
||||
int numFields = 12;
|
||||
PyObject* pyDynamicsInfo = PyTuple_New(numFields);
|
||||
PyTuple_SetItem(pyDynamicsInfo, 0, PyFloat_FromDouble(info.m_mass));
|
||||
PyTuple_SetItem(pyDynamicsInfo, 1, PyFloat_FromDouble(info.m_lateralFrictionCoeff));
|
||||
@@ -1516,6 +1522,7 @@ static PyObject* pybullet_getDynamicsInfo(PyObject* self, PyObject* args, PyObje
|
||||
PyTuple_SetItem(pyDynamicsInfo, 8, PyFloat_FromDouble(info.m_contactDamping));
|
||||
PyTuple_SetItem(pyDynamicsInfo, 9, PyFloat_FromDouble(info.m_contactStiffness));
|
||||
PyTuple_SetItem(pyDynamicsInfo, 10, PyInt_FromLong(info.m_bodyType));
|
||||
PyTuple_SetItem(pyDynamicsInfo, 11, PyFloat_FromDouble(info.m_collisionMargin));
|
||||
return pyDynamicsInfo;
|
||||
}
|
||||
}
|
||||
@@ -3938,6 +3945,37 @@ static PyObject* pybullet_getNumBodies(PyObject* self, PyObject* args, PyObject*
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static PyObject* pybullet_computeDofCount(PyObject* self, PyObject* args, PyObject* keywds)
|
||||
{
|
||||
int physicsClientId = 0;
|
||||
int bodyUniqueId = -1;
|
||||
b3PhysicsClientHandle sm = 0;
|
||||
|
||||
static char* kwlist[] = { "bodyUniqueId", "physicsClientId", NULL };
|
||||
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &bodyUniqueId, &physicsClientId))
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
{
|
||||
int dofCount = b3ComputeDofCount(sm, bodyUniqueId);
|
||||
|
||||
#if PY_MAJOR_VERSION >= 3
|
||||
return PyLong_FromLong(dofCount);
|
||||
#else
|
||||
return PyInt_FromLong(dofCount);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static PyObject* pybullet_getBodyUniqueId(PyObject* self, PyObject* args, PyObject* keywds)
|
||||
{
|
||||
int physicsClientId = 0;
|
||||
@@ -11787,7 +11825,7 @@ static PyObject* pybullet_calculateMassMatrix(PyObject* self, PyObject* args, Py
|
||||
else
|
||||
{
|
||||
PyErr_SetString(SpamError,
|
||||
"Internal error in calculateJacobian");
|
||||
"Internal error in calculateMassMatrix");
|
||||
}
|
||||
}
|
||||
free(jointPositions);
|
||||
@@ -11947,6 +11985,9 @@ static PyMethodDef SpamMethods[] = {
|
||||
{"getBodyInfo", (PyCFunction)pybullet_getBodyInfo, METH_VARARGS | METH_KEYWORDS,
|
||||
"Get the body info, given a body unique id."},
|
||||
|
||||
{ "computeDofCount", (PyCFunction)pybullet_computeDofCount, METH_VARARGS | METH_KEYWORDS,
|
||||
"computeDofCount returns the number of degrees of freedom, including 7 degrees of freedom for the base in case of floating base" },
|
||||
|
||||
{"syncBodyInfo", (PyCFunction)pybullet_syncBodyInfo, METH_VARARGS | METH_KEYWORDS,
|
||||
"syncBodyInfo(physicsClientId=0)\n"
|
||||
"Update body and constraint/joint information, in case other clients made changes."},
|
||||
|
||||
Reference in New Issue
Block a user