diff --git a/aquadrone_sim_demos/launch/empty.launch b/aquadrone_sim_demos/launch/demo_sub_empty.launch old mode 100755 new mode 100644 similarity index 100% rename from aquadrone_sim_demos/launch/empty.launch rename to aquadrone_sim_demos/launch/demo_sub_empty.launch diff --git a/aquadrone_sim_demos/launch/demo_sub_empty_dropin.launch b/aquadrone_sim_demos/launch/demo_sub_empty_dropin.launch new file mode 100644 index 00000000..7f8e9b99 --- /dev/null +++ b/aquadrone_sim_demos/launch/demo_sub_empty_dropin.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/aquadrone_sim_demos/launch/empty_wobbly.launch b/aquadrone_sim_demos/launch/demo_sub_empty_wobbly.launch old mode 100755 new mode 100644 similarity index 100% rename from aquadrone_sim_demos/launch/empty_wobbly.launch rename to aquadrone_sim_demos/launch/demo_sub_empty_wobbly.launch diff --git a/aquadrone_sim_demos/launch/pac_depth_control_demo.launch b/aquadrone_sim_demos/launch/pac_depth_control_demo.launch new file mode 100644 index 00000000..d5dfce84 --- /dev/null +++ b/aquadrone_sim_demos/launch/pac_depth_control_demo.launch @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aquadrone_sim_worlds/launch/empty_dropin_world.launch b/aquadrone_sim_worlds/launch/empty_dropin_world.launch new file mode 100644 index 00000000..8a845002 --- /dev/null +++ b/aquadrone_sim_worlds/launch/empty_dropin_world.launch @@ -0,0 +1,56 @@ + + + + + + + + + + + + + + + + + + + + + + + meshes: + seafloor: + plane: [2000, 2000, 0.1] + pose: + position: [0, 0, -100] + north: + plane: [0.1, 2000, 100] + pose: + position: [1000, 0, -50] + south: + plane: [0.1, 2000, 100] + pose: + position: [-1000, 0, -50] + west: + plane: [2000, 0.1, 100] + pose: + position: [0, -1000, -50] + east: + plane: [2000, 0.1, 100] + pose: + position: [0, 1000, -50] + + + + + + + + + + diff --git a/aquadrone_sim_worlds/worlds/empty_underwater_dropin.world b/aquadrone_sim_worlds/worlds/empty_underwater_dropin.world new file mode 100644 index 00000000..2c6dfd8d --- /dev/null +++ b/aquadrone_sim_worlds/worlds/empty_underwater_dropin.world @@ -0,0 +1,295 @@ + + + + 0.002 + 1 + 500 + + + quick + 50 + 0.5 + 0 + + + 0 + 0.2 + 100 + 0.001 + + + + + 0.01 0.01 0.01 1 + + + 12 + + + 1 + + 0.1 0.2 0.3 1 + linear + 0.001 + 10 + 200 + + 0.7 0.7 0.7 1 + + + 56.719 + 3.51562 + EARTH_WGS84 + 0 + 0 + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.1 0.1 0.1 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.5 -1 + + + 1 + + + 0 + 0 0 -100 0 -0 0 + + + 2000 2000 0.1 + + + + + + + + 0 0 -100 0 -0 0 + + + 2000 2000 0.1 + + + 10 + + + + + + + + + + + + + + + 0 + 0 0 0 0 -0 0 + + + 2000 2000 0.1 + + + + + + + + 0 + 1000 0 -50 0 -0 0 + + + 0.1 2000 100 + + + + + + + + 0 + -1000 0 -50 0 -0 0 + + + 0.1 2000 100 + + + + + + + + 0 + 0 -1000 -50 0 -0 0 + + + 2000 0.1 100 + + + + + + + + 0 + 0 1000 -50 0 -0 0 + + + 2000 0.1 100 + + + + + + + 0 + 0 + 1 + + 0 0 0 0 -0 0 + + + hydrodynamics + + current_velocity + + 0 + 0 + 5 + 0.0 + 0.0 + + + 0 + -3.141592653589793238 + 3.141592653589793238 + 0.0 + 0.0 + + + 0 + -3.141592653589793238 + 3.141592653589793238 + 0.0 + 0.0 + + + + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + 0 + 0 + 1 + + 0.313623 -0.430269 0 0 -0 0 + + + 25 956000000 + 26 34286544 + 1581973118 932660724 + 12978 + + 0.313623 -0.430269 0 0 -0 0 + 1 1 1 + + 0.313623 -0.430269 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 10 0 -0 0 + + + + + 5 -5 2 0 0.275643 2.35619 + orbit + perspective + + + + diff --git a/path_planning/launch/real_core.launch b/path_planning/launch/real_core.launch new file mode 100644 index 00000000..55339e2f --- /dev/null +++ b/path_planning/launch/real_core.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/path_planning/nodes/dry_diagnostics_tests.py b/path_planning/nodes/dry_diagnostics_tests.py new file mode 100644 index 00000000..4e872d44 --- /dev/null +++ b/path_planning/nodes/dry_diagnostics_tests.py @@ -0,0 +1,54 @@ +#!/usr/bin/env python + +import rospy + +from path_planning.base_state import InitialState +import path_planning.diagnostic_states as Diagnostics +import path_planning.basic_control_states as BasicControls + +import path_planning.ros_modules as ROS_Modules + +class PACDepthControlTest: + def __init__(self): + + self.controls = ROS_Modules.ROSControlsModule() + self.sub_state = ROS_Modules.ROSStateEstimationModule() + self.sensors = ROS_Modules.ROSSensorDataModule() + + self.state = InitialState() + self.state.initialize(self.t, self.controls, self.sub_state, None, self.sensors) + + self.depth_state_num = 0 + + self.rate = rospy.Rate(5) + + + def switch_states(self, new_state): + print("Switching from %s to %s..." % (self.state.state_name(), new_state.state_name())) + self.state.finalize(self.t(), self.controls, self.sub_state, None, self.sensors) + + self.state = new_state + self.state.initialize(self.t(), self.controls, self.sub_state, None, self.sensors) + + def t(self): + return rospy.Time.now().to_sec() + + def run(self): + while not rospy.is_shutdown(): + self.rate.sleep() + self.state.process(self.t(), self.controls, self.sub_state, None, self.sensors) + + if isinstance(self.state, InitialState): + self.switch_states(Diagnostics.CheckThrustersState(time_each=3)) + continue + + if isinstance(self.state, Diagnostics.CheckThrustersState): + + if self.state.all_thrusters_tested(): + exit() + + +if __name__ == "__main__": + rospy.init_node("dry_diagnostics_tests") + test = PACDepthControlTest() + test.run() \ No newline at end of file diff --git a/path_planning/nodes/pac_depth_control_demo.py b/path_planning/nodes/pac_depth_control_demo.py new file mode 100644 index 00000000..5270ee98 --- /dev/null +++ b/path_planning/nodes/pac_depth_control_demo.py @@ -0,0 +1,96 @@ +#!/usr/bin/env python + +import rospy + +from path_planning.base_state import InitialState +import path_planning.diagnostic_states as Diagnostics +import path_planning.basic_control_states as BasicControls + +import path_planning.ros_modules as ROS_Modules + +class PACDepthControlTest: + def __init__(self): + + self.controls = ROS_Modules.ROSControlsModule() + self.sub_state = ROS_Modules.ROSStateEstimationModule() + self.sensors = ROS_Modules.ROSSensorDataModule() + + self.state = InitialState() + self.state.initialize(self.t, self.controls, self.sub_state, None, self.sensors) + + self.depth_state_num = 0 + + self.rate = rospy.Rate(5) + + + def switch_states(self, new_state): + print("Switching from %s to %s..." % (self.state.state_name(), new_state.state_name())) + self.state.finalize(self.t(), self.controls, self.sub_state, None, self.sensors) + + self.state = new_state + self.state.initialize(self.t(), self.controls, self.sub_state, None, self.sensors) + + def t(self): + return rospy.Time.now().to_sec() + + def run(self): + while not rospy.is_shutdown(): + self.rate.sleep() + self.state.process(self.t(), self.controls, self.sub_state, None, self.sensors) + + if isinstance(self.state, InitialState): + self.switch_states(Diagnostics.CheckThrustersState(time_each=3)) + continue + + if isinstance(self.state, Diagnostics.CheckThrustersState): + + if self.state.all_thrusters_tested(): + self.switch_states(BasicControls.WaitForSubmergenceState()) + continue + + if isinstance(self.state, BasicControls.WaitForSubmergenceState): + + if self.state.has_submerged(): + self.depth_state_num = 1 + self.switch_states(BasicControls.GoToDepthState(d_goal=1, t_required=15)) + continue + + if isinstance(self.state, BasicControls.GoToDepthState) and self.depth_state_num == 1: + + if self.state.depth_is_reached(): + self.depth_state_num = 2 + + self.controls.set_yaw_goal(3.0) # Just to test it out + + self.switch_states(BasicControls.GoToDepthState(d_goal=2, t_required=15)) + continue + + if isinstance(self.state, BasicControls.GoToDepthState) and self.depth_state_num == 2: + + if self.state.depth_is_reached(): + + self.controls.planar_move_command(Fx=2.0, Fy=0, Tz=0) # Just to test it out + rospy.sleep(3.0) + self.controls.planar_move_command(Fx=0, Fy=0, Tz=0) + + self.switch_states(BasicControls.GoToSurfaceState()) + continue + + if isinstance(self.state, BasicControls.GoToSurfaceState): + + if self.state.has_surfaced(): + self.switch_states(BasicControls.TurnOffForRetrievalState()) + continue + + if isinstance(self.state, BasicControls.TurnOffForRetrievalState): + + if self.state.has_turned_off(): + # No switch; End of test + continue + + + +if __name__ == "__main__": + rospy.init_node("pac_depth_control_test") + test = PACDepthControlTest() + test.run() \ No newline at end of file diff --git a/path_planning/src/path_planning/base_state.py b/path_planning/src/path_planning/base_state.py new file mode 100644 index 00000000..3a8fb0e3 --- /dev/null +++ b/path_planning/src/path_planning/base_state.py @@ -0,0 +1,47 @@ +import math +import cv2 +import numpy as np +import scipy.misc +from simple_pid import PID + +class BaseState(object): + def __init__(self): + pass + + def state_name(self): + return "base_state" + + def initialize(self, t, controls, sub_state, world_state, sensors): + # Set up anything that needs initializing + # Run EACH time the state is chosen as the next state + # process(...) will be called with the next available data + pass + + def finalize(self, t, controls, sub_state, world_state, sensors): + # Clean up anything necessary + pass + + def process(self, t, controls, sub_state, world_state, sensors): + # Regular tick at some rate + pass + + # Expose functions to identify when a state should exit + # Should not require any arguments to call + # Ex: has_timed_out, has_lost_track_of_[some object] + + +class InitialState(BaseState): + def __init__(self): + pass + + def state_name(self): + return "init_state" + + def initialize(self, t, controls, sub_state, world_state, sensors): + pass + + def finalize(self, t, controls, sub_state, world_state, sensors): + pass + + def process(self, t, controls, sub_state, world_state, sensors): + pass \ No newline at end of file diff --git a/path_planning/src/path_planning/basic_control_states.py b/path_planning/src/path_planning/basic_control_states.py new file mode 100644 index 00000000..faaa3b22 --- /dev/null +++ b/path_planning/src/path_planning/basic_control_states.py @@ -0,0 +1,137 @@ +import math +import cv2 +import numpy as np +import scipy.misc +from simple_pid import PID + +from base_state import BaseState + + + +class WaitForSubmergenceState(BaseState): + def __init__(self): + self.current_depth = 0 + + self.time_required = 3 + + self.time_submerged = 0 + self.last_t = 0 + + def state_name(self): + return "wait_for_submergence_state" + + def initialize(self, t, controls, sub_state, world_state, sensors): + # TODO: disable motor controls while out of water + self.last_t = t + + def finalize(self, t, controls, sub_state, world_state, sensors): + # TODO: re-enable motor controls + # TODO: reset state estimation + pass + + def process(self, t, controls, sub_state, world_state, sensors): + self.current_depth = -sub_state.get_submarine_state().position.z + dt = t - self.last_t + self.last_t = t + + if self.current_depth > 0.25: + self.time_submerged = self.time_submerged + dt + else: + self.time_submerged = 0 + + + def has_submerged(self): + return self.time_submerged > self.time_required + + +class TurnOffForRetrievalState(BaseState): + def __init__(self): + self.turned_off = False + + def state_name(self): + return "turn_off_for_retrieval_state" + + def initialize(self, t, controls, sub_state, world_state, sensors): + # TODO: disable motor controls while out of water + self.turned_off = True + + def finalize(self, t, controls, sub_state, world_state, sensors): + pass + + def process(self, t, controls, sub_state, world_state, sensors): + pass + + def has_turned_off(self): + return self.turned_off + + +class GoToDepthState(BaseState): + def __init__(self, d_goal, t_required=5): + self.depth_goal = d_goal + self.current_depth = 0 + + self.time_required = t_required + + self.time_at_depth = 0 + self.last_t = 0 + + def state_name(self): + return "go_to_depth_state" + + def initialize(self, t, controls, sub_state, world_state, sensors): + controls.set_depth_goal(self.depth_goal) + self.last_t = t + + def finalize(self, t, controls, sub_state, world_state, sensors): + pass + + def process(self, t, controls, sub_state, world_state, sensors): + self.current_depth = -sub_state.get_submarine_state().position.z + dt = t - self.last_t + self.last_t = t + + depth_err = abs(self.current_depth - self.depth_goal) + + if depth_err < 0.25: + self.time_at_depth = self.time_at_depth + dt + else: + self.time_at_depth = 0 + + print("Depth err (abs): %f" % depth_err) + + def depth_is_reached(self): + return self.time_at_depth > self.time_required + + +class GoToSurfaceState(BaseState): + def __init__(self): + self.depth_goal = 0 + self.current_depth = 0 + + self.time_required = 2 + + self.time_at_surface = 0 + self.last_t = 0 + + def state_name(self): + return "go_to_surface" + + def initialize(self, t, controls, sub_state, world_state, sensors): + controls.set_depth_goal(self.depth_goal) + self.last_t = t + + def finalize(self, t, controls, sub_state, world_state, sensors): + pass + + def process(self, t, controls, sub_state, world_state, sensors): + self.current_depth = -sub_state.get_submarine_state().position.z + dt = t - self.last_t + self.last_t = t + + if self.current_depth < 0.5: + self.time_at_surface = self.time_at_surface + dt + else: + self.time_at_surface = 0 + + def has_surfaced(self): + return self.time_at_surface > self.time_required diff --git a/path_planning/src/path_planning/diagnostic_states.py b/path_planning/src/path_planning/diagnostic_states.py new file mode 100644 index 00000000..2e3c67fe --- /dev/null +++ b/path_planning/src/path_planning/diagnostic_states.py @@ -0,0 +1,56 @@ +import math +import cv2 +import numpy as np +import scipy.misc +from simple_pid import PID + +from base_state import BaseState +from aquadrone_msgs.msg import MotorControls + + +class CheckThrustersState(BaseState): + def __init__(self, time_each=1): + self.current_thruster = 0 + self.thruster_mode = -1 + + self.time_each = time_each + self.time_mode_started = 0 + + def state_name(self): + return "check_thrusters_state" + + def initialize(self, t, controls, sub_state, world_state, sensors): + # TODO: disable automatic control system + self.time_mode_started = t + pass + + def finalize(self, t, controls, sub_state, world_state, sensors): + # TODO: re-enable automatic control system + pass + + def process(self, t, controls, sub_state, world_state, sensors): + # Go through all 8 thrusters once + if self.current_thruster > 7: + return + + # Alternate between -1, 0, 1 for 1 second each + dt = t - self.time_mode_started + if dt > self.time_each: + self.time_mode_started = t + + if self.thruster_mode == 1: + self.thruster_mode = -1 + self.current_thruster = self.current_thruster + 1 + return + else: + self.thruster_mode = self.thruster_mode + 1 + + thrusts = [0 for i in range(0, 8)] + thrusts[self.current_thruster] = self.thruster_mode + + controls.raw_thrust_command(thrusts) + + + + def all_thrusters_tested(self): + return self.current_thruster > 7 diff --git a/path_planning/src/path_planning/pole_finder_demo_states.py b/path_planning/src/path_planning/pole_finder_demo_states.py index a69cc1e5..b465fc40 100644 --- a/path_planning/src/path_planning/pole_finder_demo_states.py +++ b/path_planning/src/path_planning/pole_finder_demo_states.py @@ -4,47 +4,7 @@ import scipy.misc from simple_pid import PID -class BaseState(object): - def __init__(self): - pass - - def state_name(self): - return "base_state" - - def initialize(self, t, controls, sub_state, world_state, sensors): - # Set up anything that needs initializing - # Run EACH time the state is chosen as the next state - # process(...) will be called with the next available data - pass - - def finalize(self, t, controls, sub_state, world_state, sensors): - # Clean up anything necessary - pass - - def process(self, t, controls, sub_state, world_state, sensors): - # Regular tick at some rate - pass - - # Expose functions to identify when a state should exit - # Ex: has_timed_out, has_lost_track_of_[some object] - - -class InitialState(object): - def __init__(self): - pass - - def state_name(self): - return "init_state" - - def initialize(self, t, controls, sub_state, world_state, sensors): - pass - - def finalize(self, t, controls, sub_state, world_state, sensors): - pass - - def process(self, t, controls, sub_state, world_state, sensors): - pass - +from base_state import BaseState class GoToDepthState(BaseState): def __init__(self, d): @@ -65,7 +25,7 @@ def finalize(self, t, controls, sub_state, world_state, sensors): pass def process(self, t, controls, sub_state, world_state, sensors): - self.current_depth = -sub_state.get_submarinne_state().position.z + self.current_depth = -sub_state.get_submarine_state().position.z dt = t - self.last_t self.last_t = t @@ -106,7 +66,7 @@ def process(self, t, controls, sub_state, world_state, sensors): dt = t - self.last_t self.last_t = t - yaw = sub_state.get_submarinne_state().orientation_rpy.z + yaw = sub_state.get_submarine_state().orientation_rpy.z controls.set_yaw_goal(yaw + dt * 2*math.pi * 0.05) controls.planar_move_command(Fy=0, Fx=0.05) diff --git a/path_planning/src/path_planning/ros_modules.py b/path_planning/src/path_planning/ros_modules.py index 79d72409..dcfbbc8a 100644 --- a/path_planning/src/path_planning/ros_modules.py +++ b/path_planning/src/path_planning/ros_modules.py @@ -10,6 +10,7 @@ import data_structures as DS from aquadrone_msgs.msg import SubState +from aquadrone_msgs.msg import MotorControls class ROSControlsModule: @@ -17,6 +18,7 @@ def __init__(self): self.depth_pub = rospy.Publisher("/depth_control/goal_depth", Float64, queue_size=1) self.yaw_pub = rospy.Publisher("orientation_target", Vector3, queue_size=1) self.planar_move_pub = rospy.Publisher("/movement_command", Wrench, queue_size=1) + self.raw_thruster_pub = rospy.Publisher("motor_command", MotorControls, queue_size=1) def set_depth_goal(self, d): self.depth_pub.publish(d) @@ -38,6 +40,11 @@ def planar_move_command(self, Fx=0, Fy=0, Tz=0): w.torque.z = Tz self.planar_move_pub.publish(w) + def raw_thrust_command(self, thrusts): + msg = MotorControls() + msg.motorThrusts = thrusts + self.raw_thruster_pub.publish(msg) + class ROSStateEstimationModule: def __init__(self): @@ -47,7 +54,7 @@ def __init__(self): def sub_state_callback(self, msg): self.sub_state = msg - def get_submarinne_state(self): + def get_submarine_state(self): def make_vector(msg): out = DS.Vector(0, 0, 0) diff --git a/thruster_control/nodes/real_thruster_controller.py b/thruster_control/nodes/real_thruster_controller.py old mode 100644 new mode 100755 index 990d1461..189188c2 --- a/thruster_control/nodes/real_thruster_controller.py +++ b/thruster_control/nodes/real_thruster_controller.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 import rospy @@ -45,4 +45,4 @@ stc = RealThrusterController(config, mcc, interface, specs) stc.run() - \ No newline at end of file + diff --git a/thruster_control/nodes/real_thruster_controller_manual.py b/thruster_control/nodes/real_thruster_controller_manual.py new file mode 100755 index 00000000..cf65f616 --- /dev/null +++ b/thruster_control/nodes/real_thruster_controller_manual.py @@ -0,0 +1,52 @@ +#!/usr/bin/env python3 + +import rospy + +from thruster_control.thruster_configurations import V1Configuration, V2Configuration, V28Configuration +from thruster_control.movement_command_collector import MovementCommandCollector +from thruster_control.thruster_interfaces.v28_interface import V28ThrusterInterface +from thruster_control.thruster_types import BlueRoboticsT100 +from thruster_control.thruster_control_manager import RealThrusterController + + +if __name__ == "__main__": + rospy.init_node('sim_thruster_controller') + + mcc = MovementCommandCollector() + + # Assume V28 by default + model = rospy.get_param("model", "v28") + num = 8 + + if model == "v1": + config = V1Configuration() + num = 6 + elif model == "v2": + config = V1Configuration() + num = 6 + elif model == "v28": + config = V28Configuration() + num = 8 + else: + print("Error: unknown model for controls: %s" % str(model)) + exit() + + config.initialize() + + interface = V28ThrusterInterface() + + th_spec = BlueRoboticsT100(interface.get_frequency()) + th_spec.initialize() + specs = [th_spec for i in range(0, 8)] + + + interface.init_gpio() + + interface.init_thrusters(specs) + + stc = RealThrusterController(config, mcc, interface, specs) + #stc.run() + print("Running Thruster Control") + while not rospy.is_shutdown(): + rospy.sleep(1.0) + diff --git a/thruster_control/src/thruster_control/depth_pid_controller.py b/thruster_control/src/thruster_control/depth_pid_controller.py index 2772b9cf..cae7bf2e 100755 --- a/thruster_control/src/thruster_control/depth_pid_controller.py +++ b/thruster_control/src/thruster_control/depth_pid_controller.py @@ -13,7 +13,7 @@ class DepthPIDController: - def __init__(self, Kp=1, Kd=1, Ki=0): + def __init__(self, Kp=5, Kd=10, Ki=0.1): self.depth = 0 self.Kp = Kp diff --git a/thruster_control/src/thruster_control/thruster_configurations.py b/thruster_control/src/thruster_control/thruster_configurations.py index 9f3da085..820622f6 100644 --- a/thruster_control/src/thruster_control/thruster_configurations.py +++ b/thruster_control/src/thruster_control/thruster_configurations.py @@ -1,11 +1,55 @@ import numpy as np import math -import geometry_helper as gh +#import geometry_helper as gh + +from geometry_msgs.msg import Wrench + +import aquadrone_math_utils.orientation_math as OH + +import thruster_control.geometry_helper as gh a2 = math.pi / 2.0 a4 = math.pi / 4.0 in2m = 2.54 * 0.01 +''' +class GeometryHelper: + + + def vector_to_wrench(v): + w = Wrench() + w.force.x = v[0] + w.force.y = v[1] + w.force.z = v[2] + w.torque.x = v[0] + w.torque.y = v[1] + w.torque.z = v[2] + return w + + def get_thruster_wrench_vector(x, y, z, roll, pitch, yaw): + # 6d vector, with force on top, moment on bottom + force = get_thruster_force(roll, pitch, yaw) + + # Moment is r x f + offset = np.array([x, y, z]) + offset.shape = (3,1) + moment = np.cross(offset, force, axis=0) + + return np.vstack((force, moment)) + + def get_thruster_force(r, p, y): + return np.dot(OH.RPY_Matrix(r,p,y), thruster_force_dir_vec()) + + def thruster_force_dir_vec(): + # Constant direction that a thruster applies force in + # Local to the thruster's own body + v = np.array([1, 0, 0]) + v.shape = (3,1) + return v +''' + +#gh = GeometryHelper() + class ThrusterConfiguration: def __init__(self): @@ -155,4 +199,4 @@ def wrench_to_thrusts(self, wrench_vec): return thrusts def get_num_thrusters(self): - return 8 \ No newline at end of file + return 8 diff --git a/thruster_control/src/thruster_control/thruster_interfaces/v28_interface.py b/thruster_control/src/thruster_control/thruster_interfaces/v28_interface.py index 2460528d..36105856 100644 --- a/thruster_control/src/thruster_control/thruster_interfaces/v28_interface.py +++ b/thruster_control/src/thruster_control/thruster_interfaces/v28_interface.py @@ -29,7 +29,7 @@ def init_gpio(self): def init_thrusters(self, thruster_specs): print("Init Thruster PWM") - for i in range(0, 6): + for i in range(0, 8): self.pwm[i] = self.pca.channels[self.motorPins[i]] self.pwm[i].duty_cycle = thruster_specs[i].get_zero_signal() #self.gpio.setup(self.motorPins[i], self.gpio.OUT)