p80426359
/
pxmlw6n2f
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
pxmlw6n2f/Gazebo_Distributed_TCP/gazebo/physics/bullet/BulletScrewJoint.cc

1233 lines
39 KiB
C++
Raw Blame History

/*
* Copyright (C) 2012 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#include <algorithm>
#include <limits>
#include <string>
#include "gazebo/common/Assert.hh"
#include "gazebo/common/Console.hh"
#include "gazebo/common/Exception.hh"
#include "gazebo/physics/bullet/BulletLink.hh"
#include "gazebo/physics/bullet/BulletPhysics.hh"
#include "gazebo/physics/bullet/BulletTypes.hh"
#include "gazebo/physics/bullet/BulletScrewJoint.hh"
namespace gazebo
{
namespace physics
{
class btScrewConstraint : public btSliderConstraint
{
public: btScrewConstraint(btRigidBody &_rbA, btRigidBody &_rbB,
const btTransform &_frameInA, const btTransform &_frameInB,
bool _useLinearReferenceFrameA)
: btSliderConstraint(_rbA, _rbB, _frameInA, _frameInB,
_useLinearReferenceFrameA), threadPitch(1.0) {}
public: btScrewConstraint(btRigidBody &_rbB,
const btTransform &_frameInB, bool _useLinearReferenceFrameA)
: btSliderConstraint(_rbB, _frameInB, _useLinearReferenceFrameA),
threadPitch(1.0) {}
public: virtual void getInfo1(btConstraintInfo1 *_info)
{
this->_getInfo1NonVirtual(_info);
}
public: virtual void getInfo2(btConstraintInfo2 *_info)
{
this->_getInfo2NonVirtual(
_info,
m_rbA.getCenterOfMassTransform(),
m_rbB.getCenterOfMassTransform(),
m_rbA.getLinearVelocity(),
m_rbB.getLinearVelocity(),
m_rbA.getInvMass(),
m_rbB.getInvMass());
}
public: void _getInfo1NonVirtual(btConstraintInfo1* info);
public: void _getInfo2NonVirtual(
btConstraintInfo2* info,
const btTransform& transA,
const btTransform& transB,
const btVector3& linVelA,
const btVector3& linVelB,
btScalar rbAinvMass, btScalar rbBinvMass);
public: btScalar getLinearPosition();
public: btScalar getAngularPosition();
// needed non-const version for SetForce
public: btRigidBody& getRigidBodyA();
public: btRigidBody& getRigidBodyB();
public: virtual void setThreadPitch(double _threadPitch)
{
this->threadPitch = -_threadPitch;
}
public: virtual double getThreadPitch() const
{
return -this->threadPitch;
}
private: double threadPitch;
};
}
}
using namespace gazebo;
using namespace physics;
//////////////////////////////////////////////////
BulletScrewJoint::BulletScrewJoint(btDynamicsWorld *_world, BasePtr _parent)
: ScrewJoint<BulletJoint>(_parent), bulletScrew(NULL)
{
GZ_ASSERT(_world, "bullet world pointer is NULL");
this->bulletWorld = _world;
}
//////////////////////////////////////////////////
BulletScrewJoint::~BulletScrewJoint()
{
}
//////////////////////////////////////////////////
void BulletScrewJoint::Load(sdf::ElementPtr _sdf)
{
ScrewJoint<BulletJoint>::Load(_sdf);
this->SetThreadPitch(0, this->threadPitch);
}
//////////////////////////////////////////////////
math::Vector3 BulletScrewJoint::GetAnchor(unsigned int /*index*/) const
{
gzerr << "BulletScrewJoint::GetAnchor not implemented, return 0 vector.\n";
return math::Vector3();
}
//////////////////////////////////////////////////
void BulletScrewJoint::SetAnchor(unsigned int /*index*/,
const math::Vector3 &/*_anchor*/)
{
gzerr << "BulletScrewJoint::SetAnchor not implemented.\n";
}
//////////////////////////////////////////////////
void BulletScrewJoint::Init()
{
ScrewJoint<BulletJoint>::Init();
BulletLinkPtr bulletChildLink =
boost::static_pointer_cast<BulletLink>(this->childLink);
BulletLinkPtr bulletParentLink =
boost::static_pointer_cast<BulletLink>(this->parentLink);
// Get axis unit vector (expressed in world frame).
math::Vector3 axis = this->initialWorldAxis;
if (axis == math::Vector3::Zero)
{
gzerr << "axis must have non-zero length, resetting to 0 0 1\n";
axis.Set(0, 0, 1);
}
// Local variables used to compute pivots and axes in body-fixed frames
// for the parent and child links.
math::Vector3 pivotParent, pivotChild, axisParent, axisChild;
math::Pose pose;
btTransform frameParent, frameChild;
btVector3 axis2, axis3;
// Initialize pivots to anchorPos, which is expressed in the
// world coordinate frame.
pivotParent = this->anchorPos;
pivotChild = this->anchorPos;
// Check if parentLink exists. If not, the parent will be the world.
if (this->parentLink)
{
// Compute relative pose between joint anchor and inertial frame of parent.
pose = this->parentLink->GetWorldInertialPose();
// Subtract CoG position from anchor position, both in world frame.
pivotParent -= pose.pos;
// Rotate pivot offset and axis into body-fixed inertial frame of parent.
pivotParent = pose.rot.RotateVectorReverse(pivotParent);
frameParent.setOrigin(BulletTypes::ConvertVector3(pivotParent));
axisParent = pose.rot.RotateVectorReverse(axis);
axisParent = axisParent.Normalize();
// The following math is based on btHingeConstraint.cpp:95-115
btPlaneSpace1(BulletTypes::ConvertVector3(axisParent), axis2, axis3);
frameParent.getBasis().setValue(
axisParent.x, axis2.x(), axis3.x(),
axisParent.y, axis2.y(), axis3.y(),
axisParent.z, axis2.z(), axis3.z());
}
// Check if childLink exists. If not, the child will be the world.
if (this->childLink)
{
// Compute relative pose between joint anchor and inertial frame of child.
pose = this->childLink->GetWorldInertialPose();
// Subtract CoG position from anchor position, both in world frame.
pivotChild -= pose.pos;
// Rotate pivot offset and axis into body-fixed inertial frame of child.
pivotChild = pose.rot.RotateVectorReverse(pivotChild);
frameChild.setOrigin(BulletTypes::ConvertVector3(pivotChild));
axisChild = pose.rot.RotateVectorReverse(axis);
axisChild = axisChild.Normalize();
// The following math is based on btHingeConstraint.cpp:95-115
btPlaneSpace1(BulletTypes::ConvertVector3(axisChild), axis2, axis3);
frameChild.getBasis().setValue(
axisChild.x, axis2.x(), axis3.x(),
axisChild.y, axis2.y(), axis3.y(),
axisChild.z, axis2.z(), axis3.z());
}
// If both links exist, then create a joint between the two links.
if (bulletChildLink && bulletParentLink)
{
// Parent and child constraint frames generated with btPlaneSpace1 may
// differ by 90 degrees because they are underdetermined (only one axis
// given). Here we set the child constraint frame by taking the parent
// constraint frame (in the frame of the parent Bullet link) and rotating
// it into the frame of the child Bullet link. This ensures that the
// constraint frames are initially aligned.
pose = math::Pose(pivotChild,
this->childLink->GetWorldInertialPose().rot.GetInverse() *
this->parentLink->GetWorldInertialPose().rot *
BulletTypes::ConvertPose(frameParent).rot);
frameChild = BulletTypes::ConvertPose(pose);
this->bulletScrew = new btScrewConstraint(
*bulletParentLink->GetBulletLink(),
*bulletChildLink->GetBulletLink(),
frameParent, frameChild, true);
}
// If only the child exists, then create a joint between the child
// and the world.
else if (bulletChildLink)
{
this->bulletScrew = new btScrewConstraint(
*bulletChildLink->GetBulletLink(), frameChild, true);
}
// If only the parent exists, then create a joint between the parent
// and the world.
else if (bulletParentLink)
{
this->bulletScrew = new btScrewConstraint(
*bulletParentLink->GetBulletLink(), frameParent, true);
}
// Throw an error if no links are given.
else
{
gzerr << "Unable to create a joint without links\n";
return;
}
if (!this->bulletScrew)
{
gzerr << "unable to create bullet screw joint\n";
return;
}
// Apply joint translation limits here.
// TODO: velocity and effort limits.
GZ_ASSERT(this->sdf != NULL, "Joint sdf member is NULL");
sdf::ElementPtr limitElem;
limitElem = this->sdf->GetElement("axis")->GetElement("limit");
// joint limit is set on the revolute dof in sdf,
double upper = limitElem->Get<double>("upper");
double lower = limitElem->Get<double>("lower");
this->bulletScrew->setLowerAngLimit(lower);
this->bulletScrew->setUpperAngLimit(upper);
// enforce linear dof in bullet.
double tp = this->threadPitch;
if (math::equal(tp, 0.0))
{
gzerr << "thread pitch should not be zero (joint is a slider?)"
<< " using thread pitch = 1.0e6\n";
tp = 1.0e6;
}
if (tp > 0)
{
this->bulletScrew->setLowerLinLimit(lower/tp);
this->bulletScrew->setUpperLinLimit(upper/tp);
}
else
{
this->bulletScrew->setLowerLinLimit(upper/tp);
this->bulletScrew->setUpperLinLimit(lower/tp);
}
this->bulletScrew->setThreadPitch(tp);
this->constraint = this->bulletScrew;
// Add the joint to the world
GZ_ASSERT(this->bulletWorld, "bullet world pointer is NULL");
this->bulletWorld->addConstraint(this->constraint);
// Allows access to impulse
this->constraint->enableFeedback(true);
// Setup Joint force and torque feedback
this->SetupJointFeedback();
}
//////////////////////////////////////////////////
double BulletScrewJoint::GetVelocity(unsigned int /*_index*/) const
{
double result = 0;
math::Vector3 globalAxis = this->GetGlobalAxis(0);
if (this->childLink)
result += globalAxis.Dot(this->childLink->GetWorldLinearVel());
if (this->parentLink)
result -= globalAxis.Dot(this->parentLink->GetWorldLinearVel());
return result;
}
//////////////////////////////////////////////////
void BulletScrewJoint::SetVelocity(unsigned int _index, double _vel)
{
math::Vector3 desiredVel;
if (this->parentLink)
desiredVel = this->parentLink->GetWorldLinearVel();
desiredVel += _vel * this->GetGlobalAxis(_index);
if (this->childLink)
this->childLink->SetLinearVel(desiredVel);
}
//////////////////////////////////////////////////
void BulletScrewJoint::SetAxis(unsigned int /*_index*/,
const math::Vector3 &_axis)
{
// Note that _axis is given in a world frame,
// but bullet uses a body-fixed frame
if (!this->bulletScrew)
{
// this hasn't been initialized yet, store axis in initialWorldAxis
math::Quaternion axisFrame = this->GetAxisFrame(0);
this->initialWorldAxis = axisFrame.RotateVector(_axis);
}
else
{
gzerr << "SetAxis for existing joint is not implemented\n";
}
}
//////////////////////////////////////////////////
void BulletScrewJoint::SetThreadPitch(unsigned int /*_index*/,
double _threadPitch)
{
this->SetThreadPitch(_threadPitch);
}
//////////////////////////////////////////////////
void BulletScrewJoint::SetThreadPitch(double _threadPitch)
{
this->threadPitch = _threadPitch;
}
//////////////////////////////////////////////////
double BulletScrewJoint::GetThreadPitch()
{
double result = this->threadPitch;
if (this->bulletScrew)
result = this->bulletScrew->getThreadPitch();
else
gzwarn << "bulletScrew not created yet, returning cached threadPitch.\n";
return result;
}
//////////////////////////////////////////////////
double BulletScrewJoint::GetThreadPitch(unsigned int /*_index*/)
{
return this->GetThreadPitch();
}
//////////////////////////////////////////////////
void BulletScrewJoint::SetForceImpl(unsigned int _index, double _force)
{
if (this->bulletScrew)
{
// x-axis of constraint frame
btVector3 hingeAxisLocalA =
this->bulletScrew->getFrameOffsetA().getBasis().getColumn(0);
btVector3 hingeAxisLocalB =
this->bulletScrew->getFrameOffsetB().getBasis().getColumn(0);
btVector3 hingeAxisWorldA =
this->bulletScrew->getRigidBodyA().getWorldTransform().getBasis() *
hingeAxisLocalA;
btVector3 hingeAxisWorldB =
this->bulletScrew->getRigidBodyB().getWorldTransform().getBasis() *
hingeAxisLocalB;
btVector3 hingeEffortA = _force * hingeAxisWorldA;
btVector3 hingeEffortB = _force * hingeAxisWorldB;
if (_index == 0)
{
this->bulletScrew->getRigidBodyA().applyTorque(-hingeEffortA);
this->bulletScrew->getRigidBodyB().applyTorque(hingeEffortB);
}
else if (_index == 1)
{
if (this->constraint)
{
// TODO: switch to applyForce and specify body-fixed offset
this->constraint->getRigidBodyA().applyCentralForce(-hingeEffortA);
this->constraint->getRigidBodyB().applyCentralForce(hingeEffortB);
}
else
gzerr << "BulletScrewJoint::constraint not created yet.\n";
}
else
gzerr << "Invalid index [" << _index << "]\n";
}
else
gzerr << "bulletScrew not created yet.\n";
}
//////////////////////////////////////////////////
bool BulletScrewJoint::SetHighStop(unsigned int _index,
const math::Angle &_angle)
{
Joint::SetHighStop(0, _angle);
// bulletScrew axial rotation is backward
if (!this->bulletScrew)
{
gzerr << "bulletScrew not created yet.\n";
return false;
}
if (_index == 0)
{
// _index = 0: angular constraint
double upperAng = this->bulletScrew->getUpperAngLimit();
this->bulletScrew->setUpperAngLimit(std::max(upperAng, _angle.Radian()));
// set corresponding linear constraints
double tp = this->threadPitch;
if (math::equal(tp, 0.0))
{
gzwarn << "thread pitch should not be zero (joint is a slider?)"
<< " using thread pitch = 1.0e6\n";
tp = 1.0e6;
}
// linear is angular / threadPitch
if (tp > 0)
{
double lowerLin = this->bulletScrew->getLowerLinLimit();
this->bulletScrew->setUpperLinLimit(std::max(lowerLin,
_angle.Radian()/tp));
}
else
{
// flip upper lower because thread pitch is negative
double upperLin = this->bulletScrew->getUpperLinLimit();
this->bulletScrew->setLowerLinLimit(std::min(upperLin,
_angle.Radian()/tp));
}
return true;
}
else if (_index == 1)
{
// _index = 1: linear constraint
double lowerLin = this->bulletScrew->getLowerLinLimit();
this->bulletScrew->setUpperLinLimit(std::max(lowerLin, _angle.Radian()));
// set corresponding angular constraints
double tp = this->threadPitch;
// angular is linear * threadPitch
if (tp > 0)
{
double lowerAng = this->bulletScrew->getLowerAngLimit();
this->bulletScrew->setUpperAngLimit(std::max(lowerAng,
_angle.Radian()*tp));
}
else
{
// flip upper lower because thread pitch is negative
double upperAng = this->bulletScrew->getUpperAngLimit();
this->bulletScrew->setLowerAngLimit(std::min(upperAng,
_angle.Radian()*tp));
}
return true;
}
else
{
gzerr << "Invalid index [" << _index << "]\n";
return false;
}
}
//////////////////////////////////////////////////
bool BulletScrewJoint::SetLowStop(unsigned int _index,
const math::Angle &_angle)
{
Joint::SetLowStop(0, _angle);
// bulletScrew axial rotation is backward
if (!this->bulletScrew)
{
gzerr << "bulletScrew not created yet.\n";
return false;
}
// bulletScrew axial rotation is backward
if (_index == 0)
{
// _index = 0: angular constraint
double upperAng = this->bulletScrew->getUpperAngLimit();
this->bulletScrew->setLowerAngLimit(std::min(upperAng, _angle.Radian()));
// set corresponding linear constraints
double tp = this->threadPitch;
if (math::equal(tp, 0.0))
{
gzerr << "thread pitch should not be zero (joint is a slider?)"
<< " using thread pitch = 1.0e6\n";
tp = 1.0e6;
}
// linear is angular / threadPitch
if (tp > 0)
{
double upperLin = this->bulletScrew->getUpperLinLimit();
this->bulletScrew->setLowerLinLimit(std::min(upperLin,
_angle.Radian()/tp));
}
else
{
// flip upper lower because thread pitch is negative
double lowerLin = this->bulletScrew->getLowerLinLimit();
this->bulletScrew->setUpperLinLimit(std::max(lowerLin,
_angle.Radian()/tp));
}
return true;
}
else if (_index == 1)
{
// _index = 1: linear constraint
double upperLin = this->bulletScrew->getUpperLinLimit();
this->bulletScrew->setLowerLinLimit(std::min(upperLin, _angle.Radian()));
// set corresponding angular constraints
double tp = this->threadPitch;
// angular is linear * threadPitch
if (tp > 0)
{
double upperAng = this->bulletScrew->getUpperAngLimit();
this->bulletScrew->setLowerAngLimit(std::min(upperAng,
_angle.Radian()*tp));
}
else
{
// flip upper lower because thread pitch is negative
double lowerAng = this->bulletScrew->getLowerAngLimit();
this->bulletScrew->setUpperAngLimit(std::max(lowerAng,
_angle.Radian()*tp));
}
return true;
}
else
{
gzerr << "Invalid index [" << _index << "]\n";
return false;
}
}
//////////////////////////////////////////////////
math::Vector3 BulletScrewJoint::GetGlobalAxis(unsigned int /*_index*/) const
{
math::Vector3 result = this->initialWorldAxis;
if (this->bulletScrew)
{
// bullet uses x-axis for slider
btVector3 vec =
this->bulletScrew->getRigidBodyA().getCenterOfMassTransform().getBasis()
* this->bulletScrew->getFrameOffsetA().getBasis().getColumn(0);
result = BulletTypes::ConvertVector3(vec);
}
else
gzwarn << "bulletScrew does not exist, returning fake axis\n";
return result;
}
//////////////////////////////////////////////////
math::Angle BulletScrewJoint::GetAngleImpl(unsigned int _index) const
{
math::Angle result;
if (this->bulletScrew)
{
if (_index == 0)
{
// angular position
result = this->bulletScrew->getAngularPosition();
}
else if (_index == 1)
{
// linear position
result = this->bulletScrew->getLinearPosition();
}
else
gzerr << "Invalid index [" << _index << "]\n";
}
else
gzerr << "bulletScrew not created yet\n";
return result;
}
//////////////////////////////////////////////////
double BulletScrewJoint::GetParam(
const std::string &_key, unsigned int _index)
{
if (_key == "thread_pitch")
return this->threadPitch;
else
return BulletJoint::GetParam(_key, _index);
}
//////////////////////////////////////////////////
btScalar btScrewConstraint::getAngularPosition()
{
this->calculateTransforms(
m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
const btVector3 axisA0 = m_calculatedTransformA.getBasis().getColumn(1);
const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2);
const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1);
return btAtan2(axisB0.dot(axisA1), axisB0.dot(axisA0));
}
//////////////////////////////////////////////////
btScalar btScrewConstraint::getLinearPosition()
{
this->calculateTransforms(
m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
return this->m_depth[0];
}
//////////////////////////////////////////////////
btRigidBody& btScrewConstraint::getRigidBodyA()
{
return this->m_rbA;
}
//////////////////////////////////////////////////
btRigidBody& btScrewConstraint::getRigidBodyB()
{
return this->m_rbB;
}
//////////////////////////////////////////////////
void btScrewConstraint::_getInfo2NonVirtual(
btConstraintInfo2* info,
const btTransform& transA,
const btTransform& transB,
const btVector3& linVelA,
const btVector3& linVelB,
btScalar rbAinvMass, btScalar rbBinvMass)
{
/// This is a copy of btSliderConstraint::getInfo2NonVirtual(...)
/// with minor changes to the ax1 direction constraint.
/// Mainly, the axial limit constraint is always on and is
/// changed to a screw constraint.
/// First, always turn on
const btTransform& trA = getCalculatedTransformA();
const btTransform& trB = getCalculatedTransformB();
btAssert(!m_useSolveConstraintObsolete);
int i, s = info->rowskip;
btScalar signFact = m_useLinearReferenceFrameA ?
btScalar(1.0f) : btScalar(-1.0f);
// difference between frames in WCS
btVector3 ofs = trB.getOrigin() - trA.getOrigin();
// now get weight factors depending on masses
btScalar miA = rbAinvMass;
btScalar miB = rbBinvMass;
bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
btScalar miS = miA + miB;
btScalar factA, factB;
if (miS > btScalar(0.f))
{
factA = miB / miS;
}
else
{
factA = btScalar(0.5f);
}
factB = btScalar(1.0f) - factA;
btVector3 ax1, p, q;
btVector3 ax1A = trA.getBasis().getColumn(0);
btVector3 ax1B = trB.getBasis().getColumn(0);
if (m_useOffsetForConstraintFrame)
{
// get the desired direction of slider axis
// as weighted sum of X-orthos of frameA and frameB in WCS
ax1 = ax1A * factA + ax1B * factB;
ax1.normalize();
// construct two orthos to slider axis
btPlaneSpace1(ax1, p, q);
}
else
{ // old way - use frameA
ax1 = trA.getBasis().getColumn(0);
// get 2 orthos to slider axis (Y, Z)
p = trA.getBasis().getColumn(1);
q = trA.getBasis().getColumn(2);
}
// make rotations around these orthos equal
// the slider axis should be the only unconstrained
// rotational axis, the angular velocity of the two bodies perpendicular to
// the slider axis should be equal. thus the constraint equations are
// p*w1 - p*w2 = 0
// q*w1 - q*w2 = 0
// where p and q are unit vectors normal to the slider axis, and w1 and w2
// are the angular velocity vectors of the two bodies.
info->m_J1angularAxis[0] = p[0];
info->m_J1angularAxis[1] = p[1];
info->m_J1angularAxis[2] = p[2];
info->m_J1angularAxis[s+0] = q[0];
info->m_J1angularAxis[s+1] = q[1];
info->m_J1angularAxis[s+2] = q[2];
info->m_J2angularAxis[0] = -p[0];
info->m_J2angularAxis[1] = -p[1];
info->m_J2angularAxis[2] = -p[2];
info->m_J2angularAxis[s+0] = -q[0];
info->m_J2angularAxis[s+1] = -q[1];
info->m_J2angularAxis[s+2] = -q[2];
// compute the right hand side of the constraint equation. set relative
// body velocities along p and q to bring the slider back into alignment.
// if ax1A,ax1B are the unit length slider axes as computed from bodyA and
// bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2).
// if "theta" is the angle between ax1 and ax2, we need an angular velocity
// along u to cover angle erp*theta in one step :
// |angular_velocity| = angle/time = erp*theta / stepsize
// = (erp*fps) * theta
// angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
// = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
// ...as ax1 and ax2 are unit length. if theta is smallish,
// theta ~= sin(theta), so
// angular_velocity = (erp*fps) * (ax1 x ax2)
// ax1 x ax2 is in the plane space of ax1, so we project the angular
// velocity to p and q to find the right hand side.
// btScalar k = info->fps * info->erp * getSoftnessOrthoAng();
btScalar currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTANG) ?
m_softnessOrthoAng : m_softnessOrthoAng * info->erp;
btScalar k = info->fps * currERP;
btVector3 u = ax1A.cross(ax1B);
info->m_constraintError[0] = k * u.dot(p);
info->m_constraintError[s] = k * u.dot(q);
if (m_flags & BT_SLIDER_FLAGS_CFM_ORTANG)
{
info->cfm[0] = m_cfmOrthoAng;
info->cfm[s] = m_cfmOrthoAng;
}
// last filled row
int nrow = 1;
int srow;
btScalar limit_err;
int limit;
int powered;
// next two rows.
// we want: velA + wA x relA == velB + wB x relB ... but this would
// result in three equations, so we project along two orthos to the
// slider axis
btTransform bodyA_trans = transA;
btTransform bodyB_trans = transB;
nrow++;
int s2 = nrow * s;
nrow++;
int s3 = nrow * s;
btVector3 tmpA(0, 0, 0), tmpB(0, 0, 0), relA(0, 0, 0),
relB(0, 0, 0), c(0, 0, 0);
if (m_useOffsetForConstraintFrame)
{
// get vector from bodyB to frameB in WCS
relB = trB.getOrigin() - bodyB_trans.getOrigin();
// get its projection to slider axis
btVector3 projB = ax1 * relB.dot(ax1);
// get vector directed from bodyB to slider axis (and orthogonal to it)
btVector3 orthoB = relB - projB;
// same for bodyA
relA = trA.getOrigin() - bodyA_trans.getOrigin();
btVector3 projA = ax1 * relA.dot(ax1);
btVector3 orthoA = relA - projA;
// get desired offset between frames A and B along slider axis
btScalar sliderOffs = m_linPos - m_depth[0];
// desired vector from projection of center of bodyA to projection of
// center of bodyB to slider axis
btVector3 totalDist = projA + ax1 * sliderOffs - projB;
// get offset vectors relA and relB
relA = orthoA + totalDist * factA;
relB = orthoB - totalDist * factB;
// now choose average ortho to slider axis
p = orthoB * factA + orthoA * factB;
btScalar len2 = p.length2();
if (len2 > SIMD_EPSILON)
{
p /= btSqrt(len2);
}
else
{
p = trA.getBasis().getColumn(1);
}
// make one more ortho
q = ax1.cross(p);
// fill two rows
tmpA = relA.cross(p);
tmpB = relB.cross(p);
for (i = 0; i < 3; ++i)
{
info->m_J1angularAxis[s2+i] = tmpA[i];
info->m_J2angularAxis[s2+i] = -tmpB[i];
}
tmpA = relA.cross(q);
tmpB = relB.cross(q);
if (hasStaticBody && getSolveAngLimit())
{ // to make constraint between static and dynamic objects more rigid
// remove wA (or wB) from equation if angular limit is hit
tmpB *= factB;
tmpA *= factA;
}
for (i = 0; i < 3; ++i)
{
info->m_J1angularAxis[s3+i] = tmpA[i];
info->m_J2angularAxis[s3+i] = -tmpB[i];
info->m_J1linearAxis[s2+i] = p[i];
info->m_J1linearAxis[s3+i] = q[i];
info->m_J2linearAxis[s2+i] = -p[i];
info->m_J2linearAxis[s3+i] = -q[i];
}
}
else
{
// old way - maybe incorrect if bodies are not on the slider axis
// see discussion "Bug in slider constraint"
// http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=9&t=4024&start=0
c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin();
btVector3 tmp = c.cross(p);
for (i = 0; i < 3; ++i)
{
info->m_J1angularAxis[s2+i] = factA*tmp[i];
info->m_J2angularAxis[s2+i] = factB*tmp[i];
}
tmp = c.cross(q);
for (i = 0; i < 3; ++i)
{
info->m_J1angularAxis[s3+i] = factA*tmp[i];
info->m_J2angularAxis[s3+i] = factB*tmp[i];
info->m_J1linearAxis[s2+i] = p[i];
info->m_J1linearAxis[s3+i] = q[i];
info->m_J2linearAxis[s2+i] = -p[i];
info->m_J2linearAxis[s3+i] = -q[i];
}
}
// compute two elements of right hand side
// k = info->fps * info->erp * getSoftnessOrthoLin();
currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN) ?
m_softnessOrthoLin : m_softnessOrthoLin * info->erp;
k = info->fps * currERP;
btScalar rhs = k * p.dot(ofs);
info->m_constraintError[s2] = rhs;
rhs = k * q.dot(ofs);
info->m_constraintError[s3] = rhs;
if (m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN)
{
info->cfm[s2] = m_cfmOrthoLin;
info->cfm[s3] = m_cfmOrthoLin;
}
// Screw Constraint (coupled linear and angular motion)
{
nrow++;
srow = nrow * info->rowskip;
info->m_J1linearAxis[srow+0] = -ax1[0] * this->threadPitch;
info->m_J1linearAxis[srow+1] = -ax1[1] * this->threadPitch;
info->m_J1linearAxis[srow+2] = -ax1[2] * this->threadPitch;
info->m_J1angularAxis[srow+0] = ax1[0];
info->m_J1angularAxis[srow+1] = ax1[1];
info->m_J1angularAxis[srow+2] = ax1[2];
info->m_J2linearAxis[srow+0] = ax1[0] * this->threadPitch;
info->m_J2linearAxis[srow+1] = ax1[1] * this->threadPitch;
info->m_J2linearAxis[srow+2] = ax1[2] * this->threadPitch;
info->m_J2angularAxis[srow+0] = -ax1[0];
info->m_J2angularAxis[srow+1] = -ax1[1];
info->m_J2angularAxis[srow+2] = -ax1[2];
// correction
// rhs = k * ax1.dot(ofs); // from hinge constraint
btScalar lin_disp = ax1.dot(ofs);
btScalar ang_pos = this->getAngularPosition();
info->m_constraintError[srow] =
-k * (lin_disp * this->threadPitch - ang_pos);
info->cfm[srow] = -m_cfmOrthoLin;
// debug, set cfm to 0
// info->cfm[srow] = 0;
// debug, set error correction to 0
// info->m_constraintError[srow] = 0.0;
}
// notes: below enforces
// joint limit
// powered joint
// rotation along slider axis
// check linear limits
limit_err = btScalar(0.0);
limit = 0;
if (getSolveLinLimit())
{
limit_err = getLinDepth() * signFact;
limit = (limit_err > btScalar(0.0)) ? 2 : 1;
}
powered = 0;
if (getPoweredLinMotor())
{
powered = 1;
}
// if the slider has joint limits or motor, add in the extra row
if (limit || powered)
{
nrow++;
srow = nrow * info->rowskip;
info->m_J1linearAxis[srow+0] = ax1[0];
info->m_J1linearAxis[srow+1] = ax1[1];
info->m_J1linearAxis[srow+2] = ax1[2];
info->m_J2linearAxis[srow+0] = -ax1[0];
info->m_J2linearAxis[srow+1] = -ax1[1];
info->m_J2linearAxis[srow+2] = -ax1[2];
// linear torque decoupling step:
//
// we have to be careful that the linear constraint forces (+/- ax1)
// applied to the two bodies
// do not create a torque couple. in other words, the points that the
// constraint force is applied at must lie along the same ax1 axis.
// a torque couple will result in limited slider-jointed free
// bodies from gaining angular momentum.
if (m_useOffsetForConstraintFrame)
{
// this is needed only when bodyA and bodyB are both dynamic.
if (!hasStaticBody)
{
tmpA = relA.cross(ax1);
tmpB = relB.cross(ax1);
info->m_J1angularAxis[srow+0] = tmpA[0];
info->m_J1angularAxis[srow+1] = tmpA[1];
info->m_J1angularAxis[srow+2] = tmpA[2];
info->m_J2angularAxis[srow+0] = -tmpB[0];
info->m_J2angularAxis[srow+1] = -tmpB[1];
info->m_J2angularAxis[srow+2] = -tmpB[2];
}
}
else
{ // The old way. May be incorrect if bodies are not on the slider axis
// Linear Torque Decoupling vector (a torque)
btVector3 ltd;
ltd = c.cross(ax1);
info->m_J1angularAxis[srow+0] = factA*ltd[0];
info->m_J1angularAxis[srow+1] = factA*ltd[1];
info->m_J1angularAxis[srow+2] = factA*ltd[2];
info->m_J2angularAxis[srow+0] = factB*ltd[0];
info->m_J2angularAxis[srow+1] = factB*ltd[1];
info->m_J2angularAxis[srow+2] = factB*ltd[2];
}
// right-hand part
btScalar lostop = getLowerLinLimit();
btScalar histop = getUpperLinLimit();
// issue #1104:
// if (limit && (lostop == histop)) raises warnings, using
// a warning-less implementation.
if (limit &&
math::equal(lostop, histop,
static_cast<btScalar>(std::numeric_limits<double>::epsilon())))
{
// the joint motor is ineffective
powered = 0;
}
info->m_constraintError[srow] = 0.;
info->m_lowerLimit[srow] = 0.;
info->m_upperLimit[srow] = 0.;
currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN) ?
m_softnessLimLin : info->erp;
if (powered)
{
if (m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN)
{
info->cfm[srow] = m_cfmDirLin;
}
btScalar tag_vel = getTargetLinMotorVelocity();
btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit,
m_upperLinLimit, tag_vel, info->fps * currERP);
info->m_constraintError[srow] -=
signFact * mot_fact * getTargetLinMotorVelocity();
info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps;
info->m_upperLimit[srow] += getMaxLinMotorForce() * info->fps;
}
if (limit)
{
k = info->fps * currERP;
info->m_constraintError[srow] += k * limit_err;
if (m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN)
{
info->cfm[srow] = m_cfmLimLin;
}
// issue #1104:
// if (lostop == histop) raises warnings, using
// a warning-less implementation.
if (math::equal(lostop, histop,
static_cast<btScalar>(std::numeric_limits<double>::epsilon())))
{
// limited low and high simultaneously
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
else if (limit == 1)
{
// low limit
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = 0;
}
else
{ // high limit
info->m_lowerLimit[srow] = 0;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
// bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin)
// for that)
btScalar bounce = btFabs(btScalar(1.0) - getDampingLimLin());
if (bounce > btScalar(0.0))
{
btScalar vel = linVelA.dot(ax1);
vel -= linVelB.dot(ax1);
vel *= signFact;
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if (limit == 1)
{ // low limit
if (vel < 0)
{
btScalar newc = -bounce * vel;
if (newc > info->m_constraintError[srow])
{
info->m_constraintError[srow] = newc;
}
}
}
else
{ // high limit - all those computations are reversed
if (vel > 0)
{
btScalar newc = -bounce * vel;
if (newc < info->m_constraintError[srow])
{
info->m_constraintError[srow] = newc;
}
}
}
}
info->m_constraintError[srow] *= getSoftnessLimLin();
}
// line above is the end of if (limit)
}
// line above is the end of if linear limit
// printf("tp: %f\n", this->threadPitch);
// check angular limits
limit_err = btScalar(0.0);
limit = 0;
if (getSolveAngLimit())
{
limit_err = getAngDepth();
limit = (limit_err > btScalar(0.0)) ? 1 : 2;
}
// if the slider has joint limits, add in the extra row
powered = 0;
if (getPoweredAngMotor())
{
powered = 1;
}
if (limit || powered)
{
nrow++;
srow = nrow * info->rowskip;
info->m_J1angularAxis[srow+0] = ax1[0];
info->m_J1angularAxis[srow+1] = ax1[1];
info->m_J1angularAxis[srow+2] = ax1[2];
info->m_J2angularAxis[srow+0] = -ax1[0];
info->m_J2angularAxis[srow+1] = -ax1[1];
info->m_J2angularAxis[srow+2] = -ax1[2];
btScalar lostop = getLowerAngLimit();
btScalar histop = getUpperAngLimit();
// issue #1104:
// if (limit && (lostop == histop)) raises warnings, using
// a warning-less implementation.
if (limit &&
math::equal(lostop, histop,
static_cast<btScalar>(std::numeric_limits<double>::epsilon())))
{ // the joint motor is ineffective
powered = 0;
}
currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMANG) ?
m_softnessLimAng : info->erp;
if (powered)
{
if (m_flags & BT_SLIDER_FLAGS_CFM_DIRANG)
{
info->cfm[srow] = m_cfmDirAng;
}
btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit,
m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * currERP);
info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity();
info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps;
info->m_upperLimit[srow] = getMaxAngMotorForce() * info->fps;
}
if (limit)
{
k = info->fps * currERP;
info->m_constraintError[srow] += k * limit_err;
if (m_flags & BT_SLIDER_FLAGS_CFM_LIMANG)
{
info->cfm[srow] = m_cfmLimAng;
}
// issue #1104:
// if (lostop == histop) raises warnings, using
// a warning-less implementation.
if (math::equal(lostop, histop,
static_cast<btScalar>(std::numeric_limits<double>::epsilon())))
{
// limited low and high simultaneously
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
else if (limit == 1)
{ // low limit
info->m_lowerLimit[srow] = 0;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
else
{ // high limit
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = 0;
}
// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng)
// for that)
btScalar bounce = btFabs(btScalar(1.0) - getDampingLimAng());
if (bounce > btScalar(0.0))
{
btScalar vel = m_rbA.getAngularVelocity().dot(ax1);
vel -= m_rbB.getAngularVelocity().dot(ax1);
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if (limit == 1)
{ // low limit
if (vel < 0)
{
btScalar newc = -bounce * vel;
if (newc > info->m_constraintError[srow])
{
info->m_constraintError[srow] = newc;
}
}
}
else
{ // high limit - all those computations are reversed
if (vel > 0)
{
btScalar newc = -bounce * vel;
if (newc < info->m_constraintError[srow])
{
info->m_constraintError[srow] = newc;
}
}
}
}
info->m_constraintError[srow] *= getSoftnessLimAng();
}
// line above is the end of if (limit)
}
// line above is the end of if angular limit or powered
}
//////////////////////////////////////////////////
void btScrewConstraint::_getInfo1NonVirtual(btConstraintInfo1* info)
{
/// this is a modified version of
/// void btSliderConstraint::getInfo1(btConstraintInfo1* info)
/// with the rotational limits always turned on.
/// The rotational limit constraint is modified to be a screw constraint
/// in btScrewConstraint::_getInfo2NonVirtual
/// which is a copy of btSliderConstraint::getInfo2NonVirtual.
// info->m_numConstraintRows = 6;
// Fixed 2 linear + 2 angular + 1 limit (even if not used)
// info->nub = 0;
if (m_useSolveConstraintObsolete)
{
info->m_numConstraintRows = 0;
info->nub = 0;
}
else
{
// Fixed 2 linear + 2 angular
info->m_numConstraintRows = 4;
info->nub = 2;
// Add constraint for screw motion, coupling linear and angular motion.
info->m_numConstraintRows++;
info->nub++;
// prepare constraint
calculateTransforms(
m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
testLinLimits();
if (getSolveLinLimit() || getPoweredLinMotor())
{
// limit 3rd linear as well
info->m_numConstraintRows++;
info->nub--;
}
testAngLimits();
if (getSolveAngLimit() || getPoweredAngMotor())
{
// limit 3rd angular as well
info->m_numConstraintRows++;
info->nub--;
}
}
// printf("m: %d\n", info->m_numConstraintRows);
}
Reference in New Issue View Git Blame Copy Permalink