Commit 580b7e4e authored by Max's avatar Max
Browse files

Update controller.

parent 6334256a
......@@ -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
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment