Merge pull request #173 from project-march/develop

Update master

Author: Olavhaasie

march_hardware/src/EtherCAT/EthercatMaster.cpp

@@ -183,7 +183,7 @@ int EthercatMaster::receiveProcessData()
   int wkc =  ec_receive_processdata(EC_TIMEOUTRET);
   if (wkc < this->expectedWKC)
   {
-    ROS_WARN("Working counter lower than expected. EtherCAT connection may not be optimal");
+    ROS_WARN_THROTTLE(1, "Working counter lower than expected. EtherCAT connection may not be optimal");
   }
   return wkc;
 }

march_hardware/src/IMotionCube.cpp

@@ -461,23 +461,23 @@ std::string IMotionCube::parseDetailedError(uint16 detailedError)
 
 bool IMotionCube::goToTargetState(IMotionCubeTargetState targetState)
 {
-  ROS_INFO("\tTry to go to '%s'", targetState.getDescription().c_str());
+  ROS_DEBUG("\tTry to go to '%s'", targetState.getDescription().c_str());
   while (!targetState.isReached(this->getStatusWord()))
   {
     this->setControlWord(targetState.getControlWord());
-    ROS_INFO_THROTTLE(0.5, "\tWaiting for '%s': %s", targetState.getDescription().c_str(),
+    ROS_INFO_DELAYED_THROTTLE(2, "\tWaiting for '%s': %s", targetState.getDescription().c_str(),
                       std::bitset<16>(this->getStatusWord()).to_string().c_str());
     if (targetState.getState() == IMotionCubeTargetState::OPERATION_ENABLED.getState() &&
         this->getState(this->getStatusWord()) == IMCState::fault)
     {
-      ROS_FATAL("IMotionCube went to fault state while attempting to go to %s. Shutting down.",
+      ROS_FATAL("IMotionCube went to fault state while attempting to go to '%s'. Shutting down.",
                 targetState.getDescription().c_str());
       ROS_FATAL("Detailed Error: %s", this->parseDetailedError(this->getDetailedError()).c_str());
       ROS_FATAL("Motion Error: %s", this->parseMotionError(this->getMotionError()).c_str());
       throw std::domain_error("IMC to fault state");
     }
   }
-  ROS_INFO("\tReached '%s'!", targetState.getDescription().c_str());
+  ROS_DEBUG("\tReached '%s'!", targetState.getDescription().c_str());
 }
 
 bool IMotionCube::goToOperationEnabled()

march_hardware/src/Joint.cpp

@@ -25,13 +25,13 @@ void Joint::prepareActuation()
                  this->name.c_str(), this->getActuationMode().toString().c_str());
   if (this->allowActuation)
   {
-    ROS_INFO("Preparing joint %s for actuation", this->name.c_str());
+    ROS_INFO("[%s] Preparing for actuation", this->name.c_str());
     this->iMotionCube.goToOperationEnabled();
-    ROS_INFO("\tJoint %s successfully prepared for actuation", this->name.c_str());
+    ROS_INFO("[%s] Successfully prepared for actuation", this->name.c_str());
   }
   else
   {
-    ROS_ERROR("Trying to prepare joint %s for actuation while it is not "
+    ROS_ERROR("[%s] Trying to prepare for actuation while it is not "
               "allowed to actuate",
               this->name.c_str());
   }
@@ -44,9 +44,9 @@ void Joint::resetIMotionCube()
 
 void Joint::actuateRad(float targetPositionRad)
 {
-  ROS_ASSERT_MSG(this->allowActuation, "Joint %s is not allowed to actuate, "
-                                       "yet its actuate method has been "
-                                       "called.",
+  ROS_ASSERT_MSG(this->allowActuation,
+                 "Joint %s is not allowed to actuate, "
+                 "yet its actuate method has been called",
                  this->name.c_str());
   // TODO(BaCo) check that the position is allowed and does not exceed (torque)
   // limits.
@@ -57,17 +57,17 @@ float Joint::getAngleRad()
 {
   if (!hasIMotionCube())
   {
-    ROS_WARN("Joint %s has no iMotionCube", this->name.c_str());
+    ROS_WARN("[%s] Has no iMotionCube", this->name.c_str());
     return -1;
   }
   return this->iMotionCube.getAngleRad();
 }
 
 void Joint::actuateTorque(int targetTorque)
 {
-  ROS_ASSERT_MSG(this->allowActuation, "Joint %s is not allowed to actuate, "
-                                       "yet its actuate method has been "
-                                       "called.",
+  ROS_ASSERT_MSG(this->allowActuation,
+                 "Joint %s is not allowed to actuate, "
+                 "yet its actuate method has been called",
                  this->name.c_str());
   this->iMotionCube.actuateTorque(targetTorque);
 }
@@ -76,7 +76,7 @@ float Joint::getTorque()
 {
   if (!hasIMotionCube())
   {
-    ROS_WARN("Joint %s has no iMotionCube", this->name.c_str());
+    ROS_WARN("[%s] Has no iMotionCube", this->name.c_str());
     return -1;
   }
   return this->iMotionCube.getTorque();
@@ -86,7 +86,7 @@ int Joint::getAngleIU()
 {
   if (!hasIMotionCube())
   {
-    ROS_WARN("Joint %s has no iMotionCube", this->name.c_str());
+    ROS_WARN("[%s] Has no iMotionCube", this->name.c_str());
     return -1;
   }
   return this->iMotionCube.getAngleIU();
@@ -96,7 +96,7 @@ float Joint::getTemperature()
 {
   if (!hasTemperatureGES())
   {
-    ROS_WARN("Joint %s has no temperature sensor", this->name.c_str());
+    ROS_WARN("[%s] Has no temperature sensor", this->name.c_str());
     return -1;
   }
   return this->temperatureGES.getTemperature();

march_hardware/test/TestJoint.cpp

@@ -45,14 +45,16 @@ TEST_F(JointDeathTest, ActuateDisableActuation)
     joint.setAllowActuation(false);
     joint.setName("actuate_false");
     ASSERT_FALSE(joint.canActuate());
-    ASSERT_DEATH(joint.actuateRad(0.3), "Joint actuate_false is not allowed to actuate, yet its actuate method has been called.");
+    ASSERT_DEATH(joint.actuateRad(0.3), "Joint actuate_false is not allowed to actuate, "
+                                        "yet its actuate method has been called");
 }
 
 TEST_F(JointTest, NoActuationMode)
 {
   march4cpp::Joint joint;
   joint.setName("test_joint");
-  ASSERT_DEATH(joint.actuateRad(1), "Joint test_joint is not allowed to actuate, yet its actuate method has been called.");
+  ASSERT_DEATH(joint.actuateRad(1), "Joint test_joint is not allowed to actuate, "
+                                    "yet its actuate method has been called");
 }
 
 TEST_F(JointTest, ChangeActuationModeToUnknown)

march_hardware_builder/robots/march4.yaml

@@ -18,7 +18,7 @@ march4:
         temperatureges: {slaveIndex: 7, byteOffset: 0}
         imotioncube:
           slaveIndex: 8
-          encoder: {resolution: 12, minPositionIU: 1010, maxPositionIU: 1401, zeroPositionIU: 1211, safetyMarginRad: 0.05}
+          encoder: {resolution: 12, minPositionIU: 1035, maxPositionIU: 1438, zeroPositionIU: 1242, safetyMarginRad: 0.05}
 
     - right_hip_fe:
         actuationMode: position
@@ -54,7 +54,7 @@ march4:
         temperatureges: {slaveIndex: 9, byteOffset: 4}
         imotioncube:
           slaveIndex: 10
-          encoder: {resolution: 17, minPositionIU: 19892, maxPositionIU: 63537, zeroPositionIU: 21711, safetyMarginRad: 0.05}
+          encoder: {resolution: 17, minPositionIU: 19808, maxPositionIU: 63543, zeroPositionIU: 21755, safetyMarginRad: 0.05}
 
     - right_ankle:
         actuationMode: position

march_hardware_interface/CMakeLists.txt

@@ -7,6 +7,8 @@ find_package(catkin REQUIRED COMPONENTS
     control_toolbox
     controller_manager
     joint_limits_interface
+    joint_state_controller
+    joint_trajectory_controller
     march_description
     march_hardware
     march_hardware_builder

march_hardware_interface/config/march4/controllers.yaml

@@ -14,32 +14,32 @@ march:
       allow_partial_joints_goal: true
       # When you switch to effort actuationmode, 'position_controllers' must be substituted with 'effort_controllers' and the gains must be added
       joints:
-        - right_hip_fe
+        - left_ankle
+        - left_hip_aa
         - left_hip_fe
-        - right_knee
         - left_knee
         - right_ankle
-        - left_ankle
         - right_hip_aa
-        - left_hip_aa
+        - right_hip_fe
+        - right_knee
 #      gains: # Required because we're controlling an effort interface
-#        right_hip_fe: {p: 80, i: 0, d: 10, i_clamp: 100, publish_state: true, antiwindup: true}
+#        left_ankle: {p: 160, i: 0, d: 6, i_clamp: 100, publish_state: true, antiwindup: true}
+#        left_hip_aa: {p: 290, i: 0, d: 5, i_clamp: 100, publish_state: true, antiwindup: true}
 #        left_hip_fe: {p: 80, i: 0, d: 10, i_clamp: 100, publish_state: true, antiwindup: true}
-#        right_knee: {p: 50, i: 0, d: 10, i_clamp: 100, publish_state: true, antiwindup: true}
 #        left_knee: {p: 50, i: 0, d: 10, i_clamp: 100, publish_state: true, antiwindup: true}
 #        right_ankle: {p: 140, i: 0, d: 5, i_clamp: 100, publish_state: true, antiwindup: true}
-#        left_ankle: {p: 160, i: 0, d: 6, i_clamp: 100, publish_state: true, antiwindup: true}
 #        right_hip_aa: {p: 290, i: 0, d: 5, i_clamp: 100, publish_state: true, antiwindup: true} # TODO(BaCo) tune HAA joints
-#        left_hip_aa: {p: 290, i: 0, d: 5, i_clamp: 100, publish_state: true, antiwindup: true}
+#        right_hip_fe: {p: 80, i: 0, d: 10, i_clamp: 100, publish_state: true, antiwindup: true}
+#        right_knee: {p: 50, i: 0, d: 10, i_clamp: 100, publish_state: true, antiwindup: true}
 
       constraints:
-        right_hip_fe:
+        left_ankle:
           trajectory: 0.305
           goal: 0.305
-        left_hip_fe:
+        left_hip_aa:
           trajectory: 0.305
           goal: 0.305
-        right_knee:
+        left_hip_fe:
           trajectory: 0.305
           goal: 0.305
         left_knee:
@@ -48,12 +48,12 @@ march:
         right_ankle:
           trajectory: 0.305
           goal: 0.305
-        left_ankle:
+        right_hip_aa:
           trajectory: 0.305
           goal: 0.305
-        right_hip_aa:
+        right_hip_fe:
           trajectory: 0.305
           goal: 0.305
-        left_hip_aa:
+        right_knee:
           trajectory: 0.305
           goal: 0.305

march_hardware_interface/launch/testjoint_linear.launch

@@ -10,7 +10,7 @@
             name="record"
             type="record"
             output="screen"
-            args="-o /home/projectmarch/Documents/logs/Testjoint.bag /march/controller/trajectory/state /clock"
+            args="-q -o /home/projectmarch/Documents/logs/Testjoint.bag /march/controller/trajectory/state /clock"
     />
 
     <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>

march_hardware_interface/package.xml

@@ -13,6 +13,8 @@
   <depend>roscpp</depend>
   <depend>std_msgs</depend>
   <depend>joint_limits_interface</depend>
+  <depend>joint_trajectory_controller</depend>
+  <depend>joint_state_controller</depend>
   <depend>controller_manager</depend>
   <depend>realtime_tools</depend>
   <depend>control_toolbox</depend>

march_hardware_interface/src/march_hardware_interface.cpp

@@ -86,7 +86,10 @@ void MarchHardwareInterface::init()
   {
     SoftJointLimits soft_limits;
     getSoftJointLimits(model.getJoint(joint_names_[i]), soft_limits);
-    ROS_INFO("%s soft_limits_ (%f, %f).", joint_names_[i].c_str(), soft_limits.min_position, soft_limits.max_position);
+    ROS_DEBUG("[%s] Soft limits set to (%f, %f)",
+              joint_names_[i].c_str(),
+              soft_limits.min_position,
+              soft_limits.max_position);
     soft_limits_[i] = soft_limits;
   }
 
@@ -96,7 +99,7 @@ void MarchHardwareInterface::init()
   this->read();
   for (int i = 0; i < num_joints_; ++i)
   {
-    ROS_INFO("Joint %s: first read position: %f", joint_names_[i].c_str(), joint_position_[i]);
+    ROS_DEBUG("[%s] First read position: %f", joint_names_[i].c_str(), joint_position_[i]);
   }
 
   // Create march_pdb_state interface
@@ -129,7 +132,7 @@ void MarchHardwareInterface::init()
       {
         marchRobot.getPowerDistributionBoard()->getHighVoltage().setNetOnOff(true, netNumber);
         usleep(100000);
-        ROS_INFO_THROTTLE(1, "Waiting on high voltage for joint %s", joint_names_[i].c_str());
+        ROS_INFO_THROTTLE(1, "[%s] Waiting on high voltage", joint_names_[i].c_str());
       }
     }
   }
@@ -334,7 +337,7 @@ void MarchHardwareInterface::resetIMotionCubesUntilTheyWork()
       march4cpp::Joint joint = marchRobot.getJoint(joint_names_[i]);
       if (joint.getAngleIU() == 0)
       {
-        ROS_ERROR("Joint %s failed (encoder reset)", joint_names_[i].c_str());
+        ROS_ERROR("[%s] Failed (encoder reset)", joint_names_[i].c_str());
         encoderSetCorrectly = false;
       }
     }