Update controller.

electra_tether_controller/src/electra_tether_controller/tipping_slipping_tether_controller.py

@@ -19,6 +19,9 @@ class TippingSlippingTetherControl:
     
         self.command_publisher = rospy.Publisher("/electra/force_controller/command", std_msgs.msg.Float64, queue_size=10)
 
+        self.height_tether = rospy.get_param("/height_tether")
+        rospy.logwarn(f"The tether height is: {self.height_tether}")
+
         timer_period = rospy.Duration(0.04)
         self.timer = rospy.Timer(timer_period, self.timer_callback)
 
@@ -26,9 +29,6 @@ class TippingSlippingTetherControl:
         beta = self.arm2gravity_beta_subscriber.message.data
         alpha = self.inclination_alpha_subscriber.message.data
         
-        # alpha = 0.41
-        # beta = 0.7
-
         F_gravity = 14.25
         mu = 0.25
 
@@ -38,24 +38,25 @@ class TippingSlippingTetherControl:
         except:
             rospy.logwarn_throttle(1,"Can't prevent slipping.")
             F_tether_min = F_tether_for_F_inplane_min 
-        # rospy.loginfo_throttle(1, f"F_tether_min: {F_tether_min}")
+        rospy.loginfo_throttle(1, f"F_tether_inplane_min: {F_tether_min}")
 
-        robot_tip = 0.3
+        robot_tip = 0.35; #0.3
         height_gravity = 0.14
-        height_tether = 0.4
 
         def F_tip_num_phi(phi):
-            f1 = F_gravity * (robot_tip*math.cos(alpha)+2*height_gravity*math.sin(alpha)*math.cos(phi))/ (2*height_tether*math.cos(beta-phi))
-            f2 = F_gravity * (-robot_tip*math.cos(alpha)+2*height_gravity*math.sin(alpha)*math.cos(phi))/ (2*height_tether*math.cos(beta-phi))
+            f1 = F_gravity * (robot_tip*math.cos(alpha)+2*height_gravity*math.sin(alpha)*math.cos(phi))/ (2*self.height_tether*math.cos(beta-phi))
+            f2 = F_gravity * (-robot_tip*math.cos(alpha)+2*height_gravity*math.sin(alpha)*math.cos(phi))/ (2*self.height_tether*math.cos(beta-phi))
             return np.maximum(np.maximum(f1, f2),0)
         phi_bounds = ((-np.pi,np.pi),)
         F_tip_max = minimize_scalar(F_tip_num_phi, bounds=phi_bounds).fun       
-        # rospy.loginfo_throttle(1, f"F_tip_max: {F_tip_max}")
+        rospy.loginfo_throttle(1, f"F_tip_max: {F_tip_max}")
+
+        slip_margin = 0.7
 
-        F_ref = (F_tip_max + F_tether_min)/2.0
+        F_ref = (F_tip_max + slip_margin + F_tether_min)/2.0
         F_ref = np.minimum(F_ref, F_tether_for_F_inplane_min)
         F_ref = np.minimum(F_ref, F_tip_max)
-        # rospy.loginfo_throttle(1, f"F_ref: {F_ref}")
+        rospy.loginfo_throttle(1, f"F_ref: {F_ref}")
 
         command_msg = std_msgs.msg.Float64()
         command_msg.data = F_ref