From 3edabffb1efafd4356154f52ab4455c6e51a86ca Mon Sep 17 00:00:00 2001 From: pangtao22 Date: Fri, 27 Apr 2018 16:31:58 -0400 Subject: [PATCH] quasistatic iiwa grasping and pick-up demo (#8518) * fixed debug mode assertion failures. * removed manual label in test build rule * iiwa pickup demo now works with new API. * code style * 1. checks final simulation state. 2. removed wrong comments from iiwa arm urdf. * buildify build file * passes Eigen::Quaternion to QuaternionToSpaceXYZ * documented test criteria * declare expected final position as const * compares object final position and orientation to expected values. * typo. * final end effector pose becomes an input to MakePlan(.). * replace deprecated rpy2quat * changes iiwa urdf file name * uses new RollPitchYaw class. * minor changes * replace tabs with spaces and add newline at EOF in .sdf files --- manipulation/dev/BUILD.bazel | 33 ++ manipulation/dev/box_as_table.sdf | 45 ++ .../dev/double_dumbbell_for_pick_up.sdf | 128 ++++++ manipulation/dev/dumbbell_for_pick_up.sdf | 79 ++++ .../quasistatic_iiwa_pick_and_place_demo.cc | 340 +++++++++++++++ manipulation/dev/quasistatic_system.cc | 4 +- .../dev/simplified_schunk_gripper.sdf | 116 ++++++ .../urdf/iiwa14_no_collision.urdf | 387 ++++++++++++++++++ 8 files changed, 1130 insertions(+), 2 deletions(-) create mode 100644 manipulation/dev/box_as_table.sdf create mode 100644 manipulation/dev/double_dumbbell_for_pick_up.sdf create mode 100644 manipulation/dev/dumbbell_for_pick_up.sdf create mode 100644 manipulation/dev/quasistatic_iiwa_pick_and_place_demo.cc create mode 100644 manipulation/dev/simplified_schunk_gripper.sdf create mode 100644 manipulation/models/iiwa_description/urdf/iiwa14_no_collision.urdf diff --git a/manipulation/dev/BUILD.bazel b/manipulation/dev/BUILD.bazel index 17c4eeaf281d..e3d0cd378f20 100644 --- a/manipulation/dev/BUILD.bazel +++ b/manipulation/dev/BUILD.bazel @@ -78,6 +78,39 @@ drake_cc_binary( ], ) +drake_cc_binary( + name = "quasistatic_iiwa_pick_and_place_demo", + testonly = 1, + srcs = ["quasistatic_iiwa_pick_and_place_demo.cc"], + data = [ + ":double_dumbbell_for_pick_up.sdf", + ":dumbbell_for_pick_up.sdf", + ":models", + "//examples/kuka_iiwa_arm:models", + "//manipulation/models/iiwa_description:models", + ], + deps = [ + ":quasistatic_system", + "//common:find_resource", + "//common/test_utilities:eigen_matrix_compare", + "//common/trajectories:piecewise_polynomial", + "//examples/kuka_iiwa_arm:iiwa_common", + "//lcm", + "//manipulation/util:sim_diagram_builder", + "//manipulation/util:world_sim_tree_builder", + "//math:geometric_transform", + "//systems/analysis", + "//systems/controllers:pid_controller", + "//systems/primitives:demultiplexer", + "//systems/primitives:matrix_gain", + "//systems/primitives:multiplexer", + "//systems/primitives:signal_logger", + "//systems/primitives:trajectory_source", + "@lcmtypes_bot2_core", + "@lcmtypes_robotlocomotion", + ], +) + install_data() add_lint_tests() diff --git a/manipulation/dev/box_as_table.sdf b/manipulation/dev/box_as_table.sdf new file mode 100644 index 000000000000..5f4095e3d5f8 --- /dev/null +++ b/manipulation/dev/box_as_table.sdf @@ -0,0 +1,45 @@ + + + + + + 0 0 0.4 0 -0 0 + + + + 0.5 0.5 0.8 + + + + 0.9 0.9 0.9 1 + 0.9 0.9 0.9 1 + + + + + 0.988882 + + 0.162992 + 0 + 0 + 0.162992 + 0 + 0.164814 + + + + + 10 + 0 0 0 0 -0 0 + + + 0.5 0.5 0.8 + + + + + + + + + diff --git a/manipulation/dev/double_dumbbell_for_pick_up.sdf b/manipulation/dev/double_dumbbell_for_pick_up.sdf new file mode 100644 index 000000000000..f88adbaf90ee --- /dev/null +++ b/manipulation/dev/double_dumbbell_for_pick_up.sdf @@ -0,0 +1,128 @@ + + + + + + + 1 + + 0.05 + 0 + 0 + 0.05 + 0 + 0.05 + + + + 0 0 0 0 -0 0 + + + + 0.02 0.015 0 0 -0 0 + + + 0.015 + + + + 0 1 0 1 + 0 1 0 1 + + + + + 0.02 -0.015 0 0 -0 0 + + + 0.015 + + + + 0 1 0 1 + 0 1 0 1 + + + + + -0.02 0.015 0 0 -0 0 + + + 0.015 + + + + 0 1 0 1 + 0 1 0 1 + + + + + -0.02 -0.015 0 0 -0 0 + + + 0.015 + + + + 0 1 0 1 + 0 1 0 1 + + + + + + 0.02 0.015 0 0 -0 0 + + + 0.015 + + + + + + 0.02 -0.015 0 0 -0 0 + + + 0.015 + + + + + + -0.02 0.015 0 0 -0 0 + + + 0.015 + + + + + + -0.02 -0.015 0 0 -0 0 + + + 0.015 + + + + + + + + + diff --git a/manipulation/dev/dumbbell_for_pick_up.sdf b/manipulation/dev/dumbbell_for_pick_up.sdf new file mode 100644 index 000000000000..fb1a07a96714 --- /dev/null +++ b/manipulation/dev/dumbbell_for_pick_up.sdf @@ -0,0 +1,79 @@ + + + + + + + 0.988882 + + 0.162992 + 0 + 0 + 0.162992 + 0 + 0.164814 + + + + 0 0 0 0 -0 0 + + + 0 0 0 0 1.5708 0 + + + 0.015 + 0.07 + + + + + + + + + + + -0.02 0 0 0 -0 0 + + + 0.015 + + + + + + 0.02 0 0 0 -0 0 + + + 0.015 + + + + + + + + \ No newline at end of file diff --git a/manipulation/dev/quasistatic_iiwa_pick_and_place_demo.cc b/manipulation/dev/quasistatic_iiwa_pick_and_place_demo.cc new file mode 100644 index 000000000000..978905bd60bb --- /dev/null +++ b/manipulation/dev/quasistatic_iiwa_pick_and_place_demo.cc @@ -0,0 +1,340 @@ +#include + +#include "drake/common/eigen_types.h" +#include "drake/common/find_resource.h" +#include "drake/common/test_utilities/eigen_matrix_compare.h" +#include "drake/common/trajectories/piecewise_polynomial.h" +#include "drake/examples/kuka_iiwa_arm/iiwa_common.h" +#include "drake/lcm/drake_lcm.h" +#include "drake/manipulation/dev/quasistatic_system.h" +#include "drake/manipulation/util/sim_diagram_builder.h" +#include "drake/manipulation/util/world_sim_tree_builder.h" +#include "drake/math/quaternion.h" +#include "drake/math/roll_pitch_yaw.h" +#include "drake/math/rotation_matrix.h" +#include "drake/multibody/parsers/urdf_parser.h" +#include "drake/multibody/rigid_body_plant/drake_visualizer.h" +#include "drake/systems/analysis/simulator.h" +#include "drake/systems/primitives/demultiplexer.h" +#include "drake/systems/primitives/matrix_gain.h" +#include "drake/systems/primitives/multiplexer.h" +#include "drake/systems/primitives/signal_logger.h" +#include "drake/systems/primitives/trajectory_source.h" + +namespace drake { +namespace manipulation { +namespace quasistatic_kuka_iiwa_arm { +namespace { +using Eigen::MatrixXd; +using Eigen::Vector2d; +using Eigen::Vector3d; +using Eigen::VectorXd; +using Eigen::VectorXi; +using manipulation::util::ModelInstanceInfo; +using manipulation::util::SimDiagramBuilder; +using manipulation::util::WorldSimTreeBuilder; +using math::RollPitchYaw; +using trajectories::PiecewisePolynomial; + +const Eigen::Vector3d kTableBase(0.82, 0, 0); +const Eigen::Vector3d kDumbbellBase(0.5 + 0.0725 + 0.035, 0, 0.815); + +const char kUrdfPath[] = + "drake/manipulation/models/iiwa_description/urdf/" + "/iiwa14_no_collision.urdf"; + +const char kUrdfPathDumbbell[] = + "drake/manipulation/dev/double_dumbbell_for_pick_up.sdf"; + +std::unique_ptr BuildSimWorld() { + auto tree_builder = std::make_unique>(); + tree_builder->StoreDrakeModel("iiwa", kUrdfPath); + tree_builder->StoreDrakeModel( + "wsg", "drake/manipulation/dev/simplified_schunk_gripper.sdf"); + tree_builder->StoreDrakeModel("table", + "drake/manipulation/dev/box_as_table" + ".sdf"); + tree_builder->StoreDrakeModel("dumbbell", kUrdfPathDumbbell); + + tree_builder->AddFloatingModelInstance("dumbbell", Vector3d::Zero()); + tree_builder->AddFixedModelInstance("table", kTableBase); + int id_iiwa = + tree_builder->AddFixedModelInstance("iiwa", Vector3(0, 0, 0)); + tree_builder->AddModelInstanceToFrame( + "wsg", tree_builder->tree().findFrame("iiwa_frame_ee", id_iiwa), + drake::multibody::joints::kFixed); + + return tree_builder->Build(); +} + +const std::vector kTimes{0.0, 2.0, 4.0, 6.0, 12.0}; + +// Creates a basic pointwise IK trajectory for moving the iiwa arm. +// It starts in the zero configuration (straight up). +PiecewisePolynomial MakePlan( + const Vector3d& pos_end_final, const RollPitchYaw& rpy_end_final) { + auto tree = std::make_unique>(); + parsers::urdf::AddModelInstanceFromUrdfFileToWorld( + FindResourceOrThrow(kUrdfPath), multibody::joints::kFixed, tree.get()); + + VectorXd zero_conf = tree->getZeroConfiguration(); + VectorXd joint_lb = zero_conf - VectorXd::Constant(7, 0.005); + VectorXd joint_ub = zero_conf + VectorXd::Constant(7, 0.005); + + PostureConstraint pc1(tree.get(), Vector2d(0, 0.5)); + VectorXi joint_idx(7); + joint_idx << 0, 1, 2, 3, 4, 5, 6; + pc1.setJointLimits(joint_idx, joint_lb, joint_ub); + + Vector3d pos_end(0.48, 0, 0.804); + Vector3d pos_lb = pos_end - Vector3d::Constant(0.002); + Vector3d pos_ub = pos_end + Vector3d::Constant(0.002); + WorldPositionConstraint wpc1(tree.get(), tree->FindBodyIndex("iiwa_link_ee"), + Vector3d::Zero(), pos_lb, pos_ub, + Vector2d(1.5, 4.0)); + const RollPitchYaw rpy_end1(0, 0, 0); + Eigen::Quaterniond qE = rpy_end1.ToQuaternion(); + Eigen::Vector4d quat_end(qE.w(), qE.x(), qE.y(), qE.z()); + WorldQuatConstraint wqc1(tree.get(), tree->FindBodyIndex("iiwa_link_ee"), + quat_end, 0.002, Vector2d(1.5, 5.0)); + + pos_end << 0.4, 0, 0.9; + pos_lb = pos_end - Vector3d::Constant(0.002); + pos_ub = pos_end + Vector3d::Constant(0.002); + WorldPositionConstraint wpc2(tree.get(), tree->FindBodyIndex("iiwa_link_ee"), + Vector3d::Zero(), pos_lb, pos_ub, + Vector2d(5.5, 6.0)); + const RollPitchYaw rpy_end2(0, -M_PI / 2, 0); + qE = rpy_end2.ToQuaternion(); + quat_end << qE.w(), qE.x(), qE.y(), qE.z(); + WorldQuatConstraint wqc2(tree.get(), tree->FindBodyIndex("iiwa_link_ee"), + quat_end, 0.002, Vector2d(5.5, 6.0)); + + pos_end = pos_end_final; + pos_lb = pos_end - Vector3d::Constant(0.002); + pos_ub = pos_end + Vector3d::Constant(0.002); + WorldPositionConstraint wpc4(tree.get(), tree->FindBodyIndex("iiwa_link_ee"), + Vector3d::Zero(), pos_lb, pos_ub, + Vector2d(11.5, 12.0)); + + qE = rpy_end_final.ToQuaternion(); + quat_end << qE.w(), qE.x(), qE.y(), qE.z(); + WorldQuatConstraint wqc3(tree.get(), tree->FindBodyIndex("iiwa_link_ee"), + quat_end, 0.002, Vector2d(11.5, 12.0)); + + MatrixXd q0(tree->get_num_positions(), kTimes.size()); + for (size_t i = 0; i < kTimes.size(); ++i) { + q0.col(i) = zero_conf; + } + + std::vector constraint_array; + constraint_array.push_back(&pc1); + constraint_array.push_back(&wpc1); + constraint_array.push_back(&wqc1); + constraint_array.push_back(&wpc2); + constraint_array.push_back(&wqc2); + constraint_array.push_back(&wpc4); + constraint_array.push_back(&wqc3); + + IKoptions ikoptions(tree.get()); + std::vector info(kTimes.size(), 0); + MatrixXd q_sol(tree->get_num_positions(), kTimes.size()); + + std::vector infeasible_constraint; + + inverseKinPointwise(tree.get(), kTimes.size(), kTimes.data(), q0, q0, + constraint_array.size(), constraint_array.data(), + ikoptions, &q_sol, info.data(), &infeasible_constraint); + bool info_good = true; + for (size_t i = 0; i < kTimes.size(); ++i) { + drake::log()->info("INFO[{}] = {} ", i, info[i]); + if (info[i] != 1) { + info_good = false; + } + } + + if (!info_good) { + throw std::runtime_error( + "inverseKinPointwise failed to compute a feasible solution."); + } + + std::vector knots(kTimes.size()); + for (size_t i = 0; i < kTimes.size(); ++i) { + knots[i] = q_sol.col(i); + } + + return PiecewisePolynomial::FirstOrderHold(kTimes, knots); +} + +class IiwaPickUpDumbbell : public manipulation::QuasistaticSystem { + public: + IiwaPickUpDumbbell(); + + private: + static const int idx_base; + static const std::vector idx_unactuated_bodies; + static const std::vector fixed_base_positions; + static const std::vector fixed_base_velocities; + static QuasistaticSystemOptions InitializeOptions(); +}; + +const int IiwaPickUpDumbbell::idx_base = 1; +const std::vector IiwaPickUpDumbbell::idx_unactuated_bodies = {1}; +const std::vector IiwaPickUpDumbbell::fixed_base_positions{}; +const std::vector IiwaPickUpDumbbell::fixed_base_velocities{}; + +QuasistaticSystemOptions IiwaPickUpDumbbell::InitializeOptions() { + QuasistaticSystemOptions options; + options.period_sec = 0.005; + options.mu = 0.5; + options.is_using_kinetic_energy_minimizing_QP = false; + return options; +} + +IiwaPickUpDumbbell::IiwaPickUpDumbbell() + : QuasistaticSystem(idx_base, idx_unactuated_bodies, fixed_base_positions, + fixed_base_velocities, InitializeOptions()) { + tree_ = BuildSimWorld(); + Initialize(); +} + +int do_main() { + // Loads model into a RigidBodyTree. + auto tree = BuildSimWorld(); + + // Builds simulation diagram. + lcm::DrakeLcm lcm; + systems::DiagramBuilder builder; + + auto iiwa_pickup = builder.AddSystem(); + const double h = iiwa_pickup->get_period_sec(); // time step + + const int n1 = iiwa_pickup->state_output().size(); + const int n_states = tree->get_num_positions() + tree->get_num_velocities(); + MatrixXd D1(n_states, n1); + D1.setIdentity(); + auto gain1 = builder.AddSystem(D1); + builder.Connect(iiwa_pickup->state_output(), gain1->get_input_port()); + + auto publisher = + builder.AddSystem(*tree, &lcm, true); + builder.Connect(gain1->get_output_port(), publisher->get_input_port(0)); + + // generates trajectory source for iiwa and gripper + const Vector3d pos_EE_final(0.2, 0.2, 0.8); + const RollPitchYaw rpy_EE_final(M_PI / 4, 0, 0); + PiecewisePolynomial iiwa_traj = MakePlan(pos_EE_final, rpy_EE_final); + auto iiwa_traj_src = + builder.template AddSystem>(iiwa_traj, + 1); + + std::vector knots(kTimes.size(), VectorXd::Zero(2)); + knots[1] << 0.05, -0.05; + knots[2] << 0.05, -0.05; + knots[3] << 0.05, -0.05; + knots[4] << 0.05, -0.05; + PiecewisePolynomial gripper_traj = + PiecewisePolynomial::ZeroOrderHold(kTimes, knots); + + auto gripper_qdot_src = + builder.template AddSystem>( + gripper_traj, 0); + + // picks the qdot from iiwa_traj_src output using matrix gain + const int n_qa_dot = iiwa_traj_src->get_num_total_outputs() / 2; + MatrixXd D2(n_qa_dot, n_qa_dot * 2); + D2.setZero(); + D2.block(0, n_qa_dot, n_qa_dot, n_qa_dot).setIdentity(); + auto gain2 = builder.template AddSystem(D2); + builder.Connect(iiwa_traj_src->get_output_port(), gain2->get_input_port()); + + // mux iiwa velocity and gripper velocity + std::vector qdot_sizes = {7, 2}; + auto mux = builder.template AddSystem(qdot_sizes); + builder.Connect(gain2->get_output_port(), mux->get_input_port(0)); + builder.Connect(gripper_qdot_src->get_output_port(), mux->get_input_port(1)); + + builder.Connect(mux->get_output_port(0), iiwa_pickup->get_input_port(0)); + + // set up signal loggers + auto log_state = builder.AddSystem>(n1); + const int n2 = iiwa_pickup->decision_variables_output().size(); + auto log_decision_variables = + builder.AddSystem>(n2); + + builder.Connect(iiwa_pickup->state_output(), log_state->get_input_port()); + builder.Connect(iiwa_pickup->decision_variables_output(), + log_decision_variables->get_input_port()); + + // publisher->set_publish_period(0.05); + std::unique_ptr> diagram = builder.Build(); + systems::Simulator simulator(*diagram); + + // Set the initial conditions x(0). + drake::systems::BasicVector& x0 = simulator.get_mutable_context() + .get_mutable_discrete_state() + .get_mutable_vector(); + + x0.SetZero(); + x0.SetAtIndex(0, kDumbbellBase(0)); // x + x0.SetAtIndex(1, kDumbbellBase(1)); // y + x0.SetAtIndex(2, kDumbbellBase(2)); // z + x0.SetAtIndex(3, 1); // qw1 + x0.SetAtIndex(14, -0.055); // left gripper + x0.SetAtIndex(15, 0.055); // right gripper + + simulator.set_target_realtime_rate(0); + simulator.get_mutable_integrator()->set_maximum_step_size(h); + simulator.Initialize(); + simulator.StepTo(kTimes.back()); + + // Comparing the final position and orientation of the object in world frame + // to expected values. + const int kDumbbellIndex = 1; + const int kIiwaEndEffectorIndex = 13; + + // compute offset in the x-direction (in the EE-frame) from the origin of + // the EE to the origin of the object (dumbbell). The x-axes of the end + // effector and the object are aligned with the x-axis of the world frame + // at t = 3.0, the time at which x_offset is calculated. + const Eigen::VectorXd q_grasping = + log_state->data().col(static_cast(3.0 / h)); + VectorXd v(tree->get_num_velocities()); + v.setZero(); + KinematicsCache cache = tree->CreateKinematicsCache(); + cache.initialize(q_grasping, v); + tree->doKinematics(cache); + auto transform = tree->CalcBodyPoseInWorldFrame( + cache, tree->get_body(kIiwaEndEffectorIndex)); + const double x_offset = q_grasping[0] - (transform.translation())[0]; + + // compute expected_object_CG_final_position = + // expected_end_effector_final_position + [x_offset, 0, 0] + const Eigen::Vector3d pos_offset(x_offset, 0, 0); + const Eigen::Vector3d position_final_expected = pos_EE_final + pos_offset; + + // compute expected_object_orientation as a rotation matrix. + const math::RotationMatrix R_dumbbell_expected( + rpy_EE_final.ToQuaternion()); + + // compute object final poistion and orientation. + const Eigen::VectorXd q_final = log_state->data().rightCols(1); + cache.initialize(q_final, v); + tree->doKinematics(cache); + transform = + tree->CalcBodyPoseInWorldFrame(cache, tree->get_body(kDumbbellIndex)); + + EXPECT_TRUE(CompareMatrices(position_final_expected, q_final.head<3>(), 1e-2, + MatrixCompareType::absolute)); + EXPECT_TRUE(CompareMatrices(R_dumbbell_expected.matrix(), transform.linear(), + 2e-2, MatrixCompareType::absolute)); + + return 0; +} + +} // namespace +} // namespace quasistatic_kuka_iiwa_arm +} // namespace manipulation +} // namespace drake + +int main() { return drake::manipulation::quasistatic_kuka_iiwa_arm::do_main(); } diff --git a/manipulation/dev/quasistatic_system.cc b/manipulation/dev/quasistatic_system.cc index a776905660ba..619fbba78faa 100644 --- a/manipulation/dev/quasistatic_system.cc +++ b/manipulation/dev/quasistatic_system.cc @@ -756,7 +756,7 @@ void QuasistaticSystem::StepForward( const Eigen::Ref& f, const Eigen::Ref& qa_dot_d) const { // upper bounds on delta_q - const Scalar max_delta_q = 10 * period_sec_; // The multiplier is arbitrary. + const Scalar max_delta_q = period_sec_; // The multiplier is arbitrary. const Scalar max_gamma = max_delta_q; // uppder bounds on impulses (force * delta_t) @@ -766,7 +766,7 @@ void QuasistaticSystem::StepForward( max_impulse = period_sec_ * std::abs(f(i)); } } - max_impulse *= 5; // This upper bound is arbitrary. + max_impulse *= 3; // This upper bound is arbitrary. // calcluate big M const Scalar kBigM = diff --git a/manipulation/dev/simplified_schunk_gripper.sdf b/manipulation/dev/simplified_schunk_gripper.sdf new file mode 100644 index 000000000000..d0648da201b5 --- /dev/null +++ b/manipulation/dev/simplified_schunk_gripper.sdf @@ -0,0 +1,116 @@ + + + + + + 0 -0.049 0 0 -0 0 + + + + 0.146 0.0725 0.05 + + + + + + 0.988882 + + 0.162992 + 0 + 0 + 0.162992 + 0 + 0.164814 + + + + + + + -0.008 0.025 0 0 -0 0 + + + + + 0.016 0.075 0.02 + + + + + + + + 0.016 0.075 0.02 + + + + + + 0.05 + + 0.16 + 0 + 0 + 0.16 + 0 + 0.16 + + + + + + 0.008 0.025 0 0 -0 0 + + + + 0.016 0.075 0.02 + + + + + + + + 0.016 0.075 0.02 + + + + + + 0.05 + + 0.16 + 0 + 0 + 0.16 + 0 + 0.16 + + + + + + 0 0 0 0 -0 0 + body + left_finger + + 1 0 0 + 0 + + + + + + 0 0 0 0 -0 0 + body + right_finger + + 0 + 1 0 0 + + + + + + + \ No newline at end of file diff --git a/manipulation/models/iiwa_description/urdf/iiwa14_no_collision.urdf b/manipulation/models/iiwa_description/urdf/iiwa14_no_collision.urdf new file mode 100644 index 000000000000..a4e46f4d947b --- /dev/null +++ b/manipulation/models/iiwa_description/urdf/iiwa14_no_collision.urdf @@ -0,0 +1,387 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + /iiwa + + + + + Gazebo/Grey + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Grey + 0.2 + 0.2 + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + +