Skip to content

Commit

Permalink
Finale: Source hard-tocks
Browse files Browse the repository at this point in the history
Signed-off-by: methylDragon <methylDragon@gmail.com>
  • Loading branch information
methylDragon committed Jul 14, 2022
1 parent 438a718 commit 2027b5a
Show file tree
Hide file tree
Showing 4 changed files with 46 additions and 46 deletions.
6 changes: 3 additions & 3 deletions Changelog.md
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@

### Gazebo Physics 5.1.0 (2021-11-12)

1. Remove unused ign_auto_headers.hh.in
1. Remove unused gz_auto_headers.hh.in
* [Pull request #305](https://github.com/gazebosim/gz-physics/pull/305)

1. Added DART feature for setting joint limits dynamically.
Expand Down Expand Up @@ -69,7 +69,7 @@

### Gazebo Physics 4.3.0 (2021-11-11)

1. Remove unused ign_auto_headers.hh.in
1. Remove unused gz_auto_headers.hh.in
* [Pull request #305](https://github.com/gazebosim/gz-physics/pull/305)

1. Added DART feature for setting joint limits dynamically.
Expand Down Expand Up @@ -323,7 +323,7 @@

### Gazebo Physics 2.5.0 (2021-11-09)

1. Remove unused ign_auto_headers.hh.in
1. Remove unused gz_auto_headers.hh.in
* [Pull request #305](https://github.com/gazebosim/gz-physics/pull/305)

1. Added DART feature for setting joint limits dynamically.
Expand Down
4 changes: 2 additions & 2 deletions dartsim/src/CustomHeightmapShape.cc
Original file line number Diff line number Diff line change
Expand Up @@ -47,10 +47,10 @@ CustomHeightmapShape::CustomHeightmapShape(
else
scale.Z(fabs(_size(2)) / heightmapSizeZ);

auto sizeIgn = gz::math::eigen3::convert(_size);
auto sizeGz = gz::math::eigen3::convert(_size);

std::vector<float> heightsFloat;
_input.FillHeightMap(_subSampling, vertSize, sizeIgn, scale, flipY,
_input.FillHeightMap(_subSampling, vertSize, sizeGz, scale, flipY,
heightsFloat);

this->setHeightField(vertSize, vertSize, heightsFloat);
Expand Down
74 changes: 37 additions & 37 deletions dartsim/src/SimulationFeatures.cc
Original file line number Diff line number Diff line change
Expand Up @@ -172,7 +172,7 @@ void SimulationFeatures::AddContactPropertiesCallback(
{
auto *world = this->ReferenceInterface<DartWorld>(_worldID);

auto handler = std::make_shared<IgnContactSurfaceHandler>();
auto handler = std::make_shared<GzContactSurfaceHandler>();
handler->surfaceParamsCallback = _callback;
handler->convertContact = [this](const dart::collision::Contact& _contact) {
return this->convertContact(_contact);
Expand Down Expand Up @@ -202,7 +202,7 @@ bool SimulationFeatures::RemoveContactPropertiesCallback(
}
}

dart::constraint::ContactSurfaceParams IgnContactSurfaceHandler::createParams(
dart::constraint::ContactSurfaceParams GzContactSurfaceHandler::createParams(
const dart::collision::Contact& _contact,
const size_t _numContactsOnCollisionObject) const
{
Expand All @@ -214,93 +214,93 @@ dart::constraint::ContactSurfaceParams IgnContactSurfaceHandler::createParams(

typedef SetContactPropertiesCallbackFeature F;
typedef FeaturePolicy3d P;
typename F::ContactSurfaceParams<P> pIgn;
typename F::ContactSurfaceParams<P> pGz;

pIgn.frictionCoeff = pDart.mFrictionCoeff;
pIgn.secondaryFrictionCoeff = pDart.mSecondaryFrictionCoeff;
pIgn.slipCompliance = pDart.mSlipCompliance;
pIgn.secondarySlipCompliance = pDart.mSecondarySlipCompliance;
pIgn.restitutionCoeff = pDart.mRestitutionCoeff;
pIgn.firstFrictionalDirection = pDart.mFirstFrictionalDirection;
pIgn.contactSurfaceMotionVelocity = pDart.mContactSurfaceMotionVelocity;
pGz.frictionCoeff = pDart.mFrictionCoeff;
pGz.secondaryFrictionCoeff = pDart.mSecondaryFrictionCoeff;
pGz.slipCompliance = pDart.mSlipCompliance;
pGz.secondarySlipCompliance = pDart.mSecondarySlipCompliance;
pGz.restitutionCoeff = pDart.mRestitutionCoeff;
pGz.firstFrictionalDirection = pDart.mFirstFrictionalDirection;
pGz.contactSurfaceMotionVelocity = pDart.mContactSurfaceMotionVelocity;

auto contactInternal = this->convertContact(_contact);
if (contactInternal)
{
this->surfaceParamsCallback(contactInternal.value(),
_numContactsOnCollisionObject, pIgn);

if (pIgn.frictionCoeff)
pDart.mFrictionCoeff = pIgn.frictionCoeff.value();
if (pIgn.secondaryFrictionCoeff)
pDart.mSecondaryFrictionCoeff = pIgn.secondaryFrictionCoeff.value();
if (pIgn.slipCompliance)
pDart.mSlipCompliance = pIgn.slipCompliance.value();
if (pIgn.secondarySlipCompliance)
pDart.mSecondarySlipCompliance = pIgn.secondarySlipCompliance.value();
if (pIgn.restitutionCoeff)
pDart.mRestitutionCoeff = pIgn.restitutionCoeff.value();
if (pIgn.firstFrictionalDirection)
pDart.mFirstFrictionalDirection = pIgn.firstFrictionalDirection.value();
if (pIgn.contactSurfaceMotionVelocity)
_numContactsOnCollisionObject, pGz);

if (pGz.frictionCoeff)
pDart.mFrictionCoeff = pGz.frictionCoeff.value();
if (pGz.secondaryFrictionCoeff)
pDart.mSecondaryFrictionCoeff = pGz.secondaryFrictionCoeff.value();
if (pGz.slipCompliance)
pDart.mSlipCompliance = pGz.slipCompliance.value();
if (pGz.secondarySlipCompliance)
pDart.mSecondarySlipCompliance = pGz.secondarySlipCompliance.value();
if (pGz.restitutionCoeff)
pDart.mRestitutionCoeff = pGz.restitutionCoeff.value();
if (pGz.firstFrictionalDirection)
pDart.mFirstFrictionalDirection = pGz.firstFrictionalDirection.value();
if (pGz.contactSurfaceMotionVelocity)
pDart.mContactSurfaceMotionVelocity =
pIgn.contactSurfaceMotionVelocity.value();
pGz.contactSurfaceMotionVelocity.value();

static bool warnedRollingFrictionCoeff = false;
if (!warnedRollingFrictionCoeff && pIgn.rollingFrictionCoeff)
if (!warnedRollingFrictionCoeff && pGz.rollingFrictionCoeff)
{
gzwarn << "DART doesn't support rolling friction setting" << std::endl;
warnedRollingFrictionCoeff = true;
}

static bool warnedSecondaryRollingFrictionCoeff = false;
if (!warnedSecondaryRollingFrictionCoeff &&
pIgn.secondaryRollingFrictionCoeff)
pGz.secondaryRollingFrictionCoeff)
{
gzwarn << "DART doesn't support secondary rolling friction setting"
<< std::endl;
warnedSecondaryRollingFrictionCoeff = true;
}

static bool warnedTorsionalFrictionCoeff = false;
if (!warnedTorsionalFrictionCoeff && pIgn.torsionalFrictionCoeff)
if (!warnedTorsionalFrictionCoeff && pGz.torsionalFrictionCoeff)
{
gzwarn << "DART doesn't support torsional friction setting"
<< std::endl;
warnedTorsionalFrictionCoeff = true;
}
}

this->lastIgnParams = pIgn;
this->lastGzParams = pGz;

return pDart;
}

dart::constraint::ContactConstraintPtr
IgnContactSurfaceHandler::createConstraint(
GzContactSurfaceHandler::createConstraint(
dart::collision::Contact& _contact,
const size_t _numContactsOnCollisionObject,
const double _timeStep) const
{
// this call sets this->lastIgnParams
// this call sets this->lastGzParams
auto constraint = dart::constraint::ContactSurfaceHandler::createConstraint(
_contact, _numContactsOnCollisionObject, _timeStep);

typedef SetContactPropertiesCallbackFeature F;
typedef FeaturePolicy3d P;
typename F::ContactSurfaceParams<P>& p = this->lastIgnParams;
typename F::ContactSurfaceParams<P>& p = this->lastGzParams;

if (this->lastIgnParams.errorReductionParameter)
if (this->lastGzParams.errorReductionParameter)
constraint->setErrorReductionParameter(p.errorReductionParameter.value());

if (this->lastIgnParams.maxErrorAllowance)
if (this->lastGzParams.maxErrorAllowance)
constraint->setErrorAllowance(p.maxErrorAllowance.value());

if (this->lastIgnParams.maxErrorReductionVelocity)
if (this->lastGzParams.maxErrorReductionVelocity)
constraint->setMaxErrorReductionVelocity(
p.maxErrorReductionVelocity.value());

if (this->lastIgnParams.constraintForceMixing)
if (this->lastGzParams.constraintForceMixing)
constraint->setConstraintForceMixing(p.constraintForceMixing.value());

return constraint;
Expand Down
8 changes: 4 additions & 4 deletions dartsim/src/SimulationFeatures.hh
Original file line number Diff line number Diff line change
Expand Up @@ -60,7 +60,7 @@ struct SimulationFeatureList : FeatureList<
> { };

#ifdef DART_HAS_CONTACT_SURFACE
class IgnContactSurfaceHandler : public dart::constraint::ContactSurfaceHandler
class GzContactSurfaceHandler : public dart::constraint::ContactSurfaceHandler
{
public: dart::constraint::ContactSurfaceParams createParams(
const dart::collision::Contact& _contact,
Expand All @@ -81,10 +81,10 @@ class IgnContactSurfaceHandler : public dart::constraint::ContactSurfaceHandler
> convertContact;

public: mutable typename Feature::ContactSurfaceParams<FeaturePolicy3d>
lastIgnParams;
lastGzParams;
};

using IgnContactSurfaceHandlerPtr = std::shared_ptr<IgnContactSurfaceHandler>;
using GzContactSurfaceHandlerPtr = std::shared_ptr<GzContactSurfaceHandler>;
#endif

class SimulationFeatures :
Expand Down Expand Up @@ -127,7 +127,7 @@ class SimulationFeatures :
const Identity &_worldID, const std::string &_callbackID) override;

private: std::unordered_map<
std::string, IgnContactSurfaceHandlerPtr> contactSurfaceHandlers;
std::string, GzContactSurfaceHandlerPtr> contactSurfaceHandlers;
#endif
};

Expand Down

0 comments on commit 2027b5a

Please sign in to comment.