Skip to content

Commit

Permalink
Fix linting errors
Browse files Browse the repository at this point in the history 
Signed-off-by: Aaron Chong <[email protected]>
  • Loading branch information
@aaronchongth @gbiggs
aaronchongth authored and gbiggs committed May 9, 2022
1 parent df30a0c commit 1fb51f4
Show file tree
Hide file tree
Showing 2 changed files with 104 additions and 110 deletions.
82 changes: 37 additions & 45 deletions drake_ros_examples/examples/iiwa_manipulator/iiwa_manipulator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,107 +12,99 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#include <cmath>
#include <memory>
#include <utility>

#include <drake/common/eigen_types.h>
#include <drake/examples/manipulation_station/manipulation_station.h>
#include <drake/systems/analysis/simulator.h>
#include <drake/systems/framework/diagram_builder.h>
#include <drake/systems/primitives/adder.h>
#include <drake/systems/primitives/constant_vector_source.h>
#include <drake/systems/primitives/sine.h>


#include <drake/examples/manipulation_station/manipulation_station.h>

#include <drake_ros_core/drake_ros.h>
#include <drake_ros_core/ros_interface_system.h>
#include <drake_ros_viz/rviz_visualizer.h>

#include <cmath>
#include <memory>
#include <utility>

using drake_ros_core::DrakeRos;
using drake_ros_core::RosInterfaceSystem;
using drake_ros_viz::RvizVisualizer;

using drake::examples::manipulation_station::ManipulationStation;
using drake::systems::Adder;
using drake::systems::ConstantVectorSource;
using drake::systems::Sine;
using drake::systems::Simulator;
using drake::examples::manipulation_station::ManipulationStation;

using drake::systems::Sine;

int main()
{
int main() {
drake::systems::DiagramBuilder<double> builder;

drake_ros_core::init();
auto ros_interface_system = builder.AddSystem<RosInterfaceSystem>(
std::make_unique<DrakeRos>("iiwa_manipulator_node"));
std::make_unique<DrakeRos>("iiwa_manipulator_node"));

auto manipulation_station = builder.AddSystem<ManipulationStation>();
manipulation_station->SetupClutterClearingStation();
manipulation_station->Finalize();

// Make the base joint swing sinusoidally.
auto constant_term = builder.AddSystem<ConstantVectorSource>(
drake::VectorX<double>::Zero(manipulation_station->num_iiwa_joints()));
drake::VectorX<double>::Zero(manipulation_station->num_iiwa_joints()));

drake::VectorX<double> amplitudes =
drake::VectorX<double>::Zero(manipulation_station->num_iiwa_joints());
drake::VectorX<double>::Zero(manipulation_station->num_iiwa_joints());
amplitudes[0] = M_PI / 4.; // == 45 degrees
const drake::VectorX<double> frequencies =
drake::VectorX<double>::Constant(manipulation_station->num_iiwa_joints(), 1.); // Hz
const drake::VectorX<double> frequencies = drake::VectorX<double>::Constant(
manipulation_station->num_iiwa_joints(), 1.); // Hz
const drake::VectorX<double> phases =
drake::VectorX<double>::Zero(manipulation_station->num_iiwa_joints());
drake::VectorX<double>::Zero(manipulation_station->num_iiwa_joints());
auto variable_term = builder.AddSystem<Sine>(amplitudes, frequencies, phases);

auto joint_trajectory_generator =
builder.AddSystem<Adder>(2, manipulation_station->num_iiwa_joints());

builder.Connect(
constant_term->get_output_port(),
joint_trajectory_generator->get_input_port(0));
builder.Connect(
variable_term->get_output_port(0),
joint_trajectory_generator->get_input_port(1));
builder.Connect(
joint_trajectory_generator->get_output_port(),
manipulation_station->GetInputPort("iiwa_position"));
builder.AddSystem<Adder>(2, manipulation_station->num_iiwa_joints());

builder.Connect(constant_term->get_output_port(),
joint_trajectory_generator->get_input_port(0));
builder.Connect(variable_term->get_output_port(0),
joint_trajectory_generator->get_input_port(1));
builder.Connect(joint_trajectory_generator->get_output_port(),
manipulation_station->GetInputPort("iiwa_position"));

auto rviz_visualizer = builder.AddSystem<RvizVisualizer>(
ros_interface_system->get_ros_interface());
ros_interface_system->get_ros_interface());

rviz_visualizer->RegisterMultibodyPlant(
&manipulation_station->get_multibody_plant());
&manipulation_station->get_multibody_plant());

builder.Connect(
manipulation_station->GetOutputPort("query_object"),
rviz_visualizer->get_graph_query_port());
builder.Connect(manipulation_station->GetOutputPort("query_object"),
rviz_visualizer->get_graph_query_port());

auto diagram = builder.Build();
auto context = diagram->CreateDefaultContext();

auto simulator = std::make_unique<Simulator<double>>(*diagram, std::move(context));
auto simulator =
std::make_unique<Simulator<double>>(*diagram, std::move(context));
simulator->set_target_realtime_rate(1.0);
simulator->Initialize();

auto & simulator_context = simulator->get_mutable_context();
auto& simulator_context = simulator->get_mutable_context();

auto & manipulation_station_context =
diagram->GetMutableSubsystemContext(*manipulation_station, &simulator_context);
auto& manipulation_station_context = diagram->GetMutableSubsystemContext(
*manipulation_station, &simulator_context);

auto & constant_term_context =
diagram->GetMutableSubsystemContext(*constant_term, &simulator_context);
auto& constant_term_context =
diagram->GetMutableSubsystemContext(*constant_term, &simulator_context);

// Fix gripper joints' position.
manipulation_station->GetInputPort("wsg_position")
.FixValue(&manipulation_station_context, 0.);
.FixValue(&manipulation_station_context, 0.);

// Use default positions for every joint but the base joint.
drake::systems::BasicVector<double> & constants =
constant_term->get_mutable_source_value(&constant_term_context);
drake::systems::BasicVector<double>& constants =
constant_term->get_mutable_source_value(&constant_term_context);
constants.set_value(
manipulation_station->GetIiwaPosition(manipulation_station_context));
manipulation_station->GetIiwaPosition(manipulation_station_context));
constants.get_mutable_value()[0] = -M_PI / 4.;

while (true) {
Expand Down
132 changes: 67 additions & 65 deletions drake_ros_examples/examples/rs_flip_flop/rs_flip_flop.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,44 +11,40 @@
// 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 <memory>
#include <utility>

#include <drake/systems/analysis/simulator.h>
#include <drake/systems/framework/diagram_builder.h>
#include <drake/systems/framework/leaf_system.h>

#include <drake_ros_core/drake_ros.h>
#include <drake_ros_core/ros_interface_system.h>
#include <drake_ros_core/ros_publisher_system.h>
#include <drake_ros_core/ros_subscriber_system.h>

#include <std_msgs/msg/bool.hpp>

#include <memory>
#include <utility>

using drake_ros_core::DrakeRos;
using drake_ros_core::RosInterfaceSystem;
using drake_ros_core::RosPublisherSystem;
using drake_ros_core::RosSubscriberSystem;

class NorGate : public drake::systems::LeafSystem<double>
{
public:
NorGate()
{
DeclareAbstractInputPort("A", *drake::AbstractValue::Make(std_msgs::msg::Bool()));
DeclareAbstractInputPort("B", *drake::AbstractValue::Make(std_msgs::msg::Bool()));
class NorGate : public drake::systems::LeafSystem<double> {
public:
NorGate() {
DeclareAbstractInputPort(
"A", *drake::AbstractValue::Make(std_msgs::msg::Bool()));
DeclareAbstractInputPort(
"B", *drake::AbstractValue::Make(std_msgs::msg::Bool()));

DeclareAbstractOutputPort("Q", &NorGate::calc_output_value);
}

virtual ~NorGate() = default;

private:
void
calc_output_value(
const drake::systems::Context<double> & context,
std_msgs::msg::Bool * output) const
{
private:
void calc_output_value(const drake::systems::Context<double>& context,
std_msgs::msg::Bool* output) const {
const bool a = GetInputPort("A").Eval<std_msgs::msg::Bool>(context).data;
const bool b = GetInputPort("B").Eval<std_msgs::msg::Bool>(context).data;
output->data = !(a || b);
Expand All @@ -57,48 +53,46 @@ class NorGate : public drake::systems::LeafSystem<double>

// Delay's input port by one timestep to avoid algebraic loop error
// Inspired by Simulink's Memory block
template<typename T>
class Memory : public drake::systems::LeafSystem<double>
{
public:
explicit Memory(const T & initial_value)
{
template <typename T>
class Memory : public drake::systems::LeafSystem<double> {
public:
explicit Memory(const T& initial_value) {
DeclareAbstractInputPort("value", *drake::AbstractValue::Make(T()));

// State for value
DeclareAbstractState(drake::Value<T>(initial_value));

// Output depends only on the previous state
DeclareAbstractOutputPort("value", &Memory::calc_output_value, {all_state_ticket()});

DeclarePerStepEvent(
drake::systems::UnrestrictedUpdateEvent<double>(
[this](
const drake::systems::Context<double> & context,
const drake::systems::UnrestrictedUpdateEvent<double> &,
drake::systems::State<double> * state) {
DeclareAbstractOutputPort("value", &Memory::calc_output_value,
{all_state_ticket()});

DeclarePerStepEvent(drake::systems::UnrestrictedUpdateEvent<double>(
[this](const drake::systems::Context<double>& context,
const drake::systems::UnrestrictedUpdateEvent<double>&,
drake::systems::State<double>* state) {
// Copy input value to state
drake::systems::AbstractValues & abstract_state = state->get_mutable_abstract_state();
drake::systems::AbstractValues& abstract_state =
state->get_mutable_abstract_state();
abstract_state.get_mutable_value(0).SetFrom(
get_input_port().Eval<drake::AbstractValue>(context));
get_input_port().Eval<drake::AbstractValue>(context));
}));
}

virtual ~Memory() = default;

private:
void
calc_output_value(const drake::systems::Context<double> & context, T * output) const
{
private:
void calc_output_value(const drake::systems::Context<double>& context,
T* output) const {
*output = context.get_abstract_state().get_value(0).get_value<T>();
}
};

int main()
{
int main() {
// NOR gate RS flip flop example
// Input topics /S and /R are active high (true is logic 1 and false is logic 0)
// Output topics /Q and /Q_not are active low (true is logic 0 and false is logic 1)
// Input topics /S and /R are active high (true is logic 1 and false is logic
// 0)
// Output topics /Q and /Q_not are active low (true is logic 0 and false is
// logic 1)

// Input/Output table
// S: false R: false | Q: no change Q_not: no change
Expand All @@ -112,42 +106,50 @@ int main()
drake_ros_core::init();
auto sys_ros_interface = builder.AddSystem<RosInterfaceSystem>(
std::make_unique<DrakeRos>("rs_flip_flop_node"));
auto sys_pub_Q = builder.AddSystem(
RosPublisherSystem::Make<std_msgs::msg::Bool>(
"Q", qos, sys_ros_interface->get_ros_interface()));
auto sys_pub_Q_not = builder.AddSystem(
RosPublisherSystem::Make<std_msgs::msg::Bool>(
"Q_not", qos, sys_ros_interface->get_ros_interface()));
auto sys_sub_S = builder.AddSystem(
RosSubscriberSystem::Make<std_msgs::msg::Bool>(
"S", qos, sys_ros_interface->get_ros_interface()));
auto sys_sub_R = builder.AddSystem(
RosSubscriberSystem::Make<std_msgs::msg::Bool>(
"R", qos, sys_ros_interface->get_ros_interface()));
auto sys_pub_Q =
builder.AddSystem(RosPublisherSystem::Make<std_msgs::msg::Bool>(
"Q", qos, sys_ros_interface->get_ros_interface()));
auto sys_pub_Q_not =
builder.AddSystem(RosPublisherSystem::Make<std_msgs::msg::Bool>(
"Q_not", qos, sys_ros_interface->get_ros_interface()));
auto sys_sub_S =
builder.AddSystem(RosSubscriberSystem::Make<std_msgs::msg::Bool>(
"S", qos, sys_ros_interface->get_ros_interface()));
auto sys_sub_R =
builder.AddSystem(RosSubscriberSystem::Make<std_msgs::msg::Bool>(
"R", qos, sys_ros_interface->get_ros_interface()));
auto sys_nor_gate_1 = builder.AddSystem<NorGate>();
auto sys_nor_gate_2 = builder.AddSystem<NorGate>();
auto sys_memory = builder.AddSystem<Memory<std_msgs::msg::Bool>>(std_msgs::msg::Bool());
auto sys_memory =
builder.AddSystem<Memory<std_msgs::msg::Bool>>(std_msgs::msg::Bool());

builder.Connect(sys_nor_gate_1->GetOutputPort("Q"), sys_memory->get_input_port(0));
builder.Connect(sys_nor_gate_1->GetOutputPort("Q"),
sys_memory->get_input_port(0));

builder.Connect(sys_sub_S->get_output_port(0), sys_nor_gate_1->GetInputPort("A"));
builder.Connect(sys_nor_gate_2->GetOutputPort("Q"), sys_nor_gate_1->GetInputPort("B"));
builder.Connect(sys_sub_S->get_output_port(0),
sys_nor_gate_1->GetInputPort("A"));
builder.Connect(sys_nor_gate_2->GetOutputPort("Q"),
sys_nor_gate_1->GetInputPort("B"));

builder.Connect(sys_memory->get_output_port(0), sys_nor_gate_2->GetInputPort("A"));
builder.Connect(sys_sub_R->get_output_port(0), sys_nor_gate_2->GetInputPort("B"));
builder.Connect(sys_memory->get_output_port(0),
sys_nor_gate_2->GetInputPort("A"));
builder.Connect(sys_sub_R->get_output_port(0),
sys_nor_gate_2->GetInputPort("B"));

builder.Connect(sys_nor_gate_1->GetOutputPort("Q"), sys_pub_Q->get_input_port(0));
builder.Connect(sys_nor_gate_2->GetOutputPort("Q"), sys_pub_Q_not->get_input_port(0));
builder.Connect(sys_nor_gate_1->GetOutputPort("Q"),
sys_pub_Q->get_input_port(0));
builder.Connect(sys_nor_gate_2->GetOutputPort("Q"),
sys_pub_Q_not->get_input_port(0));

auto diagram = builder.Build();
auto context = diagram->CreateDefaultContext();

auto simulator =
std::make_unique<drake::systems::Simulator<double>>(*diagram, std::move(context));
auto simulator = std::make_unique<drake::systems::Simulator<double>>(
*diagram, std::move(context));
simulator->set_target_realtime_rate(1.0);
simulator->Initialize();

auto & simulator_context = simulator->get_mutable_context();
auto& simulator_context = simulator->get_mutable_context();

while (true) {
simulator->AdvanceTo(simulator_context.get_time() + 0.1);
Expand Down

0 comments on commit 1fb51f4

Please sign in to comment.