Skip to content

Commit

Permalink
Use std::optional instead of drake::optional in attic and bindings (R…
Browse files Browse the repository at this point in the history
  • Loading branch information
jamiesnape authored Oct 30, 2019
1 parent 7a9e7d8 commit 5034063
Show file tree
Hide file tree
Showing 19 changed files with 87 additions and 79 deletions.
Original file line number Diff line number Diff line change
@@ -1,12 +1,12 @@
#include "drake/manipulation/planner/rbt_differential_inverse_kinematics.h"

#include <memory>
#include <optional>
#include <random>
#include <string>

#include <gtest/gtest.h>

#include "drake/common/drake_optional.h"
#include "drake/common/eigen_types.h"
#include "drake/common/find_resource.h"
#include "drake/common/test_utilities/eigen_matrix_compare.h"
Expand Down Expand Up @@ -78,7 +78,7 @@ class DifferentialInverseKinematicsTest : public ::testing::Test {

const double velocity_tolerance{1e-6};

ASSERT_TRUE(result.joint_velocities != nullopt);
ASSERT_TRUE(result.joint_velocities != std::nullopt);
drake::log()->info("result.joint_velocities = {}",
result.joint_velocities->transpose());

Expand Down Expand Up @@ -149,7 +149,7 @@ TEST_F(DifferentialInverseKinematicsTest, OverConstrainedTest) {
DifferentialInverseKinematicsStatus function_status{function_result.status};
drake::log()->info("function_status = {}", function_status);

ASSERT_TRUE(function_result.joint_velocities == nullopt);
ASSERT_TRUE(function_result.joint_velocities == std::nullopt);
}

TEST_F(DifferentialInverseKinematicsTest, GainTest) {
Expand All @@ -173,7 +173,7 @@ TEST_F(DifferentialInverseKinematicsTest, GainTest) {
DifferentialInverseKinematicsResult function_result =
rbt::DoDifferentialInverseKinematics(
*tree_, *cache_, V_WE_desired, *frame_E_, *params_);
ASSERT_TRUE(function_result.joint_velocities != nullopt);
ASSERT_TRUE(function_result.joint_velocities != std::nullopt);

// Transform the resulting end effector frame's velocity into body frame.
*cache_ = tree_->doKinematics(q, function_result.joint_velocities.value());
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -75,7 +75,7 @@ RandomClutterGenerator::RandomClutterGenerator(

VectorX<double> RandomClutterGenerator::GenerateFloatingClutter(
const VectorX<double>& q_nominal, std::default_random_engine* generator,
optional<double> z_height_cost) {
std::optional<double> z_height_cost) {
DRAKE_DEMAND(scene_tree_ptr_->get_num_positions() == q_nominal.size());

VectorX<double> q_nominal_candidate = q_nominal;
Expand Down Expand Up @@ -150,7 +150,7 @@ int RandomClutterGenerator::ComputeIK(
VectorX<double>* q_result,
const std::vector<RigidBodyConstraint*>& constraint_array,
const VectorX<double>& q_initial, const VectorX<double>& q_nominal,
const std::vector<int>& z_indices, optional<double> z_height_cost) {
const std::vector<int>& z_indices, std::optional<double> z_height_cost) {
VectorX<double> q_result_return;

Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(q_initial.size(), q_initial.size());
Expand Down
Original file line number Diff line number Diff line change
@@ -1,11 +1,11 @@
#pragma once

#include <optional>
#include <random>
#include <set>
#include <string>
#include <vector>

#include "drake/common/drake_optional.h"
#include "drake/common/eigen_types.h"
#include "drake/multibody/rigid_body_constraint.h"
#include "drake/multibody/rigid_body_tree.h"
Expand Down Expand Up @@ -71,16 +71,16 @@ class RandomClutterGenerator {
* configuration.
*/
VectorX<double> GenerateFloatingClutter(const VectorX<double>& q_nominal,
std::default_random_engine* generator,
optional<double> z_height_cost = {});
std::default_random_engine* generator,
std::optional<double> z_height_cost = {});

private:
int ComputeIK(VectorX<double>* q_result,
const std::vector<RigidBodyConstraint*>& constraint_array,
const VectorX<double>& q_initial,
const VectorX<double>& q_nominal,
const std::vector<int>& z_indices,
optional<double> z_height_cost = {});
std::optional<double> z_height_cost = {});

void AddBodyToOrientationConstraint(const RigidBody<double>& body,
VectorX<double>* linear_posture_lb,
Expand Down
27 changes: 15 additions & 12 deletions attic/multibody/rigid_body_plant/compliant_material.h
Original file line number Diff line number Diff line change
@@ -1,8 +1,9 @@
#pragma once

#include <optional>

#include "drake/common/drake_assert.h"
#include "drake/common/drake_copyable.h"
#include "drake/common/drake_optional.h"

namespace drake {
namespace systems {
Expand Down Expand Up @@ -88,10 +89,12 @@ class CompliantMaterial {
constructed instance will report true. Furthermore, it must
return true if set_youngs_modulus_to_default() has been called and false if
set_youngs_modulus() has been called. */
bool youngs_modulus_is_default() const { return youngs_modulus_ == nullopt; }
bool youngs_modulus_is_default() const {
return youngs_modulus_ == std::nullopt;
}

/** Reset the elasticity value to report the default value. */
void set_youngs_modulus_to_default() { youngs_modulus_ = nullopt; }
void set_youngs_modulus_to_default() { youngs_modulus_ = std::nullopt; }

/** Set the dissipation value. Attempting to set a negative value will throw
an exception. Returns a reference to `*this` so that multiple invocations
Expand All @@ -110,10 +113,10 @@ class CompliantMaterial {
constructed instance will report true. Furthermore, it must
return true if set_dissipation_to_default() has been called and false if
set_dissipation() has been called. */
bool dissipation_is_default() const { return dissipation_ == nullopt; }
bool dissipation_is_default() const { return dissipation_ == std::nullopt; }

/** Reset the dissipation value to report the default value. */
void set_dissipation_to_default() { dissipation_ = nullopt; }
void set_dissipation_to_default() { dissipation_ = std::nullopt; }

/** Sets *both* coefficients of friction to the same value.
@throws std::exception if the value is negative. Returns a reference to
Expand Down Expand Up @@ -160,13 +163,13 @@ class CompliantMaterial {
bool friction_is_default() const {
// NOTE: Friction values can only be set in tandem; so if one is default,
// both must be default.
return static_friction_ == nullopt;
return static_friction_ == std::nullopt;
}

/** Reset both friction coefficient values to report the default value. */
void set_friction_to_default() {
static_friction_ = nullopt;
dynamic_friction_ = nullopt;
static_friction_ = std::nullopt;
dynamic_friction_ = std::nullopt;
}

static const double kDefaultYoungsModulus;
Expand All @@ -182,10 +185,10 @@ class CompliantMaterial {
static void ThrowForBadFriction(double static_friction,
double dynamic_friction);

optional<double> youngs_modulus_;
optional<double> dissipation_;
optional<double> static_friction_;
optional<double> dynamic_friction_;
std::optional<double> youngs_modulus_;
std::optional<double> dissipation_;
std::optional<double> static_friction_;
std::optional<double> dynamic_friction_;
};

} // namespace systems
Expand Down
4 changes: 2 additions & 2 deletions attic/multibody/rigid_body_plant/rigid_body_plant.h
Original file line number Diff line number Diff line change
@@ -1,14 +1,14 @@
#pragma once

#include <memory>
#include <optional>
#include <string>
#include <utility>
#include <vector>

#include <Eigen/Geometry>

#include "drake/common/drake_copyable.h"
#include "drake/common/drake_optional.h"
#include "drake/multibody/constraint/constraint_solver.h"
#include "drake/multibody/rigid_body_plant/compliant_contact_model.h"
#include "drake/multibody/rigid_body_plant/kinematics_results.h"
Expand Down Expand Up @@ -544,7 +544,7 @@ class RigidBodyPlant : public LeafSystem<T> {
multibody::constraint::ConstraintSolver<double> constraint_solver_;

OutputPortIndex state_output_port_index_{};
optional<OutputPortIndex> state_derivative_output_port_index_;
std::optional<OutputPortIndex> state_derivative_output_port_index_;
OutputPortIndex kinematics_output_port_index_{};
OutputPortIndex contact_output_port_index_{};

Expand Down
2 changes: 1 addition & 1 deletion attic/systems/sensors/rgbd_camera.cc
Original file line number Diff line number Diff line change
Expand Up @@ -138,7 +138,7 @@ void RgbdCamera::InitRenderer() {
const int body_id = body->get_body_index();
auto& body_visual_indices = body_visual_indices_map_[body_id];
for (const auto& visual : body->get_visual_elements()) {
optional<VisualIndex> visual_index =
std::optional<VisualIndex> visual_index =
renderer_->RegisterVisual(visual, body_id);
if (visual_index) {
body_visual_indices.push_back(*visual_index);
Expand Down
2 changes: 1 addition & 1 deletion attic/systems/sensors/rgbd_renderer.cc
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@ RgbdRenderer::RgbdRenderer(const RenderingConfig& config,

RgbdRenderer::~RgbdRenderer() {}

optional<RgbdRenderer::VisualIndex> RgbdRenderer::RegisterVisual(
std::optional<RgbdRenderer::VisualIndex> RgbdRenderer::RegisterVisual(
const DrakeShapes::VisualElement& visual, int body_id) {
return ImplRegisterVisual(visual, body_id);
}
Expand Down
6 changes: 3 additions & 3 deletions attic/systems/sensors/rgbd_renderer.h
Original file line number Diff line number Diff line change
@@ -1,11 +1,11 @@
#pragma once

#include <limits>
#include <optional>

#include <Eigen/Dense>

#include "drake/common/drake_copyable.h"
#include "drake/common/drake_optional.h"
#include "drake/common/type_safe_index.h"
#include "drake/multibody/shapes/visual_element.h"
#include "drake/systems/sensors/color_palette.h"
Expand Down Expand Up @@ -117,7 +117,7 @@ class RgbdRenderer {
/// `nullopt` will be returned if `visual` contains an unsupported shape.
/// We assume `visual_id` will be used together with `body_id` when you call
/// UpdateVisualPose() later.
optional<VisualIndex> RegisterVisual(
std::optional<VisualIndex> RegisterVisual(
const DrakeShapes::VisualElement& visual, int body_id);

/// Updates the pose of a visual with given pose X_WV.
Expand Down Expand Up @@ -168,7 +168,7 @@ class RgbdRenderer {
private:
virtual void ImplAddFlatTerrain() = 0;

virtual optional<VisualIndex> ImplRegisterVisual(
virtual std::optional<VisualIndex> ImplRegisterVisual(
const DrakeShapes::VisualElement& visual, int body_id) = 0;

virtual void ImplUpdateVisualPose(const Eigen::Isometry3d& X_WV, int body_id,
Expand Down
10 changes: 5 additions & 5 deletions attic/systems/sensors/rgbd_renderer_ospray.cc
Original file line number Diff line number Diff line change
Expand Up @@ -121,7 +121,7 @@ class RgbdRendererOSPRay::Impl : private ModuleInitVtkRenderingOpenGL2 {

void ImplAddFlatTerrain();

optional<VisualIndex> ImplRegisterVisual(
std::optional<VisualIndex> ImplRegisterVisual(
const DrakeShapes::VisualElement& visual, int body_id);

void ImplUpdateVisualPose(const Eigen::Isometry3d& X_WV, int body_id,
Expand Down Expand Up @@ -309,7 +309,7 @@ RgbdRendererOSPRay::Impl::Impl(RgbdRendererOSPRay* parent,
cp->renderer->AddLight(light_);
}

optional<RgbdRenderer::VisualIndex>
std::optional<RgbdRenderer::VisualIndex>
RgbdRendererOSPRay::Impl::ImplRegisterVisual(
const DrakeShapes::VisualElement& visual, int body_id) {
bool shape_matched = true;
Expand Down Expand Up @@ -431,11 +431,11 @@ RgbdRendererOSPRay::Impl::ImplRegisterVisual(
actor_collections[i].push_back(actors_[i].GetPointer());
}

return optional<VisualIndex>(VisualIndex(static_cast<int>(
return std::optional<VisualIndex>(VisualIndex(static_cast<int>(
id_object_maps_[body_id][ImageType::kColor].size() - 1)));
}

return nullopt;
return std::nullopt;
}

RgbdRendererOSPRay::RgbdRendererOSPRay(const RenderingConfig& config,
Expand All @@ -445,7 +445,7 @@ RgbdRendererOSPRay::RgbdRendererOSPRay(const RenderingConfig& config,

RgbdRendererOSPRay::~RgbdRendererOSPRay() {}

optional<RgbdRenderer::VisualIndex> RgbdRendererOSPRay::ImplRegisterVisual(
std::optional<RgbdRenderer::VisualIndex> RgbdRendererOSPRay::ImplRegisterVisual(
const DrakeShapes::VisualElement& visual, int body_id) {
return impl_->ImplRegisterVisual(visual, body_id);
}
Expand Down
2 changes: 1 addition & 1 deletion attic/systems/sensors/rgbd_renderer_ospray.h
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@ class RgbdRendererOSPRay final : public RgbdRenderer {
private:
void ImplAddFlatTerrain() override;

optional<VisualIndex> ImplRegisterVisual(
std::optional<VisualIndex> ImplRegisterVisual(
const DrakeShapes::VisualElement& visual, int body_id) override;

void ImplUpdateVisualPose(const Eigen::Isometry3d& X_WV,
Expand Down
13 changes: 7 additions & 6 deletions attic/systems/sensors/rgbd_renderer_vtk.cc
Original file line number Diff line number Diff line change
Expand Up @@ -161,7 +161,7 @@ class RgbdRendererVTK::Impl : private ModuleInitVtkRenderingOpenGL2 {

void ImplAddFlatTerrain();

optional<VisualIndex> ImplRegisterVisual(
std::optional<VisualIndex> ImplRegisterVisual(
const DrakeShapes::VisualElement& visual, int body_id);

void ImplUpdateVisualPose(const Eigen::Isometry3d& X_WV, int body_id,
Expand Down Expand Up @@ -417,8 +417,9 @@ RgbdRendererVTK::Impl::Impl(RgbdRendererVTK* parent,
static_cast<float>(parent_->config().z_far));
}

optional<RgbdRenderer::VisualIndex> RgbdRendererVTK::Impl::ImplRegisterVisual(
const DrakeShapes::VisualElement& visual, int body_id) {
std::optional<RgbdRenderer::VisualIndex>
RgbdRendererVTK::Impl::ImplRegisterVisual(
const DrakeShapes::VisualElement& visual, int body_id) {
std::array<vtkNew<vtkActor>, 3> actors;
std::array<vtkNew<vtkOpenGLPolyDataMapper>, 3> mappers;
// Sets vertex and fragment shaders only to the depth mapper.
Expand Down Expand Up @@ -561,11 +562,11 @@ optional<RgbdRenderer::VisualIndex> RgbdRendererVTK::Impl::ImplRegisterVisual(
actor_collections[i].push_back(actors[i].GetPointer());
}

return optional<VisualIndex>(VisualIndex(static_cast<int>(
return std::optional<VisualIndex>(VisualIndex(static_cast<int>(
id_object_maps_[body_id][ImageType::kColor].size() - 1)));
}

return nullopt;
return std::nullopt;
}

RgbdRendererVTK::RgbdRendererVTK(const RenderingConfig& config,
Expand All @@ -575,7 +576,7 @@ RgbdRendererVTK::RgbdRendererVTK(const RenderingConfig& config,

RgbdRendererVTK::~RgbdRendererVTK() {}

optional<RgbdRenderer::VisualIndex> RgbdRendererVTK::ImplRegisterVisual(
std::optional<RgbdRenderer::VisualIndex> RgbdRendererVTK::ImplRegisterVisual(
const DrakeShapes::VisualElement& visual, int body_id) {
return impl_->ImplRegisterVisual(visual, body_id);
}
Expand Down
2 changes: 1 addition & 1 deletion attic/systems/sensors/rgbd_renderer_vtk.h
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@ class RgbdRendererVTK final : public RgbdRenderer {
private:
void ImplAddFlatTerrain() override;

optional<VisualIndex> ImplRegisterVisual(
std::optional<VisualIndex> ImplRegisterVisual(
const DrakeShapes::VisualElement& visual, int body_id) override;

void ImplUpdateVisualPose(const Eigen::Isometry3d& X_WV,
Expand Down
2 changes: 1 addition & 1 deletion bindings/pydrake/examples/manipulation_station_py.cc
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@ PYBIND11_MODULE(manipulation_station, m) {
doc.ManipulationStation.SetupManipulationClassStation.doc)
.def("SetupClutterClearingStation",
&ManipulationStation<T>::SetupClutterClearingStation,
py::arg("X_WCameraBody") = nullopt,
py::arg("X_WCameraBody") = std::nullopt,
py::arg("collision_model") = IiwaCollisionModel::kNoCollision,
doc.ManipulationStation.SetupClutterClearingStation.doc)
.def("AddManipulandFromFile",
Expand Down
2 changes: 1 addition & 1 deletion bindings/pydrake/lcm_py.cc
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@ PYBIND11_MODULE(lcm, m) {
// wreak havoc on the Python GIL with a callback.
.def("Publish",
[](Class* self, const std::string& channel, py::bytes buffer,
optional<double> time_sec) {
std::optional<double> time_sec) {
// TODO(eric.cousineau): See if there is a way to extra the raw
// bytes from `buffer` without copying.
std::string str = buffer;
Expand Down
8 changes: 4 additions & 4 deletions bindings/pydrake/multibody/plant_py.cc
Original file line number Diff line number Diff line change
Expand Up @@ -562,17 +562,17 @@ void DoScalarDependentDefinitions(py::module m, T) {
py::arg("model_instance"), cls_doc.GetBodyIndices.doc)
.def("GetJointByName",
[](const Class* self, const string& name,
optional<ModelInstanceIndex> model_instance) -> auto& {
std::optional<ModelInstanceIndex> model_instance) -> auto& {
return self->GetJointByName(name, model_instance);
},
py::arg("name"), py::arg("model_instance") = nullopt,
py::arg("name"), py::arg("model_instance") = std::nullopt,
py_reference_internal, cls_doc.GetJointByName.doc)
.def("GetMutableJointByName",
[](Class * self, const string& name,
optional<ModelInstanceIndex> model_instance) -> auto& {
std::optional<ModelInstanceIndex> model_instance) -> auto& {
return self->GetMutableJointByName(name, model_instance);
},
py::arg("name"), py::arg("model_instance") = nullopt,
py::arg("name"), py::arg("model_instance") = std::nullopt,
py_reference_internal, cls_doc.GetJointByName.doc)
.def("GetJointActuatorByName",
overload_cast_explicit<const JointActuator<T>&, const string&>(
Expand Down
Loading

0 comments on commit 5034063

Please sign in to comment.