diff --git a/examples/sampling_c3/franka_sampling_c3_controller.cc b/examples/sampling_c3/franka_sampling_c3_controller.cc index c04d64167..55db53192 100644 --- a/examples/sampling_c3/franka_sampling_c3_controller.cc +++ b/examples/sampling_c3/franka_sampling_c3_controller.cc @@ -104,6 +104,8 @@ int DoMain(int argc, char* argv[]) { // Build the contact pairs based on the demo. std::vector>> contact_pairs; + std::vector> ee_contact_pairs; + std::vector> ground_object_contact_pairs; std::unordered_map contact_geoms; // All demos include the end effector and ground. @@ -117,7 +119,6 @@ int DoMain(int argc, char* argv[]) { contact_geoms["GROUND"] = ground_geoms; std::vector> ee_ground_contact{ SortedPair(contact_geoms["EE"], contact_geoms["GROUND"])}; - contact_pairs.push_back(ee_ground_contact); if (FLAGS_demo_name == "jacktoy") { drake::geometry::GeometryId capsule1_geoms = @@ -159,18 +160,13 @@ int DoMain(int argc, char* argv[]) { contact_geoms["CAPSULE_3_SPHERE_1"] = capsule3_sphere1_geoms; contact_geoms["CAPSULE_3_SPHERE_2"] = capsule3_sphere2_geoms; - // EE-object contact pairs second. - std::vector> ee_contact_pairs; ee_contact_pairs.push_back( SortedPair(contact_geoms["EE"], contact_geoms["CAPSULE_1"])); ee_contact_pairs.push_back( SortedPair(contact_geoms["EE"], contact_geoms["CAPSULE_2"])); ee_contact_pairs.push_back( SortedPair(contact_geoms["EE"], contact_geoms["CAPSULE_3"])); - contact_pairs.push_back(ee_contact_pairs); - // Object-ground contact pairs third. - std::vector> ground_object_contact_pairs; ground_object_contact_pairs.push_back(SortedPair( contact_geoms["CAPSULE_1_SPHERE_1"], contact_geoms["GROUND"])); ground_object_contact_pairs.push_back(SortedPair( @@ -183,11 +179,50 @@ int DoMain(int argc, char* argv[]) { contact_geoms["CAPSULE_3_SPHERE_1"], contact_geoms["GROUND"])); ground_object_contact_pairs.push_back(SortedPair( contact_geoms["CAPSULE_3_SPHERE_2"], contact_geoms["GROUND"])); - contact_pairs.push_back(ground_object_contact_pairs); + } + else if (FLAGS_demo_name == "push_t") { + drake::geometry::GeometryId vertical_geoms = + plant_lcs.GetCollisionGeometriesForBody( + plant_lcs.GetBodyByName("vertical_link"))[0]; + drake::geometry::GeometryId horizontal_geoms = + plant_lcs.GetCollisionGeometriesForBody( + plant_lcs.GetBodyByName("horizontal_link"))[0]; + + drake::geometry::GeometryId top_left_sphere_geoms = + plant_lcs.GetCollisionGeometriesForBody( + plant_lcs.GetBodyByName("vertical_link"))[1]; + drake::geometry::GeometryId top_right_sphere_geoms = + plant_lcs.GetCollisionGeometriesForBody( + plant_lcs.GetBodyByName("vertical_link"))[2]; + drake::geometry::GeometryId bottom_sphere_geoms = + plant_lcs.GetCollisionGeometriesForBody( + plant_lcs.GetBodyByName("vertical_link"))[3]; + + contact_geoms["VERTICAL_LINK"] = vertical_geoms; + contact_geoms["HORIZONTAL_LINK"] = horizontal_geoms; + contact_geoms["TOP_LEFT_SPHERE"] = top_left_sphere_geoms; + contact_geoms["TOP_RIGHT_SPHERE"] = top_right_sphere_geoms; + contact_geoms["BOTTOM_SPHERE"] = bottom_sphere_geoms; + + ee_contact_pairs.push_back( + SortedPair(contact_geoms["EE"], contact_geoms["HORIZONTAL_LINK"])); + ee_contact_pairs.push_back( + SortedPair(contact_geoms["EE"], contact_geoms["VERTICAL_LINK"])); + + ground_object_contact_pairs.push_back(SortedPair( + contact_geoms["TOP_LEFT_SPHERE"], contact_geoms["GROUND"])); + ground_object_contact_pairs.push_back(SortedPair( + contact_geoms["TOP_RIGHT_SPHERE"], contact_geoms["GROUND"])); + ground_object_contact_pairs.push_back(SortedPair( + contact_geoms["BOTTOM_SPHERE"], contact_geoms["GROUND"])); } else { throw std::runtime_error("Unknown --demo_name value: " + FLAGS_demo_name); } + // Order: EE-ground, EE-object, object-ground. + contact_pairs.push_back(ee_ground_contact); + contact_pairs.push_back(ee_contact_pairs); + contact_pairs.push_back(ground_object_contact_pairs); // Piece together the diagram. DiagramBuilder builder; @@ -215,6 +250,10 @@ int DoMain(int argc, char* argv[]) { target_generator = std::make_unique( plant_object, controller_params.goal_params); + } else if (FLAGS_demo_name == "push_t") { + target_generator = + std::make_unique( + plant_object, controller_params.goal_params); } else { throw std::runtime_error("Unknown --demo_name value: " + FLAGS_demo_name); } diff --git a/examples/sampling_c3/generate_samples.cc b/examples/sampling_c3/generate_samples.cc index aef817362..62af91711 100644 --- a/examples/sampling_c3/generate_samples.cc +++ b/examples/sampling_c3/generate_samples.cc @@ -238,7 +238,6 @@ Eigen::Vector3d PerimeterSampling( double z_sample = 0; // Convert to world frame using the current object state. - Eigen::VectorXd x_lcs_world = x_lcs; Eigen::Quaterniond quat_object(x_lcs(3), x_lcs(4), x_lcs(5), x_lcs(6)); Eigen::Vector3d object_position = x_lcs.segment(7, 3); candidate_state = x_lcs; @@ -249,19 +248,19 @@ Eigen::Vector3d PerimeterSampling( // Project samples to specified sampling height in world frame. candidate_state[2] = sampling_params.sampling_height; } while (!IsSampleWithinDistanceOfSurface( - n_q, n_v, n_u, 0.0, false, candidate_state, plant, context, plant_ad, - context_ad, contact_geoms, sampling_c3_options, min_distance_index)); + n_q, n_v, n_u, 0.0, candidate_state, plant, context, plant_ad, context_ad, + contact_geoms, sampling_c3_options, min_distance_index)); // Project the sample past the surface of the object with clearance. Eigen::VectorXd projected_state = ProjectSampleOutsideObject( - candidate_state, min_distance_index, sampling_params, plant, *context, - contact_geoms); + candidate_state, min_distance_index, sampling_params, plant, *context, + contact_geoms); // Check the desired clearance is satisfied; otherwise try again. UpdateContext(n_q, n_v, n_u, plant, context, plant_ad, context_ad, projected_state); if (IsSampleWithinDistanceOfSurface( - n_q, n_v, n_u, sampling_params.sample_projection_clearance, true, + n_q, n_v, n_u, sampling_params.sample_projection_clearance, projected_state, plant, context, plant_ad, context_ad, contact_geoms, sampling_c3_options, min_distance_index)) { continue; @@ -333,8 +332,8 @@ Eigen::Vector3d ShellSampling( y_samplec + sampling_radius * sin(theta) * sin(elevation_theta); candidate_state[2] = z_samplec + sampling_radius * cos(elevation_theta); } while (!IsSampleWithinDistanceOfSurface( - n_q, n_v, n_u, 0.0, false, candidate_state, plant, context, plant_ad, - context_ad, contact_geoms, sampling_c3_options, min_distance_index)); + n_q, n_v, n_u, 0.0, candidate_state, plant, context, plant_ad, context_ad, + contact_geoms, sampling_c3_options, min_distance_index)); // Project the sample past the surface of the object with clearance. Eigen::VectorXd projected_state = ProjectSampleOutsideObject( @@ -345,7 +344,7 @@ Eigen::Vector3d ShellSampling( UpdateContext(n_q, n_v, n_u, plant, context, plant_ad, context_ad, projected_state); if (IsSampleWithinDistanceOfSurface( - n_q, n_v, n_u, sampling_params.sample_projection_clearance, true, + n_q, n_v, n_u, sampling_params.sample_projection_clearance, projected_state, plant, context, plant_ad, context_ad, contact_geoms, sampling_c3_options, min_distance_index)) { continue; @@ -390,7 +389,8 @@ double GetEERadiusFromPlant( query_port.template Eval>(context); const auto& inspector = query_object.inspector(); - // Locate the EE and obtain its radius. + // Locate the EE and obtain its radius. The first set of contact geoms has + // the EE and ground. GeometryId ee_geom_id = contact_geoms.at(0).at(0).first(); const drake::geometry::Shape& shape = inspector.GetShape(ee_geom_id); const auto* sphere = dynamic_cast(&shape); @@ -403,15 +403,14 @@ double GetEERadiusFromPlant( bool IsSampleWithinDistanceOfSurface( const int& n_q, const int& n_v, const int& n_u, const double& clearance_distance, - const bool& factor_in_ee_radius, const Eigen::VectorXd& candidate_state, drake::multibody::MultibodyPlant& plant, drake::systems::Context* context, drake::multibody::MultibodyPlant& plant_ad, drake::systems::Context* context_ad, const std::vector< - std::vector>>& - contact_geoms, + std::vector>>& + contact_geoms, SamplingC3Options sampling_c3_options, int& min_distance_index) { @@ -421,14 +420,12 @@ bool IsSampleWithinDistanceOfSurface( // Find the closest pair if there are multiple pairs std::vector distances; - - for (int i = 0; i < contact_geoms.at(0).size(); i++) { - // Evaluate the distance for each pair - SortedPair pair{(contact_geoms.at(0)).at(i)}; + for (int i = 0; i < contact_geoms.at(1).size(); i++) { + SortedPair pair{(contact_geoms.at(1)).at(i)}; multibody::GeomGeomCollider collider(plant, pair); auto [phi_i, J_i] = collider.EvalPolytope( - *context, sampling_c3_options.num_friction_directions); + *context, sampling_c3_options.num_friction_directions); distances.push_back(phi_i); } @@ -439,15 +436,8 @@ bool IsSampleWithinDistanceOfSurface( min_distance_index = std::distance(distances.begin(), min_distance_it); double min_distance = *min_distance_it; - // Factor the EE radius into the clearance distance if requested. - double ee_radius_contribution = 0.0; - if (factor_in_ee_radius) { - ee_radius_contribution = GetEERadiusFromPlant( - plant, *context, contact_geoms); - } - // Require that min_distance be at least 1 mm within the clearance distance. - return min_distance <= clearance_distance + ee_radius_contribution - 1e-3; + return min_distance <= clearance_distance - 1e-3; } Eigen::VectorXd ProjectSampleOutsideObject( @@ -462,22 +452,22 @@ Eigen::VectorXd ProjectSampleOutsideObject( // Compute the witness points between the penetrating sample and the object // surface. multibody::GeomGeomCollider collider( - plant, contact_geoms.at(0).at(min_distance_index)); - auto [p_world_contact_a, p_world_contact_b] = collider.CalcWitnessPoints( + plant, contact_geoms.at(1).at(min_distance_index)); + auto [p_world_contact_ee, p_world_contact_obj] = collider.CalcWitnessPoints( context); // Get the EE radius to factor into the projection. double ee_radius = GetEERadiusFromPlant(plant, context, contact_geoms); - // Find vector in direction from sample to contact point on object. - Eigen::Vector3d a_to_b = p_world_contact_b - p_world_contact_a; - Eigen::Vector3d a_to_b_normalized = a_to_b.normalized(); - // Add clearance to point b in the same direction. - Eigen::Vector3d p_world_contact_b_clearance = - p_world_contact_b + + // Find vector in direction from EE to object witness points. + Eigen::Vector3d ee_to_obj = p_world_contact_obj - p_world_contact_ee; + Eigen::Vector3d ee_to_obj_normalized = ee_to_obj.normalized(); + // Add clearance to the object in the same direction. + Eigen::Vector3d p_world_contact_obj_clearance = + p_world_contact_obj + (ee_radius + sampling_params.sample_projection_clearance) * - a_to_b_normalized; - candidate_state.head(3) = p_world_contact_b_clearance; + ee_to_obj_normalized; + candidate_state.head(3) = p_world_contact_obj_clearance; return candidate_state; } diff --git a/examples/sampling_c3/generate_samples.h b/examples/sampling_c3/generate_samples.h index 75c2015ff..fae008631 100644 --- a/examples/sampling_c3/generate_samples.h +++ b/examples/sampling_c3/generate_samples.h @@ -129,7 +129,6 @@ bool IsSampleWithinDistanceOfSurface( const int& n_v, const int& n_u, const double& clearance_distance, - const bool& factor_in_ee_radius, const Eigen::VectorXd& candidate_state, drake::multibody::MultibodyPlant& plant, drake::systems::Context* context, diff --git a/examples/sampling_c3/goal_generator.cc b/examples/sampling_c3/goal_generator.cc index 3bd1b5129..3a1ac790a 100644 --- a/examples/sampling_c3/goal_generator.cc +++ b/examples/sampling_c3/goal_generator.cc @@ -164,10 +164,10 @@ void SamplingC3GoalGenerator::SetRandomizedTargetFinalObjectPosition() const { double x, y = 0; while ((sqrt(x * x + y * y) > goal_params_.random_goal_radius_limits[1]) || (sqrt(x * x + y * y) < goal_params_.random_goal_radius_limits[0])) { - double x = RandomUniform(goal_params_.random_goal_x_limits[0], - goal_params_.random_goal_x_limits[1]); - double y = RandomUniform(goal_params_.random_goal_y_limits[0], - goal_params_.random_goal_y_limits[1]); + x = RandomUniform(goal_params_.random_goal_x_limits[0], + goal_params_.random_goal_x_limits[1]); + y = RandomUniform(goal_params_.random_goal_y_limits[0], + goal_params_.random_goal_y_limits[1]); } target_final_object_position_ << x, y, goal_params_.resting_object_height; diff --git a/examples/sampling_c3/goal_generator.h b/examples/sampling_c3/goal_generator.h index ab14511a5..e70f0f4d9 100644 --- a/examples/sampling_c3/goal_generator.h +++ b/examples/sampling_c3/goal_generator.h @@ -38,6 +38,11 @@ inline const std::vector kNominalOrientationsJack{ kQuatAllUp, kQuatRedDown, kQuatBlueUp, kQuatAllDown, kQuatGreenUp, kQuatBlueDown, kQuatRedUp, kQuatGreenDown}; +// Define the nominal orientations for any planar demo. +inline const Eigen::Quaterniond kQUAT_FLAT{1.0, 0.0, 0.0, 0.0}; +inline const std::vector kNominalOrientationsPlanar{ + kQUAT_FLAT}; + namespace dairlib { namespace systems { @@ -137,5 +142,17 @@ class SamplingC3GoalGeneratorJacktoy : public SamplingC3GoalGenerator { object_plant, goal_params, kNominalOrientationsJack) {} }; + +// class TargetGeneratorPushT : public TargetGenerator { +class SamplingC3GoalGeneratorPushT : public SamplingC3GoalGenerator { + public: + SamplingC3GoalGeneratorPushT( + const drake::multibody::MultibodyPlant& object_plant, + const SamplingC3GoalParams& goal_params) : + SamplingC3GoalGenerator( + object_plant, goal_params, kNominalOrientationsPlanar) {} +}; + + } // namespace systems } // namespace dairlib diff --git a/examples/sampling_c3/jacktoy/BUILD.bazel b/examples/sampling_c3/jacktoy/BUILD.bazel index aea0f689a..faec88ea6 100644 --- a/examples/sampling_c3/jacktoy/BUILD.bazel +++ b/examples/sampling_c3/jacktoy/BUILD.bazel @@ -10,7 +10,6 @@ cc_library( ]), ) - cc_library( name = "lcm_channels_jacktoy", hdrs = [], diff --git a/examples/sampling_c3/jacktoy/franka_hardware.pmd b/examples/sampling_c3/jacktoy/franka_hardware_jack.pmd similarity index 100% rename from examples/sampling_c3/jacktoy/franka_hardware.pmd rename to examples/sampling_c3/jacktoy/franka_hardware_jack.pmd diff --git a/examples/sampling_c3/jacktoy/franka_sim_jacktoy.pmd b/examples/sampling_c3/jacktoy/franka_sim_jack.pmd similarity index 100% rename from examples/sampling_c3/jacktoy/franka_sim_jacktoy.pmd rename to examples/sampling_c3/jacktoy/franka_sim_jack.pmd diff --git a/examples/sampling_c3/jacktoy/parameters/sampling_c3_controller_params.yaml b/examples/sampling_c3/jacktoy/parameters/sampling_c3_controller_params.yaml index 38ad1b11d..da0a74987 100644 --- a/examples/sampling_c3/jacktoy/parameters/sampling_c3_controller_params.yaml +++ b/examples/sampling_c3/jacktoy/parameters/sampling_c3_controller_params.yaml @@ -15,7 +15,7 @@ lcm_channels_simulation_file: examples/sampling_c3/shared_parameters/lcm_channel # Note: Use capsule_1 body name as representative of the pose of the entire # jack model. This works because capsule_1's origin is the same as the jack's. -object_model: examples/sampling_c3/urdf/jack_ground.sdf +object_model: examples/sampling_c3/urdf/jack_control.sdf object_body_name: capsule_1 include_end_effector_orientation: false diff --git a/examples/sampling_c3/jacktoy/parameters/sampling_params.yaml b/examples/sampling_c3/jacktoy/parameters/sampling_params.yaml index 3e84da937..4c0c606a5 100644 --- a/examples/sampling_c3/jacktoy/parameters/sampling_params.yaml +++ b/examples/sampling_c3/jacktoy/parameters/sampling_params.yaml @@ -19,7 +19,7 @@ num_additional_samples_repos: 1 num_additional_samples_c3: 2 # Sample buffer parameters. -consider_best_buffer_sample_when_leaving_c3: false +consider_best_buffer_sample_when_leaving_c3: true N_sample_buffer: 100 pos_error_sample_retention: 0.005 ang_error_sample_retention: 0.087 # 0.087 rad = 5 deg diff --git a/examples/sampling_c3/jacktoy/parameters/sim_params.yaml b/examples/sampling_c3/jacktoy/parameters/sim_params.yaml index cac1c35d9..2f3831551 100644 --- a/examples/sampling_c3/jacktoy/parameters/sim_params.yaml +++ b/examples/sampling_c3/jacktoy/parameters/sim_params.yaml @@ -10,6 +10,5 @@ publish_efforts: true dt: 0.0001 realtime_rate: 1 -# Set the initial locations to something more vertical: q_init_franka: [2.191, 1.1, -1.33, -2.22, 1.30, 2.02, 0.08] q_init_object: [-0.3347435, -0.026718954, 0.8878204, 0.31518626, 0.4, 0.30, 0.03] diff --git a/examples/sampling_c3/push_t/BUILD.bazel b/examples/sampling_c3/push_t/BUILD.bazel new file mode 100644 index 000000000..bd0951674 --- /dev/null +++ b/examples/sampling_c3/push_t/BUILD.bazel @@ -0,0 +1,76 @@ +# -*- mode: python -*- +# vi: set ft=python : + +package(default_visibility = ["//visibility:public"]) + +cc_library( + name = "parameters", + data = glob([ + "parameters/*.yaml", + ]), +) + +cc_library( + name = "lcm_channels_push_t", + hdrs = [], + data = [ + "//examples/sampling_c3:shared_params_yamls", + ], + deps = [ + "//examples/sampling_c3/parameter_headers:lcm_channels", + "//systems/framework:vector", + "@drake//:drake_shared_library", + ], +) + +cc_library( + name = "franka_sim_params_push_t", + hdrs = [], + data = [ + "parameters/sim_params.yaml", + ], + deps = [ + "//examples/sampling_c3/parameter_headers:franka_sim_params", + "@drake//:drake_shared_library", + ], +) + +cc_library( + name = "sampling_c3_controller_params_push_t", + hdrs = [], + data = [ + "parameters/sampling_c3_controller_params.yaml", + "parameters/sampling_params.yaml", + "//examples/sampling_c3:shared_params_yamls", + ], + deps = [ + "//solvers:c3", + "@drake//:drake_shared_library", + ], +) + +cc_library( + name = "goal_params_push_t", + hdrs = [], + data = [ + "parameters/goal_params.yaml", + ], + deps = [ + "//examples/sampling_c3/parameter_headers:goal_params", + "//solvers:c3", + "@drake//:drake_shared_library", + ], +) + +cc_library( + name = "franka_drake_lcm_driver_channels_push_t", + hdrs = [], + data = [ + "//common/parameters:franka_drake_lcm_driver_channels", + ], + deps = [ + "//common/parameters:franka_drake_lcm_driver_channels", + "//solvers:c3", + "@drake//:drake_shared_library", + ], +) diff --git a/examples/sampling_c3/push_t/franka_hardware_t.pmd b/examples/sampling_c3/push_t/franka_hardware_t.pmd new file mode 100644 index 000000000..9c953335d --- /dev/null +++ b/examples/sampling_c3/push_t/franka_hardware_t.pmd @@ -0,0 +1,103 @@ +group "operator" { + cmd "visualizer" { + exec = "bazel-bin/examples/sampling_c3/franka_visualizer --is_simulation=false --demo_name=push_t"; + host = "sampling_c3_localhost"; + } + cmd "xbox" { + exec = "bazel-bin/examples/sampling_c3/xbox_script"; + host = "sampling_c3_localhost"; + } + cmd "logger" { + exec = "python3 examples/sampling_c3/start_logging.py hw push_t"; + host = "sampling_c3_localhost"; + } + cmd "record_video" { + exec = "python3 record_video.py"; + host = "franka_control"; + } +} + +group "controllers (hardware)" { + cmd "sampling_c3" { + exec = "bazel-bin/examples/sampling_c3/franka_sampling_c3_controller --is_simulation=false --demo_name=push_t; + host = "sampling_c3_localhost"; + } + cmd "franka_osc" { + exec = "bazel-bin/examples/sampling_c3/franka_osc_controller --is_simulation=false --demo_name=push_t --lcm_url=udpm://239.255.76.67:7667?ttl=1; + host = "franka_control"; + } +} + +group "debug" { + cmd "lcm-spy" { + exec = "bazel-bin/lcmtypes/dair-lcm-spy"; + host = "sampling_c3_localhost"; + } +} + +group "drivers" { + cmd "move_to_init" { + exec = "bazel-bin/examples/sampling_c3/franka_joint_osc_controller --is_simulation=false --demo_name=push_t"; + host = "franka_control"; + } + cmd "franka_driver_out" { + exec = "bazel-bin/examples/sampling_c3/franka_bridge_driver_out --demo_name=push_t"; + host = "franka_control"; + } + cmd "franka_driver_in" { + exec = "bazel-bin/examples/sampling_c3/franka_bridge_driver_in --demo_name=push_t"; + host = "franka_control"; + } + cmd "torque_driver" { + exec = "bazel-bin/franka-driver/franka_driver_v4 --robot_ip_address=172.16.0.2 --control_mode=torque"; + host = "drake_franka_driver"; + } +} + + +script "init_experiment" { + stop cmd "sampling_c3"; + stop cmd "franka_osc"; + stop cmd "franka_driver_out"; + stop cmd "franka_driver_in"; + stop cmd "torque_driver"; + wait ms 1000; + start cmd "franka_driver_out"; + start cmd "franka_driver_in"; + start cmd "torque_driver"; + start cmd "move_to_init"; + wait ms 10000; + stop group "drivers"; +} + +script "start_experiment" { + restart cmd "xbox"; + stop cmd "franka_driver_out"; + stop cmd "franka_driver_in"; + stop cmd "torque_driver"; + stop cmd "franka_osc"; + start cmd "record_video"; + start cmd "logger"; + wait ms 1000; + start cmd "franka_driver_out"; + start cmd "franka_driver_in"; + start cmd "torque_driver"; + start cmd "move_to_init"; + wait ms 5000; + stop cmd "move_to_init"; + start cmd "franka_osc"; + start cmd "sampling_c3"; +} + +script "start_operator_commands" { + restart cmd "visualizer"; + restart cmd "xbox"; + restart cmd "lcm-spy"; +} + +script "stop_experiment" { + stop group "drivers"; + stop group "controllers (hardware)"; + stop cmd "record_video"; + stop cmd "logger"; +} diff --git a/examples/sampling_c3/push_t/franka_sim_t.pmd b/examples/sampling_c3/push_t/franka_sim_t.pmd new file mode 100644 index 000000000..420b50d58 --- /dev/null +++ b/examples/sampling_c3/push_t/franka_sim_t.pmd @@ -0,0 +1,71 @@ +group "simulations" { + cmd "franka_sim" { + exec = "bazel-bin/examples/sampling_c3/franka_sim --demo_name=push_t"; + host = "localhost"; + } +} + +group "operator" { + cmd "visualizer" { + exec = "bazel-bin/examples/sampling_c3/franka_visualizer --is_simulation=true --demo_name=push_t"; + host = "localhost"; + } + cmd "drake-director" { + exec = "bazel-bin/director/drake-director"; + host = "localhost"; + } + cmd "xbox" { + exec = "bazel-bin/examples/sampling_c3/xbox_script"; + host = "localhost"; + } + cmd "start_logging" { + exec = "python3 examples/sampling_c3/start_logging.py sim push_t"; + host = "localhost"; + } +} + +group "controllers" { + cmd "move_to_init" { + exec = "bazel-bin/examples/sampling_c3/franka_joint_osc_controller --is_simulation=true --demo_name=push_t"; + host = "localhost"; + } + cmd "franka_osc" { + exec = "bazel-bin/examples/sampling_c3/franka_osc_controller --is_simulation=true --demo_name=push_t"; + host = "localhost"; + } + cmd "sampling_c3" { + exec = "bazel-bin/examples/sampling_c3/franka_sampling_c3_controller --is_simulation=true --demo_name=push_t"; + host = "localhost"; + } +} + +group "debug" { + cmd "lcm-spy" { + exec = "bazel-bin/lcmtypes/dair-lcm-spy"; + host = "localhost"; + } +} + +script "start_experiment_no_logs"{ + stop cmd "start_logging"; + restart cmd "franka_sim"; + restart cmd "franka_osc"; + wait ms 50; + restart cmd "sampling_c3"; +} + +script "start_experiment_with_logs"{ + restart cmd "start_logging"; + restart cmd "franka_sim"; + restart cmd "franka_osc"; + wait ms 50; + restart cmd "sampling_c3"; +} + +script "end_experiment"{ + stop cmd "sampling_c3"; + stop cmd "franka_osc"; + stop cmd "franka_sim"; + wait ms 10; + stop cmd "start_logging"; +} diff --git a/examples/sampling_c3/push_t/parameters/goal_params.yaml b/examples/sampling_c3/push_t/parameters/goal_params.yaml new file mode 100644 index 000000000..f4c7785eb --- /dev/null +++ b/examples/sampling_c3/push_t/parameters/goal_params.yaml @@ -0,0 +1,36 @@ +# Goal mode options (defined as GoalMode enum in goal_params.h): +# 0. kRandom: randomly generate a new goal. +# 1. kOrientationSequence: keep position goal the same, and cycle through a +# sequence of orientations. +# 2. kFixedGoal: keep the same goal. +goal_mode: 0 + +### Parameters used for multiple goal modes. +# Success thresholds for position and orientation. +position_success_threshold: 0.02 +orientation_success_threshold: 0.1 # 0.1 rad = 5.7 degrees + +resting_object_height: -0.009 # in world frame +ee_target_z_offset_above_object: 0.06 # defines EE goal w.r.t. object position + +# Lookahead parameters to define a sub-goal for C3. +lookahead_step_size: 0.15 # meters +lookahead_angle: 2 # rad + +# Apply hysteresis on the lookahead angle so the sub-goal orientation doesn't +# flip back and forth near the 180 degree error singularity. A lower number +# means the sub-goal orientation can switch more often; a higher number means +# the sub-goal orientation is more stable but possibly less optimal. +angle_hysteresis: 0.4 + +# Factor to convert an angular error to angular velocity command (0=disabled). +angle_err_to_vel_factor: 0 + +# Initial goal (and only goal for fixed goal mode). +fixed_target_position: [0.48199167, 0.18745379, -0.009] +fixed_target_orientation: [-0.9327733, 0, 0, 0.36046353] + +# Random-specific parameters. +random_goal_x_limits: [0.44, 0.52] +random_goal_y_limits: [0.02, 0.25] +random_goal_radius_limits: [0.5, 0.52] diff --git a/examples/sampling_c3/push_t/parameters/progress_params.yaml b/examples/sampling_c3/push_t/parameters/progress_params.yaml new file mode 100644 index 000000000..48609fc7d --- /dev/null +++ b/examples/sampling_c3/push_t/parameters/progress_params.yaml @@ -0,0 +1,64 @@ +# Ways of computing C3 costs after solving the MPC problem (defined as +# C3CostComputationType enum in c3_options.h): +# 0. kSimLCS: Simulate the LCS dynamics from the planned +# inputs. +# 1. kUseC3Plan: Use the C3 planned trajectory and inputs. +# 2. kSimLCSReplaceC3EEPlan: Simulate the LCS dynamics from the planned +# inputs only for the object; use the planned +# EE trajectory. +# 3. kSimImpedance: Try to emulate the real cost of the system +# associated not only applying the planned +# inputs, but also tracking the planned EE +# trajectory with an impedance controller. +# 4. kSimImpedanceReplaceC3EEPlan: The same as kSimImpedance except the EE +# states are replaced with the plan from C3 +# at the end. +# 5. kSimImpedanceObjectCostOnly: The same as kSimImpedance except only the +# object terms contribute to the final cost. +cost_type: 5 +cost_type_position: 5 + +# Progress metric to use to determine if C3 is making progress, all phrased as +# improvement requirements over a number of control loops (defined as +# ProgressMetric enum in progress_params.h) +# 0. kC3Cost: C3 cost. +# 1. kConfigCost: Current object configuration cost. +# 2. kPosOrRotCost: Current position or rotation error. +# 3. kConfigCostDrop: Drop in object configuration cost (this is the same as +# kConfigCost if the required drop is 0; a more +# aggressive drop cuts C3 off earlier). +track_c3_progress_via: 3 + +# Ignore orientation errors when object is beyond this threshold from the goal. +cost_switching_threshold_distance: 0.50 + +# Penalize repositioning distance. +travel_cost_per_meter: 0 + +# Number of control loops to wait before cutting off C3 due to unproductivity. +# Used for kC3Cost, kConfigCost, kPosOrRotCost. +num_control_loops_to_wait: 5 +num_control_loops_to_wait_position: 5 + +# kConfigCostDrop parameters. +progress_enforced_cost_drop: 0.01 +progress_enforced_over_n_loops: 28 + +### Hysteresis parameters for switching between C3 and repositioning modes. +finished_reposition_cost: 1000000000 + +hyst_c3_to_repos: 300000 +hyst_c3_to_repos_position: 300000 +hyst_repos_to_c3: 200000 +hyst_repos_to_c3_position: 200000 +hyst_repos_to_repos: 60000000000000 +hyst_repos_to_repos_position: 60000000000000 + +# Relative hysteresis is in terms of a fraction of the current cost. +use_relative_hysteresis: true +hyst_c3_to_repos_frac: 0.4 +hyst_c3_to_repos_frac_position: 0.6 +hyst_repos_to_c3_frac: 0.9 +hyst_repos_to_c3_frac_position: 0.5 +hyst_repos_to_repos_frac: 0.3 +hyst_repos_to_repos_frac_position: 0.1 diff --git a/examples/sampling_c3/push_t/parameters/reposition_params.yaml b/examples/sampling_c3/push_t/parameters/reposition_params.yaml new file mode 100644 index 000000000..faaf08b02 --- /dev/null +++ b/examples/sampling_c3/push_t/parameters/reposition_params.yaml @@ -0,0 +1,32 @@ +# Repositioning trajectory type options (defined as RepositioningTrajectoryType +# enum in reposition_params.h): +# 0. kSpline: move from current to target with spline that distorts +# around the object. +# 1. kSpherical: move away from object to a radius, along the spherical +# surface to outside target, then in to target. +# 2. kCircular: (planar) move away from object to a planar radius, +# along the circle to outside target, then in to target. +# 3. kPiecewiseLinear: move up to waypoint height, over to above target, then +# down. +traj_type: 3 + +### Parameters used for multiple repositioning trajectory types. +speed: 0.18 # m/s + +# Thresholds for switching to straight line trajectories. +use_straight_line_traj_under_spline: 0.12 # xyz meters for spline +use_straight_line_traj_within_angle: 0.3 # rad for circle/sphere +use_straight_line_traj_under_piecewise_linear: 0.008 # xy meters for PWL + +# Spline-specific parameters. +spline_width: 0.17 + +# Spherical-specific parameters. +sphere_radius: 0.18 + +# Circular-specific parameters. +circle_radius: 0.20 +circle_height: 0.00 + +# Piecewise-linear-specific parameters. +pwl_waypoint_height: 0.06 diff --git a/examples/sampling_c3/push_t/parameters/sampling_c3_controller_params.yaml b/examples/sampling_c3/push_t/parameters/sampling_c3_controller_params.yaml new file mode 100644 index 000000000..7977849b5 --- /dev/null +++ b/examples/sampling_c3/push_t/parameters/sampling_c3_controller_params.yaml @@ -0,0 +1,23 @@ +sampling_c3_options_file: examples/sampling_c3/push_t/parameters/sampling_c3_options.yaml +reposition_params_file: examples/sampling_c3/push_t/parameters/reposition_params.yaml +progress_params_file: examples/sampling_c3/push_t/parameters/progress_params.yaml +sampling_params_file: examples/sampling_c3/push_t/parameters/sampling_params.yaml +goal_params_file: examples/sampling_c3/push_t/parameters/goal_params.yaml + +sim_params_file: examples/sampling_c3/push_t/parameters/sim_params.yaml +vis_params_file: examples/sampling_c3/push_t/parameters/vis_params.yaml +osc_params_file: examples/sampling_c3/shared_parameters/osc_params.yaml +osqp_settings_file: examples/sampling_c3/shared_parameters/sampling_c3_qp_settings.yaml +osc_qp_settings_file: examples/sampling_c3/shared_parameters/osc_qp_settings.yaml +franka_driver_channels_file: common/parameters/franka_drake_lcm_driver_channels.yaml +lcm_channels_hardware_file: examples/sampling_c3/shared_parameters/lcm_channels_hardware.yaml +lcm_channels_simulation_file: examples/sampling_c3/shared_parameters/lcm_channels_simulation.yaml + +# Note: Use vertical_link body name as representative of the pose of the entire +# T model. This works because vertical_link's origin is the same as the T's. +object_model: examples/sampling_c3/urdf/push_t_control.sdf #TODO T_vertical.sdf +object_body_name: vertical_link + +include_end_effector_orientation: false + +control_loop_delay_ms: 0 diff --git a/examples/sampling_c3/push_t/parameters/sampling_c3_options.yaml b/examples/sampling_c3/push_t/parameters/sampling_c3_options.yaml new file mode 100644 index 000000000..c60e0c76b --- /dev/null +++ b/examples/sampling_c3/push_t/parameters/sampling_c3_options.yaml @@ -0,0 +1,163 @@ +### C3 options +admm_iter: 3 +rho: 0 #This isn't used anywhere! +rho_scale: 3 +num_threads: 5 +num_outer_threads: 4 +delta_option: 1 +projection_type: 'MIQP' # 'MIQP' or 'QP' +contact_model: 'anitescu' # 'stewart_and_trinkle' or 'anitescu'. +warm_start: false +end_on_qp_step: false +use_robust_formulation: false +solve_time_filter_alpha: 0.95 +publish_frequency: 0 +penalize_changes_in_u_across_solves: false # Penalize (u-u_prev) instead of u. +num_friction_directions: 2 + +N: 5 +gamma: 1.0 # discount factor on MPC costs + +# As alpha -> 0, any complimentarity constraint error in QP projection -> 0. +qp_projection_alpha: 0.01 +qp_projection_scaling: 1 + +### Limits +# Workspace limits specified as linear constraints [x, y, z, lb, ub] +workspace_limits: [[1, 0, 0, 0.15, 0.75], + [0, 1, 0, -0.6, 0.6], + [0, 0, 1, -0.01, 0.3]] +robot_radius_limits: [0.25, 0.75] +workspace_margins: 0.02 +u_horizontal_limits: [-50, 50] +u_vertical_limits: [-50, 50] + +### Whether to use a predicted EE location for each mode. +use_predicted_x0_c3: true +use_predicted_x0_repos: true +use_predicted_x0_reset_mechanism: false # Resets if prediction is too far. + +### Push-T demo specific parameters: +# Contact pairs include a number of (ee-ground, ee-T, T-ground). +mu_per_pair_type: [0.4165, 1, 0.4615] # match URDFs with (2mu1*mu2)/(mu1+mu2) +resolve_contacts_to_lists: [[0, 1, 3], + [0, 2, 3]] + +# Index into this list to choose number of contacts for C3 solve and cost. +num_contacts_index: 0 +num_contacts_index_for_cost: 1 + +planning_dt_position: 0.1 +planning_dt_pose: 0.05 + +### Cost parameters +### If list of lists, then index into them with num_contact_index(_for_cost). +# Set 4x4 portion of Q for quaternion based on current and desired quaternion. +use_quaternion_dependent_cost: true +q_quaternion_dependent_weight: 1000 +q_quaternion_dependent_regularizer_fraction: 0 + +Kp_for_ee_pd_rollout: 100 +Kd_for_ee_pd_rollout: 0.5 + +### Pose tracking cost parameters +# Matrix scaling +w_Q: 45 +w_R: 1 +w_G: 0.25 +w_U: 0.26 + +q_vector: [0.01, 0.01, 0.01, # EE position + 0.1, 0.1, 0.1, 0.1, # object orientation + 200, 200, 120, # object position + 5, 5, 5, # EE linear velocity + 0.05, 0.05, 0.05, # object angular velocity + 0.05, 0.05, 0.05] # object linear velocity +r_vector: [0.01, 0.01, 0.01] + +# Penalty on matching projected variables +g_x: [950, 950, 950, 1, 1, 1, 1, 1, 1, 1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] +g_gamma_list: [[1, 1, 1, 1], + [1, 1, 1, 1, 1]] +g_lambda_n_list: [[1, 1, 1, 1], + [1, 1, 1, 1, 1]] +g_lambda_t_list: [[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]] +g_lambda_list: [[0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005], + [0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005]] #, +g_u: [30, 30, 30] + +# Penalty on matching the QP variables +u_x: [10, 10, 10, 100, 100, 100, 100, 10, 10, 10, 8, 8, 8, 1, 1, 1, 1, 1, 1] +u_gamma_list: [[1, 1, 1, 1], + [1, 1, 1, 1, 1]] +u_lambda_n_list: [[1, 1, 1, 1], + [1, 1, 1, 1, 1]] +u_lambda_t_list: [[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]] +# u_lambda_list is not used for the QP projection; overwritten by alpha*F. +u_lambda_list: [[10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10], + [10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10]] +u_u: [0.01, 0.01, 0.01] + +### Position tracking cost parameters +# Matrix scaling +w_Q_position: 45 +w_R_position: 1 +w_G_position: 0.25 +w_U_position: 0.26 + +q_vector_position: [0.01, 0.01, 0.01, # EE position + 0.1, 0.1, 0.1, 0.1, # object orientation + 250, 250, 250, # object position + 5, 5, 5, # EE linear velocity + 0.05, 0.05, 0.05, # object angular velocity + 0.05, 0.05, 0.05] # object linear velocity +r_vector_position: [0.01, 0.01, 0.01] + +# Penalty on matching projected variables +g_x_position: [900, 900, 900, 1, 1, 1, 1, 1, 1, 1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] +g_gamma_position_list: [[1, 1, 1, 1], + [1, 1, 1, 1, 1]] +g_lambda_n_position_list: [[1, 1, 1, 1], + [1, 1, 1, 1, 1]] +g_lambda_t_position_list: [[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]] +g_lambda_position_list: [ + [0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005], + [0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005]] +g_u_position: [30, 30, 30] + +# Penalty on matching the QP variables +u_x_position: [10, 10, 10, 100, 100, 100, 100, 10, 10, 10, 8, 8, 8, 1, 1, 1, 1, 1, 1] +u_gamma_position_list: [[1, 1, 1, 1], + [1, 1, 1, 1, 1]] +u_lambda_n_position_list: [[1, 1, 1, 1], + [1, 1, 1, 1, 1]] +u_lambda_t_position_list: [[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]] +# u_lambda is not used for the QP projection; overwritten by alpha*F. +u_lambda_position_list: [ + [10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10], + [10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10]] +u_u_position: [0.01, 0.01, 0.01] + + +### NO NEED TO CHANGE THE BELOW. Parameters needed for the C3Options struct but +### are overwritten by other sampling C3 parameters. +use_predicted_x0: false # instead: use_predicted_x0_c3, + # use_predicted_x0_repos, + # use_predicted_x0_reset_mechanism +dt: 0 # instead: planning_dt_pose, planning_dt_position +solve_dt: 0 # unused +mu: [] # instead based on indexing into mu_per_pair_type +num_contacts: 0 # instead based on summing index of resolve_contacts_to_lists +# Instead for the below, index into their _list versions. +g_gamma: [] +g_lambda_n: [] +g_lambda_t: [] +g_lambda: [] +u_gamma: [] +u_lambda_n: [] +u_lambda_t: [] +u_lambda: [] diff --git a/examples/sampling_c3/push_t/parameters/sampling_params.yaml b/examples/sampling_c3/push_t/parameters/sampling_params.yaml new file mode 100644 index 000000000..6c5d4e7c8 --- /dev/null +++ b/examples/sampling_c3/push_t/parameters/sampling_params.yaml @@ -0,0 +1,44 @@ +# Sampling strategies (defined as SamplingStrategy enum in sampling_params.h): +# 0. kRadiallySymmetric: radially distributed samples on a planar circle. +# 1. kRandomOnCircle: random samples on a planar circle. +# 2. kRandomOnSphere: random samples on a spherical surface. +# 3. kFixed: fixed set of samples. +# 4. kRandomOnPerimeter: random samples on a perimeter offset past the object +# surface (roughly planar). +# 5. kRandomOnShell: random samples on a shell offset past the object +# surface. +sampling_strategy: 4 + +### Parameters relevant for all sampling strategies. +filter_samples_for_safety: true + +# Number of samples. Add 2 for repositioning (current location, previous +# repositioning target) and 1 for C3 (current location) to get total sample +# number solved in parallel for each control loop. +num_additional_samples_repos: 1 +num_additional_samples_c3: 2 + +# Sample buffer parameters. +consider_best_buffer_sample_when_leaving_c3: true +N_sample_buffer: 100 +pos_error_sample_retention: 0.003 +ang_error_sample_retention: 0.0349 # 0.0349 rad = 2 deg + +# Shared across multiple sampling strategies. +sampling_radius: 0.16 # kRadiallySymmetric, kRandomOnCircle, kRandomOnSphere +sampling_height: 0.005 # kRadiallySymmetric, kRandomOnCircle, kRandomOnPerimeter +sample_projection_clearance: 0.02 # kRandomOnPerimeter, kRandomOnShell +min_angle_from_vertical: 1.49 # kRandomOnSphere, kRandomOnShell +max_angle_from_vertical: 1.5 # kRandomOnSphere, kRandomOnShell + +# kFixed parameters. +fixed_sample_locations: [[0.45, 0.270, 0.270], # sample 1 + [0.34, 0.455, 0.225]] # sample 2 + +# kRandomOnPerimeter parameters. +grid_x_limits: [-0.12, 0.08] +grid_y_limits: [-0.08, 0.08] + +# kRandomOnShell parameters. +min_sampling_radius: 0.07 +max_sampling_radius: 0.10 diff --git a/examples/sampling_c3/push_t/parameters/sim_params.yaml b/examples/sampling_c3/push_t/parameters/sim_params.yaml new file mode 100644 index 000000000..8ed1c5225 --- /dev/null +++ b/examples/sampling_c3/push_t/parameters/sim_params.yaml @@ -0,0 +1,14 @@ +object_model: examples/sampling_c3/urdf/push_t.sdf + +franka_publish_rate: 1000 +object_publish_rate: 10 +actuator_delay: 0.000 + +visualize_drake_sim: false +publish_efforts: true + +dt: 0.0001 +realtime_rate: 1 + +q_init_franka: [2.191, 1.1, -1.33, -2.22, 1.30, 2.02, 0.08] +q_init_object: [1.0, 0, 0, 0, 0.5, 0, -0.009] diff --git a/examples/sampling_c3/push_t/parameters/vis_params.yaml b/examples/sampling_c3/push_t/parameters/vis_params.yaml new file mode 100644 index 000000000..87c9195fd --- /dev/null +++ b/examples/sampling_c3/push_t/parameters/vis_params.yaml @@ -0,0 +1,37 @@ +ee_vis_model: examples/sampling_c3/urdf/ee_visualization_model.urdf +object_vis_model: examples/sampling_c3/urdf/push_t.sdf +visualizer_publish_rate: 30 + +camera_pose: [1.5, 0, 0.6] +camera_target: [0.5, 0, 0.5] + +visualize_c3_workspace: false +visualize_c3_state: true + +visualize_center_of_mass_plan_curr: false +visualize_c3_forces_curr: false +visualize_center_of_mass_plan_best: false +visualize_c3_forces_best: false + +visualize_is_c3_mode: true +visualize_sample_locations: true +visualize_sample_buffer: true + +# Note: Colors can either be empty ([]) or RGB values between 0 and 1. If left +# empty, the colors defined in the URDF/SDF files will be used. +is_c3_mode_color: [1.0, 0.43, 1.0] +sample_color: [1.0, 1.0, 0.0] + +visualize_execution_plan: false + +visualize_c3_plan_curr: false +c3_curr_object_color: [] +c3_curr_ee_color: [0.8, 0.8, 0.8] +df_curr_object_color: [] +df_curr_ee_color: [0.5, 1.0, 0.5] + +visualize_c3_plan_best: false +c3_best_object_color: [1.0, 0.64, 0.0] +c3_best_ee_color: [1.0, 0.64, 0.0] +df_best_object_color: [1.0, 0.64, 0.0] +df_best_ee_color: [1.0, 0.64, 0.0] diff --git a/examples/sampling_c3/urdf/jack_ground.sdf b/examples/sampling_c3/urdf/jack_control.sdf similarity index 100% rename from examples/sampling_c3/urdf/jack_ground.sdf rename to examples/sampling_c3/urdf/jack_control.sdf diff --git a/examples/sampling_c3/urdf/push_t.sdf b/examples/sampling_c3/urdf/push_t.sdf new file mode 100644 index 000000000..bc3104bcf --- /dev/null +++ b/examples/sampling_c3/urdf/push_t.sdf @@ -0,0 +1,93 @@ + + + + + + 0 0 0 0 0 0 + 0.5 + + 0.0001333 + 0.0011333 + 0.0011333 + 0.0 + 0.0 + 0.0 + + + + 0 0 0 0 0 0 + + + 0.16 0.04 0.04 + + + + 0 0.8 0.8 1 + + + + 0 0 0 0 0 0 + + + 0.16 0.04 0.04 + + + + + 3.0e7 + 0.18 + 10 + 0.3 + + + + + + + -0.10 0 0 0 0 1.5708 + 0.5 + + 0.0001333 + 0.0011333 + 0.0011333 + 0.0 + 0.0 + 0.0 + + + + -0.10 0 0 0 0 1.5708 + + + 0.16 0.04 0.04 + + + + 0 0.8 0.8 1 + + + + -0.10 0 0 0 0 1.5708 + + + 0.16 0.04 0.04 + + + + + 3.0e7 + 0.18 + 10 + 0.3 + + + + + + vertical_link + horizontal_link + 0 0 0 0 0 0 + + + + \ No newline at end of file diff --git a/examples/sampling_c3/urdf/push_t_control.sdf b/examples/sampling_c3/urdf/push_t_control.sdf new file mode 100644 index 000000000..2c017fd56 --- /dev/null +++ b/examples/sampling_c3/urdf/push_t_control.sdf @@ -0,0 +1,117 @@ + + + + + + 0 0 0 0 0 0 + 0.5 + + 0.0001333 + 0.0011333 + 0.0011333 + 0.0 + 0.0 + 0.0 + + + + 0 0 0 0 0 0 + + + 0.16 0.04 0.04 + + + + 0 0.8 0.8 1 + + + + 0 0 0 0 0 0 + + + 0.16 0.04 0.04 + + + + + 3.0e7 + 0.18 + 10 + 0.3 + + + + -0.12 0.08 -0.02 0 0 0 + + + 0.001 + + + + + -0.12 -0.08 -0.02 0 0 0 + + + 0.001 + + + + + 0.08 0.0 -0.02 0 0 0 + + + 0.001 + + + + + + + + -0.10 0 0 0 0 1.5708 + 0.5 + + 0.0001333 + 0.0011333 + 0.0011333 + 0.0 + 0.0 + 0.0 + + + + -0.10 0 0 0 0 1.5708 + + + 0.16 0.04 0.04 + + + + 0 0.8 0.8 1 + + + + -0.10 0 0 0 0 1.5708 + + + 0.16 0.04 0.04 + + + + + 3.0e7 + 0.18 + 10 + 0.3 + + + + + + vertical_link + horizontal_link + 0 0 0 0 0 0 + + + + \ No newline at end of file