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

Merge pull request #517 from erwincoumans/master

Browse Source 
b3CreateBoxCommandSetColorRGBA: allow to specify color when creating …
This commit is contained in:
erwincoumans
2015-11-06 17:41:40 -08:00
parent 4a4d861721 3b9b803683
commit 6df6df3b26
16 changed files with 96 additions and 588 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
View File

@@ -37,7 +37,7 @@ struct BulletURDFInternalData
UrdfParser m_urdfParser;
struct GUIHelperInterface* m_guiHelper;
char m_pathPrefix[1024];
btHashMap<btHashInt,btVector4> m_linkColors;
};
void BulletURDFImporter::printTree()
@@ -96,6 +96,8 @@ struct BulletErrorLogger : public ErrorLogger
bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase)
{
m_data->m_linkColors.clear();
//int argc=0;
char relativeFileName[1024];
@@ -172,6 +174,17 @@ void BulletURDFImporter::getLinkChildIndices(int linkIndex, btAlignedObjectArray
}
}
bool BulletURDFImporter::getLinkColor(int linkIndex, btVector4& colorRGBA) const
{
btVector4* rgbaPtr = m_data->m_linkColors[linkIndex];
if (rgbaPtr)
{
colorRGBA = *rgbaPtr;
return true;
}
return false;
}
std::string BulletURDFImporter::getLinkName(int linkIndex) const
{
UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
@@ -852,8 +865,18 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP
for (int v = 0; v < link->m_visualArray.size();v++)
{
const UrdfVisual& vis = link->m_visualArray[v];
btTransform childTrans = vis.m_linkLocalFrame;
btTransform childTrans = vis.m_linkLocalFrame;
btHashString matName(vis.m_materialName.c_str());
UrdfMaterial *const * matPtr = model.m_materials[matName];
if (matPtr)
{
UrdfMaterial *const mat = *matPtr;
//printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]);
m_data->m_linkColors.insert(linkIndex,mat->m_rgbaColor);
}
convertURDFToVisualShape(&vis, pathPrefix, inertialFrame.inverse()*childTrans, vertices, indices);
}
}
#endif

2
examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
View File

@@ -27,6 +27,8 @@ public:
virtual void getLinkChildIndices(int linkIndex, btAlignedObjectArray<int>& childLinkIndices) const;
virtual std::string getLinkName(int linkIndex) const;
virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const;
virtual std::string getJointName(int linkIndex) const;

6
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
View File

@@ -24,7 +24,7 @@ static btVector4 colors[4] =
};
static btVector3 selectColor2()
static btVector4 selectColor2()
{
static int curColor = 0;
@@ -371,8 +371,8 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
world1->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);
btVector3 color = selectColor2();//(0.0,0.0,0.5);
btVector4 color = selectColor2();//(0.0,0.0,0.5);
u2b.getLinkColor(urdfLinkIndex,color);
creation.createCollisionObjectGraphicsInstance(urdfLinkIndex,col,color);
btScalar friction = 0.5f;

2
examples/Importers/ImportURDFDemo/URDFImporterInterface.h
View File

@@ -24,6 +24,8 @@ public:
///pure virtual interfaces, precondition is a valid linkIndex (you can assert/terminate if the linkIndex is out of range)
virtual std::string getLinkName(int linkIndex) const =0;
/// optional method to provide the link color. return true if the color is available and copied into colorRGBA, return false otherwise
virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const { return false;}
virtual std::string getJointName(int linkIndex) const = 0;

4
examples/Importers/ImportURDFDemo/UrdfParser.cpp
View File

@@ -346,6 +346,8 @@ bool UrdfParser::parseVisual(UrdfVisual& visual, TiXmlElement* config, ErrorLogg
{
if (parseMaterial(visual.m_localMaterial, mat,logger))
{
UrdfMaterial* matPtr = new UrdfMaterial(visual.m_localMaterial);
m_model.m_materials.insert(matPtr->m_name.c_str(),matPtr);
visual.m_hasLocalMaterial = true;
}
}
@@ -786,7 +788,7 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
for (int i=0;i<link->m_visualArray.size();i++)
{
UrdfVisual& vis = link->m_visualArray.at(i);
if (!vis.m_hasLocalMaterial)
if (!vis.m_hasLocalMaterial && vis.m_materialName.c_str())
{
UrdfMaterial** mat = m_model.m_materials.find(vis.m_materialName.c_str());
if (mat && *mat)

12
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -284,6 +284,18 @@ int b3CreateBoxCommandSetCollisionShapeType(b3SharedMemoryCommandHandle commandH
return 0;
}
int b3CreateBoxCommandSetColorRGBA(b3SharedMemoryCommandHandle commandHandle, double red,double green,double blue, double alpha)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CREATE_BOX_COLLISION_SHAPE);
command->m_updateFlags |=BOX_SHAPE_HAS_COLOR;
command->m_createBoxShapeArguments.m_colorRGBA[0] = red;
command->m_createBoxShapeArguments.m_colorRGBA[1] = green;
command->m_createBoxShapeArguments.m_colorRGBA[2] = blue;
command->m_createBoxShapeArguments.m_colorRGBA[3] = alpha;
return 0;
}
int b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW)
{

1
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -94,6 +94,7 @@ int b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHan
int b3CreateBoxCommandSetHalfExtents(b3SharedMemoryCommandHandle commandHandle, double halfExtentsX,double halfExtentsY,double halfExtentsZ);
int b3CreateBoxCommandSetMass(b3SharedMemoryCommandHandle commandHandle, double mass);
int b3CreateBoxCommandSetCollisionShapeType(b3SharedMemoryCommandHandle commandHandle, int collisionShapeType);
int b3CreateBoxCommandSetColorRGBA(b3SharedMemoryCommandHandle commandHandle, double red,double green,double blue, double alpha);
///Initialize (teleport) the pose of a body/robot. You can individually set the base position, base orientation and joint angles.

2
examples/SharedMemory/PhysicsClientExample.cpp
View File

@@ -211,6 +211,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
{
b3SharedMemoryCommandHandle commandHandle = b3CreateBoxShapeCommandInit(m_physicsClientHandle);
b3CreateBoxCommandSetStartPosition(commandHandle,0,0,-3);
b3CreateBoxCommandSetColorRGBA(commandHandle,0,0,1,1);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
@@ -220,6 +221,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
b3CreateBoxCommandSetStartPosition(commandHandle,0,0,0);
b3CreateBoxCommandSetMass(commandHandle,1);
b3CreateBoxCommandSetCollisionShapeType(commandHandle,COLLISION_SHAPE_TYPE_CYLINDER_Y);
b3CreateBoxCommandSetColorRGBA(commandHandle,1,1,0,1);
double radius = 0.2;
double halfHeight = 0.5;
b3CreateBoxCommandSetHalfExtents(commandHandle,radius,halfHeight,radius);

12
examples/SharedMemory/PhysicsServer.cpp
View File

@@ -1684,7 +1684,17 @@ void PhysicsServerSharedMemory::processClientCommands()
bool isDynamic = (mass>0);
btRigidBody* rb = worldImporter->createRigidBody(isDynamic,mass,startTrans,shape,0);
rb->setRollingFriction(0.2);
m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
//m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
btVector4 colorRGBA(1,0,0,1);
if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_COLOR)
{
colorRGBA[0] = clientCmd.m_createBoxShapeArguments.m_colorRGBA[0];
colorRGBA[1] = clientCmd.m_createBoxShapeArguments.m_colorRGBA[1];
colorRGBA[2] = clientCmd.m_createBoxShapeArguments.m_colorRGBA[2];
colorRGBA[3] = clientCmd.m_createBoxShapeArguments.m_colorRGBA[3];
}
m_data->m_guiHelper->createCollisionShapeGraphicsObject(rb->getCollisionShape());
m_data->m_guiHelper->createCollisionObjectGraphicsObject(rb,colorRGBA);
SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_RIGID_BODY_CREATION_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
int bodyUniqueId = m_data->allocHandle();

2
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -205,6 +205,7 @@ enum EnumBoxShapeFlags
BOX_SHAPE_HAS_HALF_EXTENTS=4,
BOX_SHAPE_HAS_MASS=8,
BOX_SHAPE_HAS_COLLISION_SHAPE_TYPE=16,
BOX_SHAPE_HAS_COLOR=32,
};
///This command will be replaced to allow arbitrary collision shape types
struct CreateBoxShapeArgs
@@ -218,6 +219,7 @@ struct CreateBoxShapeArgs
double m_initialPosition[3];
double m_initialOrientation[4];
double m_colorRGBA[4];
};
struct SharedMemoryCommand

470
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@@ -6,7 +6,8 @@
*
* COPYRIGHT:
* Copyright (C) Stephen Thompson, <stephen@solarflare.org.uk>, 2011-2013
* Portions written By Erwin Coumans: replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix)
* Portions written By Erwin Coumans: connection to LCP solver, various multibody constraints, replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix)
* Portions written By Jakub Stepien: support for multi-DOF constraints, introduction of spatial algebra and several other improvements
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
@@ -638,7 +639,7 @@ inline btMatrix3x3 outerProduct(const btVector3& v0, const btVector3& v1) //r
#define vecMulVecTranspose(v0, v1Transposed) outerProduct(v0, v1Transposed)
//
void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt,
btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v,
btAlignedObjectArray<btMatrix3x3> &scratch_m,
@@ -654,6 +655,10 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
// Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame),
// num_links joint acceleration values.
// We added support for multi degree of freedom (multi dof) joints.
// In addition we also can compute the joint reaction forces. This is performed in a second pass,
// so that we can include the effect of the constraint solver forces (computed in the PGS LCP solver)
m_internalNeedsJointFeedback = false;
int num_links = getNumLinks();
@@ -1181,320 +1186,6 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
}
void btMultiBody::stepVelocities(btScalar dt,
btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v,
btAlignedObjectArray<btMatrix3x3> &scratch_m)
{
// Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot)
// and the base linear & angular accelerations.
// We apply damping forces in this routine as well as any external forces specified by the
// caller (via addBaseForce etc).
// output should point to an array of 6 + num_links reals.
// Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame),
// num_links joint acceleration values.
int num_links = getNumLinks();
const btScalar DAMPING_K1_LINEAR = m_linearDamping;
const btScalar DAMPING_K2_LINEAR = m_linearDamping;
const btScalar DAMPING_K1_ANGULAR = m_angularDamping;
const btScalar DAMPING_K2_ANGULAR= m_angularDamping;
btVector3 base_vel = getBaseVel();
btVector3 base_omega = getBaseOmega();
// Temporary matrices/vectors -- use scratch space from caller
// so that we don't have to keep reallocating every frame
scratch_r.resize(2*num_links + 6);
scratch_v.resize(8*num_links + 6);
scratch_m.resize(4*num_links + 4);
btScalar * r_ptr = &scratch_r[0];
btScalar * output = &scratch_r[num_links]; // "output" holds the q_double_dot results
btVector3 * v_ptr = &scratch_v[0];
// vhat_i (top = angular, bottom = linear part)
btVector3 * vel_top_angular = v_ptr; v_ptr += num_links + 1;
btVector3 * vel_bottom_linear = v_ptr; v_ptr += num_links + 1;
// zhat_i^A
btVector3 * f_zero_acc_top_angular = v_ptr; v_ptr += num_links + 1;
btVector3 * f_zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1;
// chat_i (note NOT defined for the base)
btVector3 * coriolis_top_angular = v_ptr; v_ptr += num_links;
btVector3 * coriolis_bottom_linear = v_ptr; v_ptr += num_links;
// top left, top right and bottom left blocks of Ihat_i^A.
// bottom right block = transpose of top left block and is not stored.
// Note: the top right and bottom left blocks are always symmetric matrices, but we don't make use of this fact currently.
btMatrix3x3 * inertia_top_left = &scratch_m[num_links + 1];
btMatrix3x3 * inertia_top_right = &scratch_m[2*num_links + 2];
btMatrix3x3 * inertia_bottom_left = &scratch_m[3*num_links + 3];
// Cached 3x3 rotation matrices from parent frame to this frame.
btMatrix3x3 * rot_from_parent = &m_matrixBuf[0];
btMatrix3x3 * rot_from_world = &scratch_m[0];
// hhat_i, ahat_i
// hhat is NOT stored for the base (but ahat is)
btVector3 * h_top = num_links > 0 ? &m_vectorBuf[0] : 0;
btVector3 * h_bottom = num_links > 0 ? &m_vectorBuf[num_links] : 0;
btVector3 * accel_top = v_ptr; v_ptr += num_links + 1;
btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;
// Y_i, D_i
btScalar * Y1 = r_ptr; r_ptr += num_links;
btScalar * D = num_links > 0 ? &m_realBuf[6 + num_links] : 0;
// ptr to the joint accel part of the output
btScalar * joint_accel = output + 6;
// Start of the algorithm proper.
// First 'upward' loop.
// Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
rot_from_parent[0] = btMatrix3x3(m_baseQuat);
vel_top_angular[0] = rot_from_parent[0] * base_omega;
vel_bottom_linear[0] = rot_from_parent[0] * base_vel;
if (m_fixedBase) {
f_zero_acc_top_angular[0] = f_zero_acc_bottom_linear[0] = btVector3(0,0,0);
} else {
f_zero_acc_top_angular[0] = - (rot_from_parent[0] * (m_baseForce
- m_baseMass*(DAMPING_K1_LINEAR+DAMPING_K2_LINEAR*base_vel.norm())*base_vel));
f_zero_acc_bottom_linear[0] =
- (rot_from_parent[0] * m_baseTorque);
if (m_useGyroTerm)
f_zero_acc_bottom_linear[0]+=vel_top_angular[0].cross( m_baseInertia * vel_top_angular[0] );
f_zero_acc_bottom_linear[0] += m_baseInertia * vel_top_angular[0] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[0].norm());
}
inertia_top_left[0] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero();
inertia_top_right[0].setValue(m_baseMass, 0, 0,
0, m_baseMass, 0,
0, 0, m_baseMass);
inertia_bottom_left[0].setValue(m_baseInertia[0], 0, 0,
0, m_baseInertia[1], 0,
0, 0, m_baseInertia[2]);
rot_from_world[0] = rot_from_parent[0];
for (int i = 0; i < num_links; ++i) {
const int parent = m_links[i].m_parent;
rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];
// vhat_i = i_xhat_p(i) * vhat_p(i)
SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
vel_top_angular[parent+1], vel_bottom_linear[parent+1],
vel_top_angular[i+1], vel_bottom_linear[i+1]);
// we can now calculate chat_i
// remember vhat_i is really vhat_p(i) (but in current frame) at this point
coriolis_bottom_linear[i] = vel_top_angular[i+1].cross(vel_top_angular[i+1].cross(m_links[i].m_cachedRVector))
+ 2 * vel_top_angular[i+1].cross(m_links[i].getAxisBottom(0)) * getJointVel(i);
if (m_links[i].m_jointType == btMultibodyLink::eRevolute) {
coriolis_top_angular[i] = vel_top_angular[i+1].cross(m_links[i].getAxisTop(0)) * getJointVel(i);
coriolis_bottom_linear[i] += (getJointVel(i) * getJointVel(i)) * m_links[i].getAxisTop(0).cross(m_links[i].getAxisBottom(0));
} else {
coriolis_top_angular[i] = btVector3(0,0,0);
}
// now set vhat_i to its true value by doing
// vhat_i += qidot * shat_i
vel_top_angular[i+1] += getJointVel(i) * m_links[i].getAxisTop(0);
vel_bottom_linear[i+1] += getJointVel(i) * m_links[i].getAxisBottom(0);
// calculate zhat_i^A
f_zero_acc_top_angular[i+1] = - (rot_from_world[i+1] * (m_links[i].m_appliedForce));
f_zero_acc_top_angular[i+1] += m_links[i].m_mass * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR*vel_bottom_linear[i+1].norm()) * vel_bottom_linear[i+1];
f_zero_acc_bottom_linear[i+1] =
- (rot_from_world[i+1] * m_links[i].m_appliedTorque);
if (m_useGyroTerm)
{
f_zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].cross( m_links[i].m_inertiaLocal * vel_top_angular[i+1] );
}
f_zero_acc_bottom_linear[i+1] += m_links[i].m_inertiaLocal * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm());
// calculate Ihat_i^A
inertia_top_left[i+1] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero();
inertia_top_right[i+1].setValue(m_links[i].m_mass, 0, 0,
0, m_links[i].m_mass, 0,
0, 0, m_links[i].m_mass);
inertia_bottom_left[i+1].setValue(m_links[i].m_inertiaLocal[0], 0, 0,
0, m_links[i].m_inertiaLocal[1], 0,
0, 0, m_links[i].m_inertiaLocal[2]);
}
// 'Downward' loop.
// (part of TreeForwardDynamics in Mirtich.)
for (int i = num_links - 1; i >= 0; --i) {
h_top[i] = inertia_top_left[i+1] * m_links[i].getAxisTop(0) + inertia_top_right[i+1] * m_links[i].getAxisBottom(0);
h_bottom[i] = inertia_bottom_left[i+1] * m_links[i].getAxisTop(0) + inertia_top_left[i+1].transpose() * m_links[i].getAxisBottom(0);
btScalar val = SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), h_top[i], h_bottom[i]);
D[i] = val;
//Y1 = joint torque - zero acceleration force - coriolis force
Y1[i] = m_links[i].m_jointTorque[0]
- SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), f_zero_acc_top_angular[i+1], f_zero_acc_bottom_linear[i+1])
- SpatialDotProduct(h_top[i], h_bottom[i], coriolis_top_angular[i], coriolis_bottom_linear[i]);
const int parent = m_links[i].m_parent;
btAssert(D[i]!=0.f);
// Ip += pXi * (Ii - hi hi' / Di) * iXp
const btScalar one_over_di = 1.0f / D[i];
const btMatrix3x3 TL = inertia_top_left[i+1] - vecMulVecTranspose(one_over_di * h_top[i] , h_bottom[i]);
const btMatrix3x3 TR = inertia_top_right[i+1] - vecMulVecTranspose(one_over_di * h_top[i] , h_top[i]);
const btMatrix3x3 BL = inertia_bottom_left[i+1]- vecMulVecTranspose(one_over_di * h_bottom[i] , h_bottom[i]);
btMatrix3x3 r_cross;
r_cross.setValue(
0, -m_links[i].m_cachedRVector[2], m_links[i].m_cachedRVector[1],
m_links[i].m_cachedRVector[2], 0, -m_links[i].m_cachedRVector[0],
-m_links[i].m_cachedRVector[1], m_links[i].m_cachedRVector[0], 0);
inertia_top_left[parent+1] += rot_from_parent[i+1].transpose() * ( TL - TR * r_cross ) * rot_from_parent[i+1];
inertia_top_right[parent+1] += rot_from_parent[i+1].transpose() * TR * rot_from_parent[i+1];
inertia_bottom_left[parent+1] += rot_from_parent[i+1].transpose() *
(r_cross * (TL - TR * r_cross) + BL - TL.transpose() * r_cross) * rot_from_parent[i+1];
// Zp += pXi * (Zi + Ii*ci + hi*Yi/Di)
btVector3 in_top, in_bottom, out_top, out_bottom;
const btScalar Y_over_D = Y1[i] * one_over_di;
in_top = f_zero_acc_top_angular[i+1]
+ inertia_top_left[i+1] * coriolis_top_angular[i]
+ inertia_top_right[i+1] * coriolis_bottom_linear[i]
+ Y_over_D * h_top[i];
in_bottom = f_zero_acc_bottom_linear[i+1]
+ inertia_bottom_left[i+1] * coriolis_top_angular[i]
+ inertia_top_left[i+1].transpose() * coriolis_bottom_linear[i]
+ Y_over_D * h_bottom[i];
InverseSpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
in_top, in_bottom, out_top, out_bottom);
f_zero_acc_top_angular[parent+1] += out_top;
f_zero_acc_bottom_linear[parent+1] += out_bottom;
}
// Second 'upward' loop
// (part of TreeForwardDynamics in Mirtich)
if (m_fixedBase)
{
accel_top[0] = accel_bottom[0] = btVector3(0,0,0);
}
else
{
if (num_links > 0)
{
//Matrix<btScalar, 6, 6> Imatrix;
//Imatrix.block<3,3>(0,0) = inertia_top_left[0];
//Imatrix.block<3,3>(3,0) = inertia_bottom_left[0];
//Imatrix.block<3,3>(0,3) = inertia_top_right[0];
//Imatrix.block<3,3>(3,3) = inertia_top_left[0].transpose();
//cached_imatrix_lu.reset(new Eigen::LU<Matrix<btScalar, 6, 6> >(Imatrix)); // TODO: Avoid memory allocation here?
m_cachedInertiaTopLeft = inertia_top_left[0];
m_cachedInertiaTopRight = inertia_top_right[0];
m_cachedInertiaLowerLeft = inertia_bottom_left[0];
m_cachedInertiaLowerRight= inertia_top_left[0].transpose();
}
btVector3 rhs_top (f_zero_acc_top_angular[0][0], f_zero_acc_top_angular[0][1], f_zero_acc_top_angular[0][2]);
btVector3 rhs_bot (f_zero_acc_bottom_linear[0][0], f_zero_acc_bottom_linear[0][1], f_zero_acc_bottom_linear[0][2]);
float result[6];
solveImatrix(rhs_top, rhs_bot, result);
// printf("result=%f,%f,%f,%f,%f,%f\n",result[0],result[0],result[0],result[0],result[0],result[0]);
for (int i = 0; i < 3; ++i) {
accel_top[0][i] = -result[i];
accel_bottom[0][i] = -result[i+3];
}
}
// now do the loop over the m_links
for (int i = 0; i < num_links; ++i)
{
//acceleration of the child link = acceleration of the parent transformed into child frame +
// + coriolis acceleration
// + joint acceleration
const int parent = m_links[i].m_parent;
SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
accel_top[parent+1], accel_bottom[parent+1],
accel_top[i+1], accel_bottom[i+1]);
joint_accel[i] = (Y1[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1])) / D[i];
accel_top[i+1] += coriolis_top_angular[i] + joint_accel[i] * m_links[i].getAxisTop(0);
accel_bottom[i+1] += coriolis_bottom_linear[i] + joint_accel[i] * m_links[i].getAxisBottom(0);
}
// transform base accelerations back to the world frame.
btVector3 omegadot_out = rot_from_parent[0].transpose() * accel_top[0];
output[0] = omegadot_out[0];
output[1] = omegadot_out[1];
output[2] = omegadot_out[2];
btVector3 vdot_out = rot_from_parent[0].transpose() * accel_bottom[0];
output[3] = vdot_out[0];
output[4] = vdot_out[1];
output[5] = vdot_out[2];
/////////////////
//printf("q = [");
//printf("%.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f", m_baseQuat.x(), m_baseQuat.y(), m_baseQuat.z(), m_baseQuat.w(), m_basePos.x(), m_basePos.y(), m_basePos.z());
//for(int link = 0; link < getNumLinks(); ++link)
// printf("%.2f ", m_links[link].m_jointPos[0]);
//printf("]\n");
////
//printf("qd = [");
//for(int dof = 0; dof < getNumLinks() + 6; ++dof)
// printf("%.2f ", m_realBuf[dof]);
//printf("]\n");
////
//printf("qdd = [");
//for(int dof = 0; dof < getNumLinks() + 6; ++dof)
// printf("%.2f ", output[dof]);
//printf("]\n");
/////////////////
// Final step: add the accelerations (times dt) to the velocities.
//todo: do we already need to update the velocities, or can we move this into the constraint solver?
//the gravity (and other forces) will cause an undesired bounce/restitution effect when using this approach
applyDeltaVee(output, dt);
}
void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const
{
@@ -1762,151 +1453,6 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
}
void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output,
btAlignedObjectArray<btScalar> &scratch_r, btAlignedObjectArray<btVector3> &scratch_v) const
{
// Temporary matrices/vectors -- use scratch space from caller
// so that we don't have to keep reallocating every frame
int num_links = getNumLinks();
scratch_r.resize(num_links);
scratch_v.resize(4*num_links + 4);
btScalar * r_ptr = num_links == 0 ? 0 : &scratch_r[0];
btVector3 * v_ptr = &scratch_v[0];
// zhat_i^A (scratch space)
btVector3 * zero_acc_top_angular = v_ptr; v_ptr += num_links + 1;
btVector3 * zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1;
// rot_from_parent (cached from calcAccelerations)
const btMatrix3x3 * rot_from_parent = &m_matrixBuf[0];
// hhat (cached), accel (scratch)
const btVector3 * h_top = num_links > 0 ? &m_vectorBuf[0] : 0;
const btVector3 * h_bottom = num_links > 0 ? &m_vectorBuf[num_links] : 0;
btVector3 * accel_top = v_ptr; v_ptr += num_links + 1;
btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;
// Y_i (scratch), D_i (cached)
btScalar * Y = r_ptr; r_ptr += num_links;
const btScalar * D = num_links > 0 ? &m_realBuf[6 + num_links] : 0;
btAssert(num_links == 0 || r_ptr - &scratch_r[0] == scratch_r.size());
btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
// First 'upward' loop.
// Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
btVector3 input_force(force[3],force[4],force[5]);
btVector3 input_torque(force[0],force[1],force[2]);
// Fill in zero_acc
// -- set to force/torque on the base, zero otherwise
if (m_fixedBase)
{
zero_acc_top_angular[0] = zero_acc_bottom_linear[0] = btVector3(0,0,0);
} else
{
zero_acc_top_angular[0] = - (rot_from_parent[0] * input_force);
zero_acc_bottom_linear[0] = - (rot_from_parent[0] * input_torque);
}
for (int i = 0; i < num_links; ++i)
{
zero_acc_top_angular[i+1] = zero_acc_bottom_linear[i+1] = btVector3(0,0,0);
}
// 'Downward' loop.
for (int i = num_links - 1; i >= 0; --i)
{
// btScalar sdp = -SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]);
Y[i] = - SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]);
Y[i] += force[6 + i]; // add joint torque
const int parent = m_links[i].m_parent;
// Zp += pXi * (Zi + hi*Yi/Di)
btVector3 in_top, in_bottom, out_top, out_bottom;
const btScalar Y_over_D = Y[i] / D[i];
in_top = zero_acc_top_angular[i+1] + Y_over_D * h_top[i];
in_bottom = zero_acc_bottom_linear[i+1] + Y_over_D * h_bottom[i];
InverseSpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
in_top, in_bottom, out_top, out_bottom);
zero_acc_top_angular[parent+1] += out_top;
zero_acc_bottom_linear[parent+1] += out_bottom;
}
// ptr to the joint accel part of the output
btScalar * joint_accel = output + 6;
// Second 'upward' loop
if (m_fixedBase)
{
accel_top[0] = accel_bottom[0] = btVector3(0,0,0);
} else
{
btVector3 rhs_top (zero_acc_top_angular[0][0], zero_acc_top_angular[0][1], zero_acc_top_angular[0][2]);
btVector3 rhs_bot (zero_acc_bottom_linear[0][0], zero_acc_bottom_linear[0][1], zero_acc_bottom_linear[0][2]);
float result[6];
solveImatrix(rhs_top,rhs_bot, result);
// printf("result=%f,%f,%f,%f,%f,%f\n",result[0],result[0],result[0],result[0],result[0],result[0]);
for (int i = 0; i < 3; ++i) {
accel_top[0][i] = -result[i];
accel_bottom[0][i] = -result[i+3];
}
}
// now do the loop over the m_links
for (int i = 0; i < num_links; ++i) {
const int parent = m_links[i].m_parent;
SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
accel_top[parent+1], accel_bottom[parent+1],
accel_top[i+1], accel_bottom[i+1]);
btScalar Y_minus_hT_a = (Y[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1]));
joint_accel[i] = Y_minus_hT_a / D[i];
accel_top[i+1] += joint_accel[i] * m_links[i].getAxisTop(0);
accel_bottom[i+1] += joint_accel[i] * m_links[i].getAxisBottom(0);
}
// transform base accelerations back to the world frame.
btVector3 omegadot_out;
omegadot_out = rot_from_parent[0].transpose() * accel_top[0];
output[0] = omegadot_out[0];
output[1] = omegadot_out[1];
output[2] = omegadot_out[2];
btVector3 vdot_out;
vdot_out = rot_from_parent[0].transpose() * accel_bottom[0];
output[3] = vdot_out[0];
output[4] = vdot_out[1];
output[5] = vdot_out[2];
/////////////////
/*
int ndof = getNumLinks() + 6;
printf("test force(impulse) (%d) = [\n",ndof);
for (int i=0;i<ndof;i++)
{
printf("%.2f ", force[i]);
printf("]\n");
}
printf("delta(%d) = [",ndof);
for(int dof = 0; dof < getNumLinks() + 6; ++dof)
printf("%.2f ", output[dof]);
printf("]\n");
/////////////////
*/
//int dummy = 0;
}
void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd)
@@ -2042,7 +1588,7 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
}
}
void btMultiBody::filConstraintJacobianMultiDof(int link,
void btMultiBody::fillConstraintJacobianMultiDof(int link,
const btVector3 &contact_point,
const btVector3 &normal_ang,
const btVector3 &normal_lin,

63
src/BulletDynamics/Featherstone/btMultiBody.h
View File

@@ -6,7 +6,8 @@
*
* COPYRIGHT:
* Copyright (C) Stephen Thompson, <stephen@solarflare.org.uk>, 2011-2013
* Portions written By Erwin Coumans: replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix)
* Portions written By Erwin Coumans: connection to LCP solver, various multibody constraints, replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix)
* Portions written By Jakub Stepien: support for multi-DOF constraints, introduction of spatial algebra and several other improvements
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
@@ -30,6 +31,8 @@
#include "LinearMath/btMatrix3x3.h"
#include "LinearMath/btAlignedObjectArray.h"
///serialization data, don't change them if you are not familiar with the details of the serialization mechanisms
#ifdef BT_USE_DOUBLE_PRECISION
#define btMultiBodyData btMultiBodyDoubleData
#define btMultiBodyDataName "btMultiBodyDoubleData"
@@ -334,72 +337,26 @@ void addJointTorque(int i, btScalar Q);
// improvement, at least on Windows (where dynamic memory
// allocation appears to be fairly slow).
//
void stepVelocities(btScalar dt,
btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v,
btAlignedObjectArray<btMatrix3x3> &scratch_m);
void stepVelocitiesMultiDof(btScalar dt,
void computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt,
btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v,
btAlignedObjectArray<btMatrix3x3> &scratch_m,
bool isConstraintPass=false
);
// calcAccelerationDeltas
// calcAccelerationDeltasMultiDof
// input: force vector (in same format as jacobian, i.e.:
// 3 torque values, 3 force values, num_links joint torque values)
// output: 3 omegadot values, 3 vdot values, num_links q_double_dot values
// (existing contents of output array are replaced)
// stepVelocities must have been called first.
void calcAccelerationDeltas(const btScalar *force, btScalar *output,
btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v) const;
void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output,
btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v) const;
// apply a delta-vee directly. used in sequential impulses code.
void applyDeltaVee(const btScalar * delta_vee)
{
for (int i = 0; i < 6 + getNumLinks(); ++i)
{
m_realBuf[i] += delta_vee[i];
}
}
void applyDeltaVee(const btScalar * delta_vee, btScalar multiplier)
{
btScalar sum = 0;
for (int i = 0; i < 6 + getNumLinks(); ++i)
{
sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier;
}
btScalar l = btSqrt(sum);
/*
static btScalar maxl = -1e30f;
if (l>maxl)
{
maxl=l;
// printf("maxl=%f\n",maxl);
}
*/
if (l>m_maxAppliedImpulse)
{
// printf("exceeds 100: l=%f\n",maxl);
multiplier *= m_maxAppliedImpulse/l;
}
for (int i = 0; i < 6 + getNumLinks(); ++i)
{
sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier;
m_realBuf[i] += delta_vee[i] * multiplier;
btClamp(m_realBuf[i],-m_maxCoordinateVelocity,m_maxCoordinateVelocity);
}
}
void applyDeltaVeeMultiDof2(const btScalar * delta_vee, btScalar multiplier)
{
for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
@@ -462,11 +419,11 @@ void addJointTorque(int i, btScalar Q);
btScalar *jac,
btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v,
btAlignedObjectArray<btMatrix3x3> &scratch_m) const { filConstraintJacobianMultiDof(link, contact_point, btVector3(0, 0, 0), normal, jac, scratch_r, scratch_v, scratch_m); }
btAlignedObjectArray<btMatrix3x3> &scratch_m) const { fillConstraintJacobianMultiDof(link, contact_point, btVector3(0, 0, 0), normal, jac, scratch_r, scratch_v, scratch_m); }
//a more general version of fillContactJacobianMultiDof which does not assume..
//.. that the constraint in question is contact or, to be more precise, constrains linear velocity only
void filConstraintJacobianMultiDof(int link,
void fillConstraintJacobianMultiDof(int link,
const btVector3 &contact_point,
const btVector3 &normal_ang,
const btVector3 &normal_lin,

57
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
View File

@@ -34,8 +34,7 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
for (int j=0;j<m_multiBodyNonContactConstraints.size();j++)
{
btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[j];
//if (iteration < constraint.m_overrideNumSolverIterations)
//resolveSingleConstraintRowGenericMultiBody(constraint);
resolveSingleConstraintRowGeneric(constraint);
if(constraint.m_multiBodyA)
constraint.m_multiBodyA->setPosUpdated(false);
@@ -192,60 +191,6 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
}
void btMultiBodyConstraintSolver::resolveSingleConstraintRowGenericMultiBody(const btMultiBodySolverConstraint& c)
{
btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
btScalar deltaVelADotn=0;
btScalar deltaVelBDotn=0;
int ndofA=0;
int ndofB=0;
if (c.m_multiBodyA)
{
ndofA = c.m_multiBodyA->getNumDofs() + 6;
for (int i = 0; i < ndofA; ++i)
deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i];
}
if (c.m_multiBodyB)
{
ndofB = c.m_multiBodyB->getNumDofs() + 6;
for (int i = 0; i < ndofB; ++i)
deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i];
}
deltaImpulse -= deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom
deltaImpulse -= deltaVelBDotn*c.m_jacDiagABInv;
const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
if (sum < c.m_lowerLimit)
{
deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
c.m_appliedImpulse = c.m_lowerLimit;
}
else if (sum > c.m_upperLimit)
{
deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
c.m_appliedImpulse = c.m_upperLimit;
}
else
{
c.m_appliedImpulse = sum;
}
if (c.m_multiBodyA)
{
applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA);
c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
}
if (c.m_multiBodyB)
{
applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB);
c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
}
}
void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint,

2
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h
View File

@@ -44,7 +44,7 @@ protected:
int m_tmpNumMultiBodyConstraints;
void resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c);
void resolveSingleConstraintRowGenericMultiBody(const btMultiBodySolverConstraint& c);
void convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal);
btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0);

18
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
View File

@@ -508,7 +508,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
{
if(!bod->isUsingRK4Integration())
{
bod->stepVelocitiesMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
}
else
{
@@ -596,7 +596,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
btScalar h = solverInfo.m_timeStep;
#define output &scratch_r[bod->getNumDofs()]
//calc qdd0 from: q0 & qd0
bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., scratch_r, scratch_v, scratch_m);
pCopy(output, scratch_qdd0, 0, numDofs);
//calc q1 = q0 + h/2 * qd0
pResetQx();
@@ -606,7 +606,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
//
//calc qdd1 from: q1 & qd1
pCopyToVelocityVector(bod, scratch_qd1);
bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., scratch_r, scratch_v, scratch_m);
pCopy(output, scratch_qdd1, 0, numDofs);
//calc q2 = q0 + h/2 * qd1
pResetQx();
@@ -616,7 +616,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
//
//calc qdd2 from: q2 & qd2
pCopyToVelocityVector(bod, scratch_qd2);
bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., scratch_r, scratch_v, scratch_m);
pCopy(output, scratch_qdd2, 0, numDofs);
//calc q3 = q0 + h * qd2
pResetQx();
@@ -626,7 +626,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
//
//calc qdd3 from: q3 & qd3
pCopyToVelocityVector(bod, scratch_qd3);
bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., scratch_r, scratch_v, scratch_m);
pCopy(output, scratch_qdd3, 0, numDofs);
//
@@ -661,7 +661,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
{
for(int link = 0; link < bod->getNumLinks(); ++link)
bod->getLink(link).updateCacheMultiDof();
bod->stepVelocitiesMultiDof(0, scratch_r, scratch_v, scratch_m);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, scratch_r, scratch_v, scratch_m);
}
}
@@ -710,7 +710,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
if(!bod->isUsingRK4Integration())
{
bool isConstraintPass = true;
bod->stepVelocitiesMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m, isConstraintPass);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m, isConstraintPass);
}
}
}
@@ -718,8 +718,8 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
}
for (int i=0;i<this->m_multiBodies.size();i++)
{
btMultiBody* bod = m_multiBodies[i];
{
btMultiBody* bod = m_multiBodies[i];
bod->processDeltaVeeMultiDof2();
}

4
src/LinearMath/btHashMap.h
View File

@@ -399,6 +399,10 @@ protected:
return find(key);
}
const Value* operator[](const Key& key) const {
return find(key);
}
const Value* find(const Key& key) const
{
int index = findIndex(key);