Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Set collision detector and solver from SDF #684

Merged
merged 10 commits into from
Jun 16, 2021
173 changes: 173 additions & 0 deletions examples/worlds/physics_options.sdf
Original file line number Diff line number Diff line change
@@ -0,0 +1,173 @@
<?xml version="1.0" ?>
<!--
Demo using custom physics options
-->
<sdf version="1.8">
<world name="shapes">
<physics name="1ms" type="ignored">
<max_step_size>0.001</max_step_size>
<real_time_factor>1.0</real_time_factor>
<dart>
<collision_detector>bullet</collision_detector>
<solver>
<solver_type>pgs</solver_type>
</solver>
</dart>
</physics>

<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>

<model name="ground_plane">
<static>true</static>
<pose>0 0 -0.5 0 -0.52 0</pose>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
</collision>
<visual name="visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
</model>

<model name="box">
<pose>0 0 0.5 0 0 0</pose>
<link name="box_link">
<inertial>
<inertia>
<ixx>1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1</iyy>
<iyz>0</iyz>
<izz>1</izz>
</inertia>
<mass>1.0</mass>
</inertial>
<collision name="box_collision">
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
</collision>

<visual name="box_visual">
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<material>
<ambient>1 0 0 1</ambient>
<diffuse>1 0 0 1</diffuse>
<specular>1 0 0 1</specular>
</material>
</visual>
</link>
</model>

<model name="cylinder">
<pose>0 -1.5 0.5 1.57079632679 0 0</pose>
<link name="cylinder_link">
<inertial>
<inertia>
<ixx>2</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>2</iyy>
<iyz>0</iyz>
<izz>2</izz>
</inertia>
<mass>2.0</mass>
</inertial>
<collision name="cylinder_collision">
<geometry>
<cylinder>
<radius>0.5</radius>
<length>1.0</length>
</cylinder>
</geometry>
</collision>

<visual name="cylinder_visual">
<geometry>
<cylinder>
<radius>0.5</radius>
<length>1.0</length>
</cylinder>
</geometry>
<material>
<ambient>0 1 0 1</ambient>
<diffuse>0 1 0 1</diffuse>
<specular>0 1 0 1</specular>
</material>
</visual>
</link>
</model>

<model name="sphere">
<pose>0 1.5 0.5 0 0 0</pose>
<link name="sphere_link">
<inertial>
<inertia>
<ixx>3</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>3</iyy>
<iyz>0</iyz>
<izz>3</izz>
</inertia>
<mass>3.0</mass>
</inertial>
<collision name="sphere_collision">
<geometry>
<sphere>
<radius>0.5</radius>
</sphere>
</geometry>
</collision>

<visual name="sphere_visual">
<geometry>
<sphere>
<radius>0.5</radius>
</sphere>
</geometry>
<material>
<ambient>0 0 1 1</ambient>
<diffuse>0 0 1 1</diffuse>
<specular>0 0 1 1</specular>
</material>
</visual>
</link>
</model>
</world>
</sdf>
17 changes: 17 additions & 0 deletions include/ignition/gazebo/components/Physics.hh
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,8 @@
#ifndef IGNITION_GAZEBO_COMPONENTS_PHYSICS_HH_
#define IGNITION_GAZEBO_COMPONENTS_PHYSICS_HH_

#include <string>

#include <ignition/msgs/physics.pb.h>

#include <sdf/Physics.hh>
Expand Down Expand Up @@ -48,6 +50,21 @@ namespace components
serializers::PhysicsSerializer>;
IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Physics",
Physics)

/// \brief The name of the collision detector to be used. The supported
/// options will depend on the physics engine being used.
using PhysicsCollisionDetector = Component<std::string,
class PhysicsCollisionDetectorTag, serializers::StringSerializer>;
IGN_GAZEBO_REGISTER_COMPONENT(
"ign_gazebo_components.PhysicsCollisionDetector",
PhysicsCollisionDetector)

/// \brief The name of the solver to be used. The supported options will
/// depend on the physics engine being used.
using PhysicsSolver = Component<std::string,
class PhysicsSolverTag, serializers::StringSerializer>;
IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.PhysicsSolver",
PhysicsSolver)
}
}
}
Expand Down
28 changes: 28 additions & 0 deletions src/LevelManager.cc
Original file line number Diff line number Diff line change
Expand Up @@ -111,6 +111,34 @@ void LevelManager::ReadLevelPerformerInfo()
this->runner->entityCompMgr.CreateComponent(this->worldEntity,
components::Physics(*physics));

// Populate physics options that aren't accessible outside the Element()
// See https://github.com/osrf/sdformat/issues/508
if (physics->Element())
{
if (physics->Element()->HasElement("dart"))
chapulina marked this conversation as resolved.
Show resolved Hide resolved
{
auto dartElem = physics->Element()->GetElement("dart");

if (dartElem->HasElement("collision_detector"))
{
auto collisionDetector =
dartElem->Get<std::string>("collision_detector");

this->runner->entityCompMgr.CreateComponent(worldEntity,
components::PhysicsCollisionDetector(collisionDetector));
}
if (dartElem->HasElement("solver") &&
dartElem->GetElement("solver")->HasElement("solver_type"))
{
auto solver =
dartElem->GetElement("solver")->Get<std::string>("solver_type");

this->runner->entityCompMgr.CreateComponent(worldEntity,
components::PhysicsSolver(solver));
}
}
}

this->runner->entityCompMgr.CreateComponent(this->worldEntity,
components::MagneticField(this->runner->sdfWorld->MagneticField()));

Expand Down
28 changes: 28 additions & 0 deletions src/SdfEntityCreator.cc
Original file line number Diff line number Diff line change
Expand Up @@ -279,6 +279,34 @@ Entity SdfEntityCreator::CreateEntities(const sdf::World *_world)
this->dataPtr->ecm->CreateComponent(worldEntity,
components::Physics(*physics));

// Populate physics options that aren't accessible outside the Element()
// See https://github.com/osrf/sdformat/issues/508
if (physics->Element())
{
if (physics->Element()->HasElement("dart"))
{
chapulina marked this conversation as resolved.
Show resolved Hide resolved
auto dartElem = physics->Element()->GetElement("dart");

if (dartElem->HasElement("collision_detector"))
{
auto collisionDetector =
dartElem->Get<std::string>("collision_detector");

this->dataPtr->ecm->CreateComponent(worldEntity,
components::PhysicsCollisionDetector(collisionDetector));
}
if (dartElem->HasElement("solver") &&
dartElem->GetElement("solver")->HasElement("solver_type"))
{
auto solver =
dartElem->GetElement("solver")->Get<std::string>("solver_type");

this->dataPtr->ecm->CreateComponent(worldEntity,
components::PhysicsSolver(solver));
}
}
}

// MagneticField
this->dataPtr->ecm->CreateComponent(worldEntity,
components::MagneticField(_world->MagneticField()));
Expand Down
31 changes: 31 additions & 0 deletions src/gui/plugins/component_inspector/ComponentInspector.cc
Original file line number Diff line number Diff line change
Expand Up @@ -49,8 +49,11 @@
#include "ignition/gazebo/components/Performer.hh"
#include "ignition/gazebo/components/PerformerAffinity.hh"
#include "ignition/gazebo/components/Physics.hh"
#include "ignition/gazebo/components/PhysicsEnginePlugin.hh"
#include "ignition/gazebo/components/Pose.hh"
#include "ignition/gazebo/components/PoseCmd.hh"
#include "ignition/gazebo/components/RenderEngineGuiPlugin.hh"
#include "ignition/gazebo/components/RenderEngineServerPlugin.hh"
#include "ignition/gazebo/components/SelfCollide.hh"
#include "ignition/gazebo/components/Sensor.hh"
#include "ignition/gazebo/components/SourceFilePath.hh"
Expand Down Expand Up @@ -631,12 +634,40 @@ void ComponentInspector::Update(const UpdateInfo &,
if (comp)
setData(item, comp->Data());
}
else if (typeId == components::PhysicsCollisionDetector::typeId)
{
auto comp = _ecm.Component<components::PhysicsCollisionDetector>(
this->dataPtr->entity);
if (comp)
setData(item, comp->Data());
}
else if (typeId == components::PhysicsSolver::typeId)
{
auto comp = _ecm.Component<components::PhysicsSolver>(
this->dataPtr->entity);
if (comp)
setData(item, comp->Data());
}
else if (typeId == components::Pose::typeId)
{
auto comp = _ecm.Component<components::Pose>(this->dataPtr->entity);
if (comp)
setData(item, comp->Data());
}
else if (typeId == components::RenderEngineGuiPlugin::typeId)
{
auto comp = _ecm.Component<components::RenderEngineGuiPlugin>(
this->dataPtr->entity);
if (comp)
setData(item, comp->Data());
}
else if (typeId == components::RenderEngineServerPlugin::typeId)
{
auto comp = _ecm.Component<components::RenderEngineServerPlugin>(
this->dataPtr->entity);
if (comp)
setData(item, comp->Data());
}
else if (typeId == components::Static::typeId)
{
auto comp = _ecm.Component<components::Static>(this->dataPtr->entity);
Expand Down
Loading