fix feeding contact parameters. Drake side driving straight works.

RobotLocomotion · Sep 19, 2016 · e66d9a0 · e66d9a0
1 parent badf2c6
commit e66d9a0
Show file tree

Hide file tree

Showing 5 changed files with 35 additions and 14 deletions.
diff --git a/drake/automotive/README.md b/drake/automotive/README.md
@@ -19,7 +19,7 @@ To run the Drake Visualizer, open a terminal and execute the following commands:
 
 ```
 $ cd [drake-distro]/drake/automotive
-$ ../../../build/install/bin/drake-visualizer
+$ ../../build/install/bin/drake-visualizer
 ```
 
 The Drake Visualizer window should appear.
@@ -65,7 +65,7 @@ To start the simulation, open a new terminal and execute the following:
 
 ```
 $ cd [drake-distro]/drake/automotive
-$ ../../../build/drake/bin/car_sim_lcm models/prius/prius.urdf models/stata_garage_p1.sdf
+$ ../../build/drake/bin/car_sim_lcm models/prius/prius.urdf models/stata_garage_p1.sdf
 ```
 
 ### Simulation Using Drake + LCM + ROS

diff --git a/drake/automotive/car_sim_lcm.cc b/drake/automotive/car_sim_lcm.cc
@@ -24,9 +24,15 @@ int do_main(int argc, const char* argv[]) {
 
   drake::parsers::ModelInstanceIdTable model_instances;
 
+  double penetration_stiffness = atof(argv[argc-3]);
+  double penetration_damping = atof(argv[argc-2]);
+  double friction_coefficient = atof(argv[argc-1]); 
+
   // Initializes the rigid body system.
   auto rigid_body_sys = CreateRigidBodySystem(argc, argv, &duration,
-      &model_instances);
+      &model_instances, &penetration_stiffness, &penetration_damping,&friction_coefficient);
+
+
 
   // Initializes and cascades all of the other systems.
   auto vehicle_sys = CreateVehicleSystem(rigid_body_sys);

diff --git a/drake/automotive/car_simulation.cc b/drake/automotive/car_simulation.cc
@@ -54,7 +54,10 @@ void PrintUsageInstructions(const std::string& executable_name) {
 
 std::shared_ptr<RigidBodySystem> CreateRigidBodySystem(
     int argc, const char* argv[], double* duration,
-    ModelInstanceIdTable* model_instance_id_table) {
+    ModelInstanceIdTable* model_instance_id_table,
+    double* penetration_stiffness,
+    double* penetration_damping,
+    double* friction_coefficient) {
   if (argc < 2) {
     PrintUsageInstructions(argv[0]);
     exit(EXIT_FAILURE);
@@ -83,8 +86,9 @@ std::shared_ptr<RigidBodySystem> CreateRigidBodySystem(
     *duration = std::numeric_limits<double>::infinity();
   }
 
+  if (argc>3){
   // Adds the environment.
-  for (int ii = 2; ii < argc; ++ii) {
+  for (int ii = 2; ii < argc-3; ++ii) {
     if (std::string(argv[ii]) == "--duration") {
       if (++ii == argc) {
         throw std::runtime_error(
@@ -101,6 +105,12 @@ std::shared_ptr<RigidBodySystem> CreateRigidBodySystem(
     }
   }
 
+  rigid_body_sys->penetration_stiffness = atof(argv[argc-3]);
+  rigid_body_sys->penetration_damping = atof(argv[argc-2]);
+  rigid_body_sys->friction_coefficient = atof(argv[argc-1]); 
+    // SetRigidBodySystemParameters(rigid_body_sys, atof(argv[argc-3]),atof(argv[argc-2]),atof(argv[argc-1]));
+}
+
   // Adds a flat terrain if no environment is specified.
   if (argc < 3) {
     const std::shared_ptr<RigidBodyTree>& tree =
@@ -109,16 +119,16 @@ std::shared_ptr<RigidBodySystem> CreateRigidBodySystem(
   }
 
   // Sets various simulation parameters.
-  // SetRigidBodySystemParameters(rigid_body_sys.get());
+
 
   return rigid_body_sys;
 }
 
-void SetRigidBodySystemParameters(RigidBodySystem* rigid_body_sys) {
-  rigid_body_sys->penetration_stiffness = 5000.0;
-  rigid_body_sys->penetration_damping =
-    rigid_body_sys->penetration_stiffness / 10.0;
-  rigid_body_sys->friction_coefficient = 10.0;  // essentially infinite friction
+void SetRigidBodySystemParameters(RigidBodySystem* rigid_body_sys, double penetration_stiffness,
+  double penetration_damping, double friction_coefficient) {
+  rigid_body_sys->penetration_stiffness = penetration_stiffness;
+  rigid_body_sys->penetration_damping = penetration_damping;
+  rigid_body_sys->friction_coefficient = friction_coefficient;  // essentially infinite friction
 }
 
 double ParseDuration(int argc, const char* argv[]) {

diff --git a/drake/automotive/car_simulation.h b/drake/automotive/car_simulation.h
@@ -60,7 +60,10 @@ void PrintUsageInstructions(const std::string& executable_name);
 DRAKEAUTOMOTIVE_EXPORT
 std::shared_ptr<RigidBodySystem> CreateRigidBodySystem(
     int argc, const char* argv[], double* duration,
-    drake::parsers::ModelInstanceIdTable* model_instance_id_table);
+    drake::parsers::ModelInstanceIdTable* model_instance_id_table,
+    double* penetration_stiffness,
+    double* penetration_damping,
+    double* friction_coefficient);
 
 /**
  * Checks the command line arguments looking for a "--duration" flag followed
@@ -83,7 +86,9 @@ double ParseDuration(int argc, const char* argv[]);
  * @param[in] rigid_body_sys The rigid body system to modify.
  */
 DRAKEAUTOMOTIVE_EXPORT
-void SetRigidBodySystemParameters(RigidBodySystem* rigid_body_sys);
+void SetRigidBodySystemParameters(RigidBodySystem* rigid_body_sys,
+     double penetration_stiffness = 750000, double penetration_damping = 75000, 
+    double friction_coefficient =10);
 
 /**
  * Adds a box-shaped terrain to the specified rigid body tree. This directly

diff --git a/drake/automotive/steering_command_driver.py b/drake/automotive/steering_command_driver.py
@@ -44,7 +44,7 @@
 TURN_LEFT_SIGN = 1.0
 TURN_RIGHT_SIGN = -1.0
 
-THROTTLE_SCALE = 1
+THROTTLE_SCALE = 4
 BRAKE_SCALE = 1.0
 
 def _limit_steering(requested_value):