Added all the configurations to launch two motors

JorisWeeda · JorisWeeda · commit bc97020afb1d · 2020-08-19T17:24:18.000+02:00
diff --git a/march_hardware/config/right_hip_fe.json b/march_hardware/config/right_hip_fe.json
@@ -70,9 +70,4 @@
         "type":"float",
         "value":0
     },
-    {
-        "name":"axis1.config.watchdog_timeout",
-        "type":"float",
-        "value":0
-    }
 ]
diff --git a/march_hardware/config/right_knee.json b/march_hardware/config/right_knee.json
@@ -0,0 +1,74 @@
+[
+    {
+        "name":"axis1.motor.config.current_lim",
+        "type":"float",
+        "value":30.0
+    },
+    {
+        "name":"axis1.encoder.config.cpr",
+        "type":"int32",
+        "value":4096
+    },
+    {
+        "name":"axis1.encoder.config.use_index",
+        "type":"bool",
+        "value":1
+    },
+    {
+        "name":"config.brake_resistance",
+        "type":"float",
+        "value":0.0
+    },
+    {
+        "name":"axis1.motor.config.pole_pairs",
+        "type":"int32",
+        "value":21
+    },
+    {
+        "name":"axis1.motor.config.calibration_current",
+        "type":"float",
+        "value":10.0
+    },
+    {
+        "name":"axis1.controller.config.vel_limit",
+        "type":"float",
+        "value":200000.0
+    },
+    {
+        "name":"axis1.motor.config.current_control_bandwidth",
+        "type":"float",
+        "value":100.0
+    },
+
+    {
+        "name":"axis1.motor.config.motor_type",
+        "type":"uint8",
+        "value":0
+    },
+    {
+        "name":"axis1.controller.config.vel_gain",
+        "type":"float",
+        "value":0.01
+    },
+    {
+        "name":"axis1.controller.config.vel_integrator_gain",
+        "type":"float",
+        "value":0.05
+    },
+    {
+        "name":"axis1.controller.config.control_mode",
+        "type":"uint8",
+        "value":1
+    },
+    {
+        "name":"axis1.sensorless_estimator.config.pm_flux_linkage",
+        "type":"float",
+        "value":0.001944723
+    },
+
+    {
+        "name":"axis1.config.watchdog_timeout",
+        "type":"float",
+        "value":0
+    }
+]
diff --git a/march_hardware/include/march_hardware/motor_controller/odrive/usb_master.h b/march_hardware/include/march_hardware/motor_controller/odrive/usb_master.h
@@ -19,7 +19,7 @@ class UsbMaster
   std::shared_ptr<OdriveEndpoint> getSerialConnection(const std::string& serial_number);
 
 private:
-  std::vector<std::shared_ptr<OdriveEndpoint>> odrive_endpoints;
+  std::vector<std::shared_ptr<OdriveEndpoint>> odrive_endpoints_;
 };
 
 }  // namespace march
diff --git a/march_hardware/src/odrive/usb_master.cpp b/march_hardware/src/odrive/usb_master.cpp
@@ -6,20 +6,20 @@ namespace march
 {
 std::shared_ptr<OdriveEndpoint> UsbMaster::getSerialConnection(const std::string& serial_number)
 {
-  for (uint i = 0; i < this->odrive_endpoints.size(); ++i)
+  for (uint i = 0; i < this->odrive_endpoints_.size(); ++i)
   {
-    if (odrive_endpoints[i]->getSerialNumber().compare(serial_number) == 0)
+    if (this->odrive_endpoints_[i]->getSerialNumber().compare(serial_number) == 0)
     {
-      return odrive_endpoints[i];
+      return this->odrive_endpoints_[i];
     }
   }
 
   std::shared_ptr<OdriveEndpoint> odrive_endpoint = std::make_shared<OdriveEndpoint>();
-  ROS_INFO("Open serial connection %s", serial_number);
+  ROS_INFO("Open serial connection %s", serial_number.c_str());
   odrive_endpoint->open_connection(serial_number);
 
-  odrive_endpoints.push_back(odrive_endpoint);
-  odrive_endpoint->getSerialNumber();
+  this->odrive_endpoints_.push_back(odrive_endpoint);
+
   return odrive_endpoint;
 }
 }  // namespace march
diff --git a/march_hardware_builder/include/march_hardware_builder/allowed_robot.h b/march_hardware_builder/include/march_hardware_builder/allowed_robot.h
@@ -42,7 +42,7 @@ class AllowedRobot
     }
     else if (robot_name == "two_odrive_joints")
     {
-        this->value = two_odrive_joints;
+      this->value = two_odrive_joints;
     }
     else if (robot_name == "test_joint_linear")
     {
@@ -76,7 +76,7 @@ class AllowedRobot
     }
     else if (this->value == AllowedRobot::two_odrive_joints)
     {
-        return base_path.append("/robots/two_odrive_joints.yaml");
+      return base_path.append("/robots/two_odrive_joints.yaml");
     }
     else if (this->value == AllowedRobot::odrive_test_joint_rotational)
     {
@@ -126,6 +126,9 @@ class AllowedRobot
       case odrive_test_joint_rotational:
         out << "odrive_test_joint_rotational";
         break;
+      case two_odrive_joints:
+        out << "two_odrive_joints";
+        break;
       case pdb:
         out << "pdb";
         break;
diff --git a/march_hardware_builder/include/march_hardware_builder/hardware_builder.h b/march_hardware_builder/include/march_hardware_builder/hardware_builder.h
@@ -71,7 +71,7 @@ class HardwareBuilder
 
   static march::Joint createJoint(const YAML::Node& joint_config, const std::string& joint_name,
                                   const urdf::JointConstSharedPtr& urdf_joint, march::PdoInterfacePtr pdo_interface,
-                                  march::SdoInterfacePtr sdo_interface, march::UsbMaster usb_master);
+                                  march::SdoInterfacePtr sdo_interface, march::UsbMaster& usb_master);
   static std::unique_ptr<march::AbsoluteEncoder> createAbsoluteEncoder(const YAML::Node& absolute_encoder_config,
                                                                        const urdf::JointConstSharedPtr& urdf_joint);
   static std::unique_ptr<march::IncrementalEncoder>
@@ -89,7 +89,7 @@ class HardwareBuilder
 
   static std::unique_ptr<march::OdriveMotor> createOdrive(const YAML::Node& imc_config, march::ActuationMode mode,
                                                           const urdf::JointConstSharedPtr& urdf_joint,
-                                                          march::UsbMaster usb_master);
+                                                          march::UsbMaster& usb_master);
 
   static const std::vector<std::string> INCREMENTAL_ENCODER_REQUIRED_KEYS;
   static const std::vector<std::string> ABSOLUTE_ENCODER_REQUIRED_KEYS;
@@ -113,7 +113,7 @@ class HardwareBuilder
    * @return list of created joints
    */
   std::vector<march::Joint> createJoints(const YAML::Node& joints_config, march::PdoInterfacePtr pdo_interface,
-                                         march::SdoInterfacePtr sdo_interface, march::UsbMaster usb_master) const;
+                                         march::SdoInterfacePtr sdo_interface, march::UsbMaster& usb_master) const;
 
   YAML::Node robot_config_;
   urdf::Model urdf_;
diff --git a/march_hardware_builder/robots/two_odrive_joints.yaml b/march_hardware_builder/robots/two_odrive_joints.yaml
@@ -3,26 +3,26 @@ march4:
   cycleTime: 4
   slaveTimeout: 50
   joints:
-#    - right_hip_fe:
-#        actuationMode: torque
-#        allowActuation: true
-#        odrive:
-#          serial_number: "0x2084387E304E"
-#          axis: "axis0"
-#          absoluteEncoder:
-#            resolution: 17
-#            minPositionIU: 2045
-#            maxPositionIU: 45632
-#          incrementalEncoder:
-#            resolution: 12
-#            transmission: 101
+    - right_hip_fe:
+        actuationMode: torque
+        allowActuation: true
+        odrive:
+          serial_number: "0x2084387E304E"
+          axis: "axis0"
+          absoluteEncoder:
+            resolution: 17
+            minPositionIU: 2045
+            maxPositionIU: 45632
+          incrementalEncoder:
+            resolution: 12
+            transmission: 101
 
     - right_knee:
         actuationMode: torque
         allowActuation: true
         odrive:
           serial_number: "0x2084387E304E"
-          axis: "axis0"
+          axis: "axis1"
           absoluteEncoder:
             resolution: 17
             minPositionIU: 41533
diff --git a/march_hardware_builder/src/hardware_builder.cpp b/march_hardware_builder/src/hardware_builder.cpp
@@ -72,7 +72,7 @@ std::unique_ptr<march::MarchRobot> HardwareBuilder::createMarchRobot()
   auto pdo_interface = march::PdoInterfaceImpl::create();
   auto sdo_interface = march::SdoInterfaceImpl::create();
 
-  auto usb_master = march::UsbMaster();
+  march::UsbMaster usb_master = march::UsbMaster();
 
   std::vector<march::Joint> joints = this->createJoints(config["joints"], pdo_interface, sdo_interface, usb_master);
 
@@ -98,7 +98,7 @@ std::unique_ptr<march::MarchRobot> HardwareBuilder::createMarchRobot()
 march::Joint HardwareBuilder::createJoint(const YAML::Node& joint_config, const std::string& joint_name,
                                           const urdf::JointConstSharedPtr& urdf_joint,
                                           march::PdoInterfacePtr pdo_interface, march::SdoInterfacePtr sdo_interface,
-                                          march::UsbMaster usb_master)
+                                          march::UsbMaster& usb_master)
 {
   ROS_DEBUG("Starting creation of joint %s", joint_name.c_str());
   if (!urdf_joint)
@@ -152,7 +152,7 @@ march::Joint HardwareBuilder::createJoint(const YAML::Node& joint_config, const
 std::unique_ptr<march::OdriveMotor> HardwareBuilder::createOdrive(const YAML::Node& odrive_config,
                                                                   march::ActuationMode mode,
                                                                   const urdf::JointConstSharedPtr& urdf_joint,
-                                                                  march::UsbMaster usb_master)
+                                                                  march::UsbMaster& usb_master)
 {
   if (!odrive_config || !urdf_joint)
   {
@@ -165,11 +165,25 @@ std::unique_ptr<march::OdriveMotor> HardwareBuilder::createOdrive(const YAML::No
   YAML::Node absolute_encoder_config = odrive_config["absoluteEncoder"];
   std::string axis = odrive_config["axis"].as<std::string>();
   std::string serial_number = odrive_config["serial_number"].as<std::string>();
-  std::string json_configuration_path = ros::package::getPath("march_hardware").append("/config/march_odrive.json");
+
+  std::string configuration_path =
+      ros::package::getPath("march_hardware").append("/config/" + urdf_joint->name + ".json");
+
+  std::ifstream file(configuration_path);
+
+  if (file.fail())
+  {
+    throw std::runtime_error("Could not open configuration file for the odrive");
+  }
+
+  if (file.is_open())
+  {
+    file.close();
+  }
 
   auto odrive_endpoint = usb_master.getSerialConnection(serial_number);
 
-  return std::make_unique<march::OdriveMotor>(axis, odrive_endpoint, mode, json_configuration_path);
+  return std::make_unique<march::OdriveMotor>(axis, odrive_endpoint, mode, configuration_path);
 }
 
 std::unique_ptr<march::IMotionCube> HardwareBuilder::createIMotionCube(const YAML::Node& imc_config,
@@ -329,7 +343,7 @@ void HardwareBuilder::initUrdf()
 std::vector<march::Joint> HardwareBuilder::createJoints(const YAML::Node& joints_config,
                                                         march::PdoInterfacePtr pdo_interface,
                                                         march::SdoInterfacePtr sdo_interface,
-                                                        march::UsbMaster usb_master) const
+                                                        march::UsbMaster& usb_master) const
 {
   std::vector<march::Joint> joints;
   for (const YAML::Node& joint_config : joints_config)
diff --git a/march_hardware_interface/config/two_odrive_joints/controllers.yaml b/march_hardware_interface/config/two_odrive_joints/controllers.yaml
@@ -13,16 +13,16 @@ march:
       type: effort_controllers/JointTrajectoryController
       allow_partial_joints_goal: true
       joints:
-#        - right_hip_fe
+        - right_hip_fe
         - right_knee
       gains: # Required because we're controlling an effort interface
-#        right_hip_fe: {p: 0, i: 0, d: 0, i_clamp: 100, publish_state: true, antiwindup: true}
+        right_hip_fe: {p: 0, i: 0, d: 0, i_clamp: 100, publish_state: true, antiwindup: true}
         right_knee: {p: 0, i: 0, d: 0, i_clamp: 100, publish_state: true, antiwindup: true}
       constraints:
-#        right_hip_fe:
-#          margin_soft_limit_error: 0.5
-#          trajectory: 0.305
-#          goal: 0.305
+        right_hip_fe:
+          margin_soft_limit_error: 0.5
+          trajectory: 0.305
+          goal: 0.305
         right_knee:
           margin_soft_limit_error: 0.5
           trajectory: 0.305

Original file line number	Diff line number	Diff line change
`@@ -6,20 +6,20 @@ namespace march`
`6`	`6`	`{`
`7`	`7`	`std::shared_ptr<OdriveEndpoint> UsbMaster::getSerialConnection(const std::string& serial_number)`
`8`	`8`	`{`
`9`		`- for (uint i = 0; i < this->odrive_endpoints.size(); ++i)`
	`9`	`+ for (uint i = 0; i < this->odrive_endpoints_.size(); ++i)`
`10`	`10`	`{`
`11`		`- if (odrive_endpoints[i]->getSerialNumber().compare(serial_number) == 0)`
	`11`	`+ if (this->odrive_endpoints_[i]->getSerialNumber().compare(serial_number) == 0)`
`12`	`12`	`{`
`13`		`- return odrive_endpoints[i];`
	`13`	`+ return this->odrive_endpoints_[i];`
`14`	`14`	`}`
`15`	`15`	`}`
`16`	`16`
`17`	`17`	`std::shared_ptr<OdriveEndpoint> odrive_endpoint = std::make_shared<OdriveEndpoint>();`
`18`		`- ROS_INFO("Open serial connection %s", serial_number);`
	`18`	`+ ROS_INFO("Open serial connection %s", serial_number.c_str());`
`19`	`19`	`odrive_endpoint->open_connection(serial_number);`
`20`	`20`
`21`		`- odrive_endpoints.push_back(odrive_endpoint);`
`22`		`- odrive_endpoint->getSerialNumber();`
	`21`	`+ this->odrive_endpoints_.push_back(odrive_endpoint);`
	`22`	`+`
`23`	`23`	`return odrive_endpoint;`
`24`	`24`	`}`
`25`	`25`	`} // namespace march`
Original file line number	Diff line number	Diff line change
`@@ -42,7 +42,7 @@ class AllowedRobot`
`42`	`42`	`}`
`43`	`43`	`else if (robot_name == "two_odrive_joints")`
`44`	`44`	`{`
`45`		`- this->value = two_odrive_joints;`
	`45`	`+ this->value = two_odrive_joints;`
`46`	`46`	`}`
`47`	`47`	`else if (robot_name == "test_joint_linear")`
`48`	`48`	`{`
`@@ -76,7 +76,7 @@ class AllowedRobot`
`76`	`76`	`}`
`77`	`77`	`else if (this->value == AllowedRobot::two_odrive_joints)`
`78`	`78`	`{`
`79`		`- return base_path.append("/robots/two_odrive_joints.yaml");`
	`79`	`+ return base_path.append("/robots/two_odrive_joints.yaml");`
`80`	`80`	`}`
`81`	`81`	`else if (this->value == AllowedRobot::odrive_test_joint_rotational)`
`82`	`82`	`{`
`@@ -126,6 +126,9 @@ class AllowedRobot`
`126`	`126`	`case odrive_test_joint_rotational:`
`127`	`127`	`out << "odrive_test_joint_rotational";`
`128`	`128`	`break;`
	`129`	`+ case two_odrive_joints:`
	`130`	`+ out << "two_odrive_joints";`
	`131`	`+ break;`
`129`	`132`	`case pdb:`
`130`	`133`	`out << "pdb";`
`131`	`134`	`break;`