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

856 lines
27 KiB
C++
Raw Permalink Blame History

/*
* Copyright (C) 2014 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 "gazebo/common/Assert.hh"
#include "gazebo/common/Console.hh"
#include "gazebo/common/Exception.hh"
#include "gazebo/physics/World.hh"
#include "gazebo/physics/WorldPrivate.hh"
#include "gazebo/physics/dart/dart_inc.h"
#include "gazebo/physics/dart/DARTCollision.hh"
#include "gazebo/physics/dart/DARTPhysics.hh"
#include "gazebo/physics/dart/DARTModel.hh"
#include "gazebo/physics/dart/DARTLink.hh"
#include "gazebo/physics/dart/DARTJoint.hh"
#include "gazebo/physics/dart/DARTSurfaceParams.hh"
#include "gazebo/physics/dart/DARTLinkPrivate.hh"
using namespace gazebo;
using namespace physics;
//////////////////////////////////////////////////
DARTLink::DARTLink(EntityPtr _parent)
: Link(_parent), dataPtr(new DARTLinkPrivate())
{
}
//////////////////////////////////////////////////
DARTLink::~DARTLink()
{
delete this->dataPtr;
this->dataPtr = nullptr;
}
//////////////////////////////////////////////////
void DARTLink::Load(sdf::ElementPtr _sdf)
{
this->dataPtr->dartPhysics = boost::dynamic_pointer_cast<DARTPhysics>(
this->GetWorld()->GetPhysicsEngine());
if (this->dataPtr->dartPhysics == NULL)
gzthrow("Not using the dart physics engine");
// Check if soft_contact element is contained in this link. If so,
// SoftBodyNode will be created. Otherwise, BodyNode will be created.
sdf::ElementPtr dartElem;
sdf::ElementPtr softCollElem;
sdf::ElementPtr softGeomElem;
if (_sdf->HasElement("collision"))
{
sdf::ElementPtr collElem = _sdf->GetElement("collision");
while (collElem)
{
// Geometry
sdf::ElementPtr geomElem = collElem->GetElement("geometry");
// Surface
if (collElem->HasElement("surface"))
{
sdf::ElementPtr surfaceElem = collElem->GetElement("surface");
// Soft contact
if (surfaceElem->HasElement("soft_contact"))
{
sdf::ElementPtr softContactElem
= surfaceElem->GetElement("soft_contact");
if (softContactElem->HasElement("dart"))
{
if (dartElem != NULL)
{
gzerr << "DART supports only one deformable body in a link.\n";
break;
}
dartElem = softContactElem->GetElement("dart");
softCollElem = collElem;
softGeomElem = geomElem;
}
}
}
collElem = collElem->GetNextElement("collision");
}
}
if (dartElem != NULL)
{
// Create DART SoftBodyNode
dart::dynamics::SoftBodyNode *dtSoftBodyNode
= new dart::dynamics::SoftBodyNode();
// Mass
double fleshMassFraction = dartElem->Get<double>("flesh_mass_fraction");
// bone_attachment (Kv)
if (dartElem->HasElement("bone_attachment"))
{
double kv = dartElem->Get<double>("bone_attachment");
dtSoftBodyNode->setVertexSpringStiffness(kv);
}
// stiffness (Ke)
if (dartElem->HasElement("stiffness"))
{
double ke = dartElem->Get<double>("stiffness");
dtSoftBodyNode->setEdgeSpringStiffness(ke);
}
// damping
if (dartElem->HasElement("damping"))
{
double damping = dartElem->Get<double>("damping");
dtSoftBodyNode->setDampingCoefficient(damping);
}
// pose
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
gzdbg << "pose" << T.matrix() << std::endl;
if (softCollElem->HasElement("pose"))
{
T = DARTTypes::ConvPose(softCollElem->Get<math::Pose>("pose"));
}
// geometry
if (softGeomElem->HasElement("box"))
{
sdf::ElementPtr boxEle = softGeomElem->GetElement("box");
Eigen::Vector3d size
= DARTTypes::ConvVec3(boxEle->Get<math::Vector3>("size"));
dart::dynamics::SoftBodyNodeHelper::setBox(
dtSoftBodyNode, size, T, fleshMassFraction);
dtSoftBodyNode->addCollisionShape(
new dart::dynamics::SoftMeshShape(dtSoftBodyNode));
}
// else if (geomElem->HasElement("ellipsoid"))
// {
// sdf::ElementPtr ellipsoidEle = geomElem->GetElement("ellipsoid");
// Eigen::Vector3d size
// = DARTTypes::ConvVec3(ellipsoidEle->Get<math::Vector3>("size"));
// double nSlices = ellipsoidEle->Get<double>("num_slices");
// double nStacks = ellipsoidEle->Get<double>("num_stacks");
// dart::dynamics::SoftBodyNodeHelper::setEllipsoid(
// dtSoftBodyNode, size, nSlices, nStacks, fleshMassFraction);
// dtSoftBodyNode->addCollisionShape(
// new dart::dynamics::SoftMeshShape(dtSoftBodyNode));
// }
else
{
gzerr << "Unknown soft shape" << std::endl;
}
this->dataPtr->dtBodyNode = dtSoftBodyNode;
}
else
{
// Create DART BodyNode
this->dataPtr->dtBodyNode = new dart::dynamics::BodyNode();
}
Link::Load(_sdf);
}
//////////////////////////////////////////////////
void DARTLink::Init()
{
Link::Init();
// Name
std::string bodyName = this->GetName();
this->dataPtr->dtBodyNode->setName(bodyName);
// Mass
double mass = this->inertial->GetMass();
this->dataPtr->dtBodyNode->setMass(mass);
// Inertia
double Ixx = this->inertial->GetIXX();
double Iyy = this->inertial->GetIYY();
double Izz = this->inertial->GetIZZ();
double Ixy = this->inertial->GetIXY();
double Ixz = this->inertial->GetIXZ();
double Iyz = this->inertial->GetIYZ();
this->dataPtr->dtBodyNode->setMomentOfInertia(Ixx, Iyy, Izz, Ixy, Ixz, Iyz);
// Visual
this->visuals;
// COG offset
math::Vector3 cog = this->inertial->GetCoG();
this->dataPtr->dtBodyNode->setLocalCOM(DARTTypes::ConvVec3(cog));
// Gravity mode
this->SetGravityMode(this->sdf->Get<bool>("gravity"));
// Friction coefficient
/// \todo FIXME: Friction Parameters
/// Gazebo allows different friction parameters per collision objects,
/// while DART stores the friction parameter per link (BodyNode in DART). For
/// now, the average friction parameter of all the child collision objects is
/// stored in this->dataPtr->dtBodyNode.
/// Final friction coefficient is applied in DART's constraint solver by
/// taking the lower of the 2 colliding rigidLink's.
/// See also:
/// - https://github.com/dartsim/dart/issues/141
/// - https://github.com/dartsim/dart/issues/266
double hackAvgMu1 = 0;
double hackAvgMu2 = 0;
int numCollisions = 0;
for (auto const &child : this->children)
{
if (child->HasType(Base::COLLISION))
{
CollisionPtr collision =
boost::static_pointer_cast<Collision>(child);
SurfaceParamsPtr surface = collision->GetSurface();
GZ_ASSERT(surface, "Surface pointer for is invalid");
FrictionPyramidPtr friction = surface->FrictionPyramid();
GZ_ASSERT(friction, "Friction pointer for is invalid");
numCollisions++;
hackAvgMu1 += friction->MuPrimary();
hackAvgMu2 += friction->MuSecondary();
}
}
hackAvgMu1 /= static_cast<double>(numCollisions);
hackAvgMu2 /= static_cast<double>(numCollisions);
this->dataPtr->dtBodyNode->setFrictionCoeff(0.5 * (hackAvgMu1 + hackAvgMu2));
// We don't add dart body node to the skeleton here because dart body node
// should be set its parent joint before being added. This body node will be
// added to the skeleton in DARTModel::Init().
}
//////////////////////////////////////////////////
void DARTLink::Fini()
{
Link::Fini();
}
/////////////////////////////////////////////////////////////////////
void DARTLink::UpdateMass()
{
if (this->dataPtr->dtBodyNode && this->inertial)
{
this->dataPtr->dtBodyNode->setMass(this->inertial->GetMass());
auto Ixxyyzz = this->inertial->GetPrincipalMoments();
auto Ixyxzyz = this->inertial->GetProductsofInertia();
this->dataPtr->dtBodyNode->setMomentOfInertia(
Ixxyyzz[0], Ixxyyzz[1], Ixxyyzz[2],
Ixyxzyz[0], Ixyxzyz[1], Ixyxzyz[2]);
auto cog = DARTTypes::ConvVec3(this->inertial->GetCoG());
this->dataPtr->dtBodyNode->setLocalCOM(cog);
}
}
//////////////////////////////////////////////////
void DARTLink::OnPoseChange()
{
Link::OnPoseChange();
// DART body node always have its parent joint.
dart::dynamics::Joint *joint = this->dataPtr->dtBodyNode->getParentJoint();
// This is for the case this function called before DARTModel::Init() is
// called.
if (joint == NULL)
return;
dart::dynamics::FreeJoint *freeJoint =
dynamic_cast<dart::dynamics::FreeJoint*>(joint);
if (freeJoint)
{
// If the parent joint is free joint, set the 6 dof to fit the target pose.
const Eigen::Isometry3d &W = DARTTypes::ConvPose(this->GetWorldPose());
const Eigen::Isometry3d &T1 = joint->getTransformFromParentBodyNode();
const Eigen::Isometry3d &InvT2 = joint->getTransformFromChildBodyNode();
Eigen::Isometry3d P = Eigen::Isometry3d::Identity();
if (this->dataPtr->dtBodyNode->getParentBodyNode())
P = this->dataPtr->dtBodyNode->getParentBodyNode()->getTransform();
Eigen::Isometry3d Q = T1.inverse() * P.inverse() * W * InvT2;
// Convert homogeneous transformation matrix to 6-dimensional generalized
// coordinates. There are several ways of conversions. Here is the way of
// DART. The orientation part is converted by using logarithm map, which
// maps SO(3) to so(3), and it takes the first three components of the
// generalized coordinates. On the other hand, the position part just takes
// the last three components of the generalized coordinates without any
// conversion.
Eigen::Vector6d q;
q.head<3>() = dart::math::logMap(Q.linear());
q.tail<3>() = Q.translation();
freeJoint->setPositions(q);
// TODO: The above 4 lines will be reduced to single line as:
// freeJoint->setPositions(FreeJoint::convertToPositions(Q));
// after the following PR is merged:
// https://github.com/dartsim/dart/pull/322
// Update all the transformations of the links in the parent model.
freeJoint->getSkeleton()->computeForwardKinematics(true, false, false);
// TODO: This kinematic updating will be done automatically after pull
// request (https://github.com/dartsim/dart/pull/319) is merged so that
// we don't need this line anymore.
}
else
{
gzdbg << "OnPoseChange() doesn't make sense if the parent joint "
<< "is not free joint (6-dof).\n";
}
}
//////////////////////////////////////////////////
void DARTLink::SetEnabled(bool /*_enable*/) const
{
// TODO: DART does not support this functionality.
}
//////////////////////////////////////////////////
bool DARTLink::GetEnabled() const
{
// TODO: DART does not support this functionality.
return true;
}
//////////////////////////////////////////////////
void DARTLink::SetLinearVel(const math::Vector3 &_vel)
{
// DART body node always have its parent joint.
dart::dynamics::Joint *joint = this->dataPtr->dtBodyNode->getParentJoint();
// This is for the case this function called before DARTModel::Init() is
// called.
if (joint == NULL)
{
gzerr << "DARTModel::Init() should be called first.\n";
return;
}
// Check if the parent joint is free joint
dart::dynamics::FreeJoint *freeJoint =
dynamic_cast<dart::dynamics::FreeJoint*>(joint);
// If the parent joint is free joint, set the proper generalized velocity to
// fit the linear velocity of the link
if (freeJoint)
{
// Generalized velocities
Eigen::Vector3d genVel = DARTTypes::ConvVec3(_vel);
dart::dynamics::BodyNode *dtBodyNode = this->dataPtr->dtBodyNode;
// If this link has parent link then subtract the effect of parent link's
// linear and angular velocities
if (dtBodyNode->getParentBodyNode())
{
// Local transformation from the parent link frame to this link frame
Eigen::Isometry3d T = freeJoint->getLocalTransform();
// Parent link's linear and angular velocities
Eigen::Vector3d parentLinVel =
dtBodyNode->getParentBodyNode()->getBodyLinearVelocity();
Eigen::Vector3d parentAngVel =
dtBodyNode->getParentBodyNode()->getBodyAngularVelocity();
// The effect of the parent link's velocities
Eigen::Vector3d propagatedLinVel =
T.linear().transpose() *
(parentAngVel.cross(T.translation()) + parentLinVel);
// Subtract the effect
genVel -= propagatedLinVel;
}
// Rotation matrix from world frame to this link frame
Eigen::Matrix3d R = dtBodyNode->getTransform().linear();
// Change the reference frame to world
genVel = R * genVel;
// Set the generalized velocities
freeJoint->setVelocity(3, genVel[0]);
freeJoint->setVelocity(4, genVel[1]);
freeJoint->setVelocity(5, genVel[2]);
// Update spatial velocities of all the links in the model
freeJoint->getSkeleton()->computeForwardKinematics(false, true, false);
}
else
{
gzdbg << "DARTLink::SetLinearVel() doesn't make sense if the parent joint "
<< "is not free joint (6-dof).\n";
}
}
//////////////////////////////////////////////////
void DARTLink::SetAngularVel(const math::Vector3 &_vel)
{
// DART body node always have its parent joint.
dart::dynamics::Joint *joint = this->dataPtr->dtBodyNode->getParentJoint();
// This is for the case this function called before DARTModel::Init() is
// called.
if (joint == NULL)
{
gzerr << "DARTModel::Init() should be called first.\n";
return;
}
// Check if the parent joint is free joint
dart::dynamics::FreeJoint *freeJoint =
dynamic_cast<dart::dynamics::FreeJoint*>(joint);
// If the parent joint is free joint, set the proper generalized velocity to
// fit the linear velocity of the link
if (freeJoint)
{
// Generalized velocities
Eigen::Vector3d genVel = DARTTypes::ConvVec3(_vel);
dart::dynamics::BodyNode *dtBodyNode = this->dataPtr->dtBodyNode;
// If this link has parent link then subtract the effect of parent link's
// linear and angular velocities
if (dtBodyNode->getParentBodyNode())
{
// Local transformation from the parent link frame to this link frame
Eigen::Isometry3d T = freeJoint->getLocalTransform();
// Parent link's linear and angular velocities
Eigen::Vector3d parentAngVel =
dtBodyNode->getParentBodyNode()->getBodyAngularVelocity();
// The effect of the parent link's velocities
Eigen::Vector3d propagatedAngVel = T.linear().transpose() * parentAngVel;
// Subtract the effect
genVel -= propagatedAngVel;
}
// Rotation matrix from world frame to this link frame
Eigen::Matrix3d R = dtBodyNode->getTransform().linear();
// Change the reference frame to world
genVel = R * genVel;
// Set the generalized velocities
freeJoint->setVelocity(0, genVel[0]);
freeJoint->setVelocity(1, genVel[1]);
freeJoint->setVelocity(2, genVel[2]);
// Update spatial velocities of all the links in the model
freeJoint->getSkeleton()->computeForwardKinematics(false, true, false);
}
else
{
gzdbg << "DARTLink::SetLinearVel() doesn't make sense if the parent joint "
<< "is not free joint (6-dof).\n";
}
}
//////////////////////////////////////////////////
void DARTLink::SetForce(const math::Vector3 &_force)
{
// DART assume that _force is external force.
this->dataPtr->dtBodyNode->setExtForce(DARTTypes::ConvVec3(_force));
}
//////////////////////////////////////////////////
void DARTLink::SetTorque(const math::Vector3 &_torque)
{
// DART assume that _torque is external torque.
this->dataPtr->dtBodyNode->setExtTorque(DARTTypes::ConvVec3(_torque));
}
//////////////////////////////////////////////////
void DARTLink::AddForce(const math::Vector3 &_force)
{
this->dataPtr->dtBodyNode->addExtForce(DARTTypes::ConvVec3(_force));
}
/////////////////////////////////////////////////
void DARTLink::AddRelativeForce(const math::Vector3 &_force)
{
this->dataPtr->dtBodyNode->addExtForce(DARTTypes::ConvVec3(_force),
Eigen::Vector3d::Zero(),
true, true);
}
/////////////////////////////////////////////////
void DARTLink::AddForceAtWorldPosition(const math::Vector3 &_force,
const math::Vector3 &_pos)
{
this->dataPtr->dtBodyNode->addExtForce(DARTTypes::ConvVec3(_pos),
DARTTypes::ConvVec3(_force),
false, false);
}
/////////////////////////////////////////////////
void DARTLink::AddForceAtRelativePosition(const math::Vector3 &_force,
const math::Vector3 &_relpos)
{
this->dataPtr->dtBodyNode->addExtForce(DARTTypes::ConvVec3(_force),
DARTTypes::ConvVec3(_relpos),
true, true);
}
//////////////////////////////////////////////////
void DARTLink::AddLinkForce(const math::Vector3 &/*_force*/,
const math::Vector3 &/*_offset*/)
{
gzlog << "DARTLink::AddLinkForce not yet implemented (issue #1477)."
<< std::endl;
}
/////////////////////////////////////////////////
void DARTLink::AddTorque(const math::Vector3 &_torque)
{
this->dataPtr->dtBodyNode->addExtTorque(DARTTypes::ConvVec3(_torque));
}
/////////////////////////////////////////////////
void DARTLink::AddRelativeTorque(const math::Vector3 &_torque)
{
this->dataPtr->dtBodyNode->addExtTorque(DARTTypes::ConvVec3(_torque), true);
}
//////////////////////////////////////////////////
gazebo::math::Vector3 DARTLink::GetWorldLinearVel(
const math::Vector3 &_offset) const
{
Eigen::Vector3d linVel =
this->dataPtr->dtBodyNode->getWorldLinearVelocity(
DARTTypes::ConvVec3(_offset));
return DARTTypes::ConvVec3(linVel);
}
//////////////////////////////////////////////////
math::Vector3 DARTLink::GetWorldLinearVel(
const gazebo::math::Vector3 &_offset,
const gazebo::math::Quaternion &_q) const
{
Eigen::Matrix3d R1 = Eigen::Matrix3d(DARTTypes::ConvQuat(_q));
Eigen::Vector3d worldOffset = R1 * DARTTypes::ConvVec3(_offset);
Eigen::Vector3d bodyOffset =
this->dataPtr->dtBodyNode->getTransform().linear().transpose() *
worldOffset;
Eigen::Vector3d linVel =
this->dataPtr->dtBodyNode->getWorldLinearVelocity(bodyOffset);
return DARTTypes::ConvVec3(linVel);
}
//////////////////////////////////////////////////
math::Vector3 DARTLink::GetWorldCoGLinearVel() const
{
Eigen::Vector3d linVel = this->dataPtr->dtBodyNode->getWorldCOMVelocity();
return DARTTypes::ConvVec3(linVel);
}
//////////////////////////////////////////////////
math::Vector3 DARTLink::GetWorldAngularVel() const
{
Eigen::Vector3d angVel = this->dataPtr->dtBodyNode->getWorldAngularVelocity();
return DARTTypes::ConvVec3(angVel);
}
/////////////////////////////////////////////////
math::Vector3 DARTLink::GetWorldForce() const
{
Eigen::Vector6d F = this->dataPtr->dtBodyNode->getExternalForceGlobal();
return DARTTypes::ConvVec3(F.tail<3>());
}
//////////////////////////////////////////////////
math::Vector3 DARTLink::GetWorldTorque() const
{
// TODO: Need verification
math::Vector3 torque;
Eigen::Isometry3d W = this->dataPtr->dtBodyNode->getTransform();
Eigen::Matrix6d G = this->dataPtr->dtBodyNode->getSpatialInertia();
Eigen::VectorXd V = this->dataPtr->dtBodyNode->getBodyVelocity();
Eigen::VectorXd dV = this->dataPtr->dtBodyNode->getBodyAcceleration();
Eigen::Vector6d F = G * dV - dart::math::dad(V, G * V);
torque = DARTTypes::ConvVec3(W.linear() * F.head<3>());
return torque;
}
//////////////////////////////////////////////////
void DARTLink::SetGravityMode(bool _mode)
{
this->sdf->GetElement("gravity")->Set(_mode);
this->dataPtr->dtBodyNode->setGravityMode(_mode);
}
//////////////////////////////////////////////////
bool DARTLink::GetGravityMode() const
{
return this->dataPtr->dtBodyNode->getGravityMode();
}
//////////////////////////////////////////////////
void DARTLink::SetSelfCollide(bool _collide)
{
this->sdf->GetElement("self_collide")->Set(_collide);
dart::dynamics::BodyNode *dtBodyNode = this->dataPtr->dtBodyNode;
// If this function is called before the body node is not added to a skeleton,
// the body node does not have parent skeleton. So we just return here. Self
// collision setting will be done later in DARTModel::Init().
if (dtBodyNode->getSkeleton() == NULL)
return;
dart::simulation::World *dtWorld = this->dataPtr->dartPhysics->GetDARTWorld();
dart::dynamics::Skeleton *dtSkeleton = dtBodyNode->getSkeleton();
dart::collision::CollisionDetector *dtCollDet =
dtWorld->getConstraintSolver()->getCollisionDetector();
Link_V links = this->GetModel()->GetLinks();
bool isSkeletonSelfCollidable =
dtBodyNode->getSkeleton()->isEnabledSelfCollisionCheck();
if (_collide)
{
// If the skeleton is already self collidable, then we enable self
// collidable pairs those are associated with this link. The links in the
// pairs should be all and not itself each other.
if (isSkeletonSelfCollidable)
{
for (size_t i = 0; i < links.size(); ++i)
{
if (links[i].get() != this && links[i]->GetSelfCollide())
{
dart::dynamics::BodyNode *itdtBodyNode =
boost::dynamic_pointer_cast<DARTLink>(links[i])->GetDARTBodyNode();
// If this->dataPtr->dtBodyNode and itdtBodyNode are connected then
// don't enable the pair.
// Please see: https://bitbucket.org/osrf/gazebo/issue/899
if ((dtBodyNode->getParentBodyNode() == itdtBodyNode) ||
itdtBodyNode->getParentBodyNode() == this->dataPtr->dtBodyNode)
continue;
dtCollDet->enablePair(dtBodyNode, itdtBodyNode);
}
}
}
// If the skeleton is not self collidable, we first set the skeleton as
// self collidable. If the skeleton is self collidable, then DART regards
// that all the links in the skeleton is self collidable. So, we disable all
// the pairs of which both of the links in the pair is not self collidable.
else
{
dtSkeleton->enableSelfCollision();
for (size_t i = 0; i < links.size() - 1; ++i)
{
for (size_t j = i + 1; j < links.size(); ++j)
{
dart::dynamics::BodyNode *itdtBodyNode1 =
boost::dynamic_pointer_cast<DARTLink>(links[i])->GetDARTBodyNode();
dart::dynamics::BodyNode *itdtBodyNode2 =
boost::dynamic_pointer_cast<DARTLink>(links[j])->GetDARTBodyNode();
// If this->dataPtr->dtBodyNode and itdtBodyNode are connected then
// don't enable the pair.
// Please see: https://bitbucket.org/osrf/gazebo/issue/899
if ((itdtBodyNode1->getParentBodyNode() == itdtBodyNode2) ||
itdtBodyNode2->getParentBodyNode() == itdtBodyNode1)
dtCollDet->disablePair(itdtBodyNode1, itdtBodyNode2);
if (!links[i]->GetSelfCollide() || !links[j]->GetSelfCollide())
dtCollDet->disablePair(itdtBodyNode1, itdtBodyNode2);
}
}
}
}
else
{
// If the skeleton is self collidable, then we disable all the pairs
// associated with this link.
if (isSkeletonSelfCollidable)
{
for (size_t i = 0; i < links.size(); ++i)
{
if (links[i].get() != this)
{
dart::dynamics::BodyNode *itdtBodyNode =
boost::dynamic_pointer_cast<DARTLink>(links[i])->GetDARTBodyNode();
dtCollDet->disablePair(dtBodyNode, itdtBodyNode);
}
}
}
// If now all the links are not self collidable, then we set the skeleton
// as not self collidable.
bool isAllLinksNotCollidable = true;
for (size_t i = 0; i < links.size(); ++i)
{
if (links[i]->GetSelfCollide())
{
isAllLinksNotCollidable = false;
break;
}
}
if (isAllLinksNotCollidable)
dtSkeleton->disableSelfCollision();
}
}
//////////////////////////////////////////////////
void DARTLink::SetLinearDamping(double /*_damping*/)
{
// see: https://github.com/dartsim/dart/issues/85
gzwarn << "DART does not support DARTLink::SetLinearDamping() yet.\n";
}
//////////////////////////////////////////////////
void DARTLink::SetAngularDamping(double /*_damping*/)
{
// see: https://github.com/dartsim/dart/issues/85
gzwarn << "DART does not support DARTLink::SetAngularDamping() yet.\n";
}
//////////////////////////////////////////////////
void DARTLink::SetKinematic(const bool& _state)
{
this->sdf->GetElement("kinematic")->Set(_state);
gzwarn << "DART does not support DARTLink::SetKinematic() yet.\n";
}
//////////////////////////////////////////////////
bool DARTLink::GetKinematic() const
{
// DART does not support kinematic mode for link.";
return false;
}
//////////////////////////////////////////////////
void DARTLink::SetAutoDisable(bool /*_disable*/)
{
gzwarn << "DART does not support DARTLink::SetAutoDisable() yet.\n";
}
//////////////////////////////////////////////////
void DARTLink::SetLinkStatic(bool _static)
{
if (_static == this->dataPtr->staticLink)
return;
if (_static == true)
{
// Add weld joint constraint to DART
this->dataPtr->dtWeldJointConst =
new dart::constraint::WeldJointConstraint(this->dataPtr->dtBodyNode);
GetDARTWorld()->getConstraintSolver()->addConstraint(
this->dataPtr->dtWeldJointConst);
}
else
{
// Remove ball and revolute joint constraints from DART
GetDARTWorld()->getConstraintSolver()->removeConstraint(
this->dataPtr->dtWeldJointConst);
delete this->dataPtr->dtWeldJointConst;
this->dataPtr->dtWeldJointConst = NULL;
}
this->dataPtr->staticLink = _static;
}
//////////////////////////////////////////////////
void DARTLink::updateDirtyPoseFromDARTTransformation()
{
// Step 1: get dart body's transformation
// Step 2: set gazebo link's pose using the transformation
math::Pose newPose = DARTTypes::ConvPose(
this->dataPtr->dtBodyNode->getTransform());
// Set the new pose to this link
this->dirtyPose = newPose;
// Set the new pose to the world
// (Below method can be changed in gazebo code)
this->world->dataPtr->dirtyPoses.push_back(this);
}
//////////////////////////////////////////////////
DARTPhysicsPtr DARTLink::GetDARTPhysics(void) const
{
return boost::dynamic_pointer_cast<DARTPhysics>(
this->GetWorld()->GetPhysicsEngine());
}
//////////////////////////////////////////////////
dart::simulation::World *DARTLink::GetDARTWorld(void) const
{
return GetDARTPhysics()->GetDARTWorld();
}
//////////////////////////////////////////////////
DARTModelPtr DARTLink::GetDARTModel() const
{
return boost::dynamic_pointer_cast<DARTModel>(this->GetModel());
}
//////////////////////////////////////////////////
dart::dynamics::BodyNode *DARTLink::GetDARTBodyNode() const
{
return this->dataPtr->dtBodyNode;
}
//////////////////////////////////////////////////
void DARTLink::SetDARTParentJoint(DARTJointPtr _dartParentJoint)
{
this->dataPtr->dartParentJoint = _dartParentJoint;
}
//////////////////////////////////////////////////
void DARTLink::AddDARTChildJoint(DARTJointPtr _dartChildJoint)
{
this->dataPtr->dartChildJoints.push_back(_dartChildJoint);
}
Reference in New Issue View Git Blame Copy Permalink