Skip to content

Commit

Permalink
5 ➡️ 6 (main)
Browse files Browse the repository at this point in the history 
Signed-off-by: Louise Poubel <louise@openrobotics.org>
  • Loading branch information
@chapulina
chapulina committed Dec 28, 2021
2 parents acf393d + 20a9d69 commit eeefd45
Show file tree
Hide file tree
Showing 11 changed files with 763 additions and 59 deletions.
48 changes: 48 additions & 0 deletions Changelog.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,26 @@

### Ignition Physics 5.x.x (20XX-XX-XX)

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

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

1. Added DART feature for setting joint limits dynamically.
* [Pull request #260](https://github.com/ignitionrobotics/ign-physics/pull/260)

1. Allow customization of contact surface properties
* [Pull request #267](https://github.com/ignitionrobotics/ign-physics/pull/267)

1. Avoid `auto` usage in Eigen expressions.
* [Pull request #301](https://github.com/ignitionrobotics/ign-physics/pull/301)

1. Fix cmake script for bullet
* [Pull request #297](https://github.com/ignitionrobotics/ign-physics/pull/297)

1. Improved README.md folder structure
* [Pull request #295](https://github.com/ignitionrobotics/ign-physics/pull/295)

### Ignition Physics 5.0.0 (2021-09-30)

1. Add GetJointTransmittedWrench feature
Expand Down Expand Up @@ -274,6 +294,34 @@

### Ignition Physics 2.x.x (20XX-XX-XX)

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

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

1. Added DART feature for setting joint limits dynamically.
* [Pull request #260](https://github.com/ignitionrobotics/ign-physics/pull/260)

1. Allow customization of contact surface properties
* [Pull request #267](https://github.com/ignitionrobotics/ign-physics/pull/267)

1. [dartsim] Add support for joint frame semantics
* [Pull request #288](https://github.com/ignitionrobotics/ign-physics/pull/288)

1. Use slip compliance API's available in upstream dartsim release
* [Pull request #265](https://github.com/ignitionrobotics/ign-physics/pull/265)

1. Fix dart deprecation warning
* [Pull request #263](https://github.com/ignitionrobotics/ign-physics/pull/263)

1. [Citadel] Update tutorials
* [Pull request #204](https://github.com/ignitionrobotics/ign-physics/pull/204)

1. Infrastructure
* [Pull request #257](https://github.com/ignitionrobotics/ign-physics/pull/257)
* [Pull request #281](https://github.com/ignitionrobotics/ign-physics/pull/281)
* [Pull request #287](https://github.com/ignitionrobotics/ign-physics/pull/287)

### Ignition Physics 2.4.0 (2021-04-14)

1. [TPE] Update link pose and velocity
Expand Down
2 changes: 1 addition & 1 deletion bullet/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,7 @@ set(versioned ${CMAKE_SHARED_LIBRARY_PREFIX}${bullet_plugin}${CMAKE_SHARED_LIBRA
set(unversioned ${CMAKE_SHARED_LIBRARY_PREFIX}${PROJECT_NAME_NO_VERSION_LOWER}-${engine_name}${CMAKE_SHARED_LIBRARY_SUFFIX})
if (WIN32)
# disable MSVC inherit via dominance warning
target_compile_options(${dartsim_plugin} PUBLIC "/wd4250")
target_compile_options(${bullet_plugin} PUBLIC "/wd4250")
INSTALL(CODE "EXECUTE_PROCESS(COMMAND ${CMAKE_COMMAND} -E copy
${IGNITION_PHYSICS_ENGINE_INSTALL_DIR}\/${versioned}
${IGNITION_PHYSICS_ENGINE_INSTALL_DIR}\/${unversioned})")
Expand Down
25 changes: 24 additions & 1 deletion dartsim/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,3 @@

# This component expresses custom features of the dartsim plugin, which can
# expose native dartsim data types.
ign_add_component(dartsim INTERFACE
Expand Down Expand Up @@ -38,6 +37,26 @@ target_link_libraries(${dartsim_plugin}
ignition-common${IGN_COMMON_VER}::profiler
)

# The ignition fork of DART contains additional code that allows customizing
# contact constraints. We check for the presence of "ContactSurface.hpp", which
# was added to enable these customizations, to detect if the feature is
# available.
include(CheckIncludeFileCXX)
if (MSVC)
set(CMAKE_REQUIRED_FLAGS "/std:c++14")
else()
set(CMAKE_REQUIRED_FLAGS "-std=c++14")
endif()
set(CMAKE_REQUIRED_INCLUDES "${DART_INCLUDE_DIRS};${EIGEN3_INCLUDE_DIRS}")
set(CMAKE_REQUIRED_QUIET false)

CHECK_INCLUDE_FILE_CXX(dart/constraint/ContactSurface.hpp DART_HAS_CONTACT_SURFACE_HEADER)
if (DART_HAS_CONTACT_SURFACE_HEADER)
target_compile_definitions(${dartsim_plugin} PRIVATE DART_HAS_CONTACT_SURFACE)
else()
message(STATUS "The version of DART does not support Contact constraint customizations.")
endif()

# Note that plugins are currently being installed in 2 places: /lib and the engine-plugins dir
install(TARGETS ${dartsim_plugin} DESTINATION ${IGNITION_PHYSICS_ENGINE_INSTALL_DIR})

Expand Down Expand Up @@ -77,6 +96,10 @@ foreach(test ${tests})
"TEST_WORLD_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/worlds/\""
"IGNITION_PHYSICS_RESOURCE_DIR=\"${IGNITION_PHYSICS_RESOURCE_DIR}\"")

if (DART_HAS_CONTACT_SURFACE_HEADER)
target_compile_definitions(${test} PRIVATE DART_HAS_CONTACT_SURFACE)
endif()

# Helps when we want to build a single test after making changes to dartsim_plugin
add_dependencies(${test} ${dartsim_plugin})
endforeach()
Expand Down
217 changes: 189 additions & 28 deletions dartsim/src/SimulationFeatures.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,11 +15,19 @@
*
*/

#include <memory>
#include <string>
#include <unordered_map>
#include <utility>


#include <dart/collision/CollisionObject.hpp>
#include <dart/collision/CollisionResult.hpp>
#include <dart/constraint/ConstraintSolver.hpp>
#include <dart/constraint/ContactConstraint.hpp>
#ifdef DART_HAS_CONTACT_SURFACE
#include <dart/constraint/ContactSurface.hpp>
#endif

#include <ignition/common/Profiler.hh>

Expand Down Expand Up @@ -111,41 +119,194 @@ SimulationFeatures::GetContactsFromLastStep(const Identity &_worldID) const

for (const auto &dtContact : colResult.getContacts())
{
dart::collision::CollisionObject *dtCollObj1 = dtContact.collisionObject1;
dart::collision::CollisionObject *dtCollObj2 = dtContact.collisionObject2;
auto contact = this->convertContact(dtContact);
if (contact)
outContacts.push_back(contact.value());
}

return outContacts;
}

std::optional<SimulationFeatures::ContactInternal>
SimulationFeatures::convertContact(
const dart::collision::Contact& _contact) const
{
auto *dtCollObj1 = _contact.collisionObject1;
auto *dtCollObj2 = _contact.collisionObject2;

const dart::dynamics::ShapeFrame *dtShapeFrame1 =
dtCollObj1->getShapeFrame();
const dart::dynamics::ShapeFrame *dtShapeFrame2 =
dtCollObj2->getShapeFrame();
auto *dtShapeFrame1 = dtCollObj1->getShapeFrame();
auto *dtShapeFrame2 = dtCollObj2->getShapeFrame();

dart::dynamics::ConstBodyNodePtr dtBodyNode1;
dart::dynamics::ConstBodyNodePtr dtBodyNode2;
if (this->shapes.HasEntity(dtShapeFrame1->asShapeNode()) &&
this->shapes.HasEntity(dtShapeFrame2->asShapeNode()))
{
std::size_t shape1ID =
this->shapes.IdentityOf(dtShapeFrame1->asShapeNode());
std::size_t shape2ID =
this->shapes.IdentityOf(dtShapeFrame2->asShapeNode());

CompositeData extraData;

// Add normal, depth and wrench to extraData.
auto& extraContactData =
extraData.Get<SimulationFeatures::ExtraContactData>();
extraContactData.force = _contact.force;
extraContactData.normal = _contact.normal;
extraContactData.depth = _contact.penetrationDepth;


return SimulationFeatures::ContactInternal {
this->GenerateIdentity(shape1ID, this->shapes.at(shape1ID)),
this->GenerateIdentity(shape2ID, this->shapes.at(shape2ID)),
_contact.point, extraData
};
}

return std::nullopt;
}

if (this->shapes.HasEntity(dtShapeFrame1->asShapeNode()) &&
this->shapes.HasEntity(dtShapeFrame2->asShapeNode()))
#ifdef DART_HAS_CONTACT_SURFACE
void SimulationFeatures::AddContactPropertiesCallback(
const Identity& _worldID, const std::string& _callbackID,
SurfaceParamsCallback _callback)
{
auto *world = this->ReferenceInterface<DartWorld>(_worldID);

auto handler = std::make_shared<IgnContactSurfaceHandler>();
handler->surfaceParamsCallback = _callback;
handler->convertContact = [this](const dart::collision::Contact& _contact) {
return this->convertContact(_contact);
};

this->contactSurfaceHandlers[_callbackID] = handler;
world->getConstraintSolver()->addContactSurfaceHandler(handler);
}

bool SimulationFeatures::RemoveContactPropertiesCallback(
const Identity& _worldID, const std::string& _callbackID)
{
auto *world = this->ReferenceInterface<DartWorld>(_worldID);

if (this->contactSurfaceHandlers.find(_callbackID) !=
this->contactSurfaceHandlers.end())
{
const auto handler = this->contactSurfaceHandlers[_callbackID];
this->contactSurfaceHandlers.erase(_callbackID);
return world->getConstraintSolver()->removeContactSurfaceHandler(handler);
}
else
{
ignerr << "Could not find the contact surface handler to be removed"
<< std::endl;
return false;
}
}

dart::constraint::ContactSurfaceParams IgnContactSurfaceHandler::createParams(
const dart::collision::Contact& _contact,
const size_t _numContactsOnCollisionObject) const
{
auto pDart = ContactSurfaceHandler::createParams(
_contact, _numContactsOnCollisionObject);

if (!this->surfaceParamsCallback)
return pDart;

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

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;

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)
pDart.mContactSurfaceMotionVelocity =
pIgn.contactSurfaceMotionVelocity.value();

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

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

static bool warnedTorsionalFrictionCoeff = false;
if (!warnedTorsionalFrictionCoeff && pIgn.torsionalFrictionCoeff)
{
std::size_t shape1ID =
this->shapes.IdentityOf(dtShapeFrame1->asShapeNode());
std::size_t shape2ID =
this->shapes.IdentityOf(dtShapeFrame2->asShapeNode());

CompositeData extraData;

// Add normal, depth and wrench to extraData.
auto &extraContactData = extraData.Get<ExtraContactData>();
extraContactData.force = dtContact.force;
extraContactData.normal = dtContact.normal;
extraContactData.depth = dtContact.penetrationDepth;

outContacts.push_back(
{this->GenerateIdentity(shape1ID, this->shapes.at(shape1ID)),
this->GenerateIdentity(shape2ID, this->shapes.at(shape2ID)),
dtContact.point, extraData});
ignwarn << "DART doesn't support torsional friction setting"
<< std::endl;
warnedTorsionalFrictionCoeff = true;
}
}
return outContacts;

this->lastIgnParams = pIgn;

return pDart;
}

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

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

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

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

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

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

return constraint;
}
#endif

}
}
}
Loading

0 comments on commit eeefd45

Please sign in to comment.