test_urdf.cc

#include <memory>
#include <string>

#include "fmt/ostream.h"
#include <gflags/gflags.h>

#include "drake/common/drake_assert.h"
#include "drake/common/find_resource.h"
#include "drake/geometry/geometry_visualization.h"
#include "drake/geometry/scene_graph.h"
#include "drake/lcm/drake_lcm.h"

// #include "drake/systems/analysis/implicit_euler_integrator.h"
// #include "drake/systems/analysis/runge_kutta3_integrator.h"
// #include "drake/systems/analysis/semi_explicit_euler_integrator.h"
#include "drake/systems/analysis/simulator.h"
#include "drake/systems/analysis/simulator_gflags.h"
#include "drake/systems/analysis/simulator_print_stats.h"
#include "drake/systems/framework/diagram_builder.h"

#include "drake/multibody/parsing/parser.h"
#include "drake/multibody/plant/multibody_plant.h"
#include "drake/multibody/tree/revolute_joint.h"
#include "drake/multibody/tree/revolute_joint.h"

#include "drake/math/rotation_matrix.h"



// #include "drake/systems/primitives/constant_vector_source.h"


DEFINE_double(
    mbp_discrete_update_period, 1.0E-3,
    "The fixed-time step period (in seconds) of discrete updates for the "
    "multibody plant modeled as a discrete system. Strictly positive. "
    "Set to zero for a continuous plant model.");

DEFINE_double(target_realtime_rate, 1.0,
              "Playback speed.  See documentation for "
              "Simulator::set_target_realtime_rate() for details.");


namespace drake {
namespace examples {
namespace test_urdf {
namespace {

using math::RigidTransformd;

using geometry::SceneGraph;
using geometry::SourceId;
using geometry::Sphere;
using lcm::DrakeLcm;
using Eigen::Vector3d;
using Eigen::VectorXd;
using drake::multibody::MultibodyPlant;
using multibody::Parser;
using multibody::RevoluteJoint;

using multibody::CoulombFriction;


int do_main(){
 // Build a generic multibody plant.
	systems::DiagramBuilder<double> builder;

	SceneGraph<double>& scene_graph = *builder.AddSystem<SceneGraph>();
  	scene_graph.set_name("scene_graph");

  	MultibodyPlant<double>& plant =
      *builder.AddSystem<MultibodyPlant>(FLAGS_mbp_discrete_update_period);
  	plant.RegisterAsSourceForSceneGraph(&scene_graph);

  	Parser parser(&plant);
  	std::string full_name =
      FindResourceOrThrow("drake/examples/ddr_sdf/model_ddrcustom.urdf");
  	      // FindResourceOrThrow("drake/examples/ddr_sdf/pioneer2dx/model.sdf");

  	parser.AddModelFromFile(full_name);

  	// Now the model is complete.
    // plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("chassis"));

    const double kPadMinorRadius = 6e-3;   // 6 mm.
  	Eigen::Vector3d p_FS;
  	p_FS(0) = 0;
  	p_FS(1) = 0;
  	p_FS(2) = 0;

  	// Pose of the sphere frame S in the finger frame F.
    const RigidTransformd X_FS(p_FS);

    auto& base_link = plant.GetBodyByName("base_link");

	CoulombFriction<double> friction(0.5,0.5);

  	plant.RegisterCollisionGeometry(base_link, X_FS, Sphere(kPadMinorRadius),
                                     "collision" + std::to_string(1), friction);

  	// const Vector4<double> red(0.8, 0.2, 0.2, 1.0);
  	// plant.RegisterVisualGeometry(base_link, X_FS, Sphere(kPadMinorRadius),
   //                                   "collision" + std::to_string(1), friction);


  	fmt::print("Test Point 1\n");




  	plant.Finalize();

  	  // Set how much penetration (in meters) we are willing to accept.
  	plant.set_penetration_allowance(1e-5);
  	plant.set_stiction_tolerance(1e-2);

  // There are two actuators. Both are revolute joints
  	DRAKE_DEMAND(plant.num_actuators() == 2);
  	DRAKE_DEMAND(plant.num_actuated_dofs() == 2);


  	// Sanity check on the availability of the optional source id before using it.
	DRAKE_DEMAND(plant.geometry_source_is_registered());

  	fmt::print("Test Point 2\n");

  	builder.Connect(
      plant.get_geometry_poses_output_port(),
      scene_graph.get_source_pose_port(plant.get_source_id().value()));

  	builder.Connect(
      scene_graph.get_query_output_port(),
      plant.get_geometry_query_input_port());

  	// geometry::ConnectDrakeVisualizer(&builder, scene_graph);

  	fmt::print("Test Point 3\n");

  	auto diagram = builder.Build();

  	DrakeLcm lcm;
 	geometry::ConnectDrakeVisualizer(&builder, scene_graph, &lcm);

  // Create a context for this system:
  	std::unique_ptr<systems::Context<double>> diagram_context =
      diagram->CreateDefaultContext();
  	diagram->SetDefaultContext(diagram_context.get());
  	systems::Context<double>& plant_context =
      diagram->GetMutableSubsystemContext(plant, diagram_context.get());

    fmt::print("Test Point 4\n");

  	const Eigen::VectorXd tau = Eigen::VectorXd::Zero(plant.num_actuated_dofs());
    fmt::print("tau = {}\n", tau);
  	plant.get_actuation_input_port().FixValue(&plant_context, tau);
  	// plant.get_input_port(0).FixValue(&plant_context, Vector1d(0.0));

  	fmt::print("Test Point 5\n");


  	// systems::Simulator<double> simulator(*diagram);
  	systems::Simulator<double> simulator(*diagram, std::move(diagram_context));

  	fmt::print("Test Point 6\n");

  	simulator.set_target_realtime_rate(FLAGS_target_realtime_rate);
  	fmt::print("Test Point 7\n");
  	simulator.get_mutable_context().SetAccuracy(1e-4);
  	fmt::print("Test Point 8\n");

  	simulator.Initialize();
  	simulator.AdvanceTo(10);
  	fmt::print("Test Point 9\n");

  	// Add a line break before simulator statistics.
  	fmt::print("\n");

  	systems::PrintSimulatorStatistics(simulator);
  	fmt::print("Test Point 10\n");


	return 0;
}



}  //namespace

}  //test_urdf
} // example
} // drake


int main(int argc, char* argv[]) {
  gflags::SetUsageMessage(
      "Demo used to exercise MultibodyPlant's contact modeling in a gripping "
      "scenario. SceneGraph is used for both visualization and contact "
      "handling. "
      "Launch drake-visualizer before running this example.");
  gflags::ParseCommandLineFlags(&argc, &argv, true);
  return drake::examples::test_urdf::do_main();
}