added straightline guidance control
This commit is contained in:
parent
5836e59301
commit
f93f526f37
|
|
@ -0,0 +1,54 @@
|
||||||
|
<?xml version="1.0"?>
|
||||||
|
<launch>
|
||||||
|
<arg name="model" default="headless_spiri_with_tether"/>
|
||||||
|
<arg name='test' default="none"/>
|
||||||
|
<arg name='turtle' default="none"/>
|
||||||
|
<group ns="sim">
|
||||||
|
<rosparam file="$(find oscillation_ctrl)/config/noCtrl_param.yaml" />
|
||||||
|
</group>
|
||||||
|
|
||||||
|
|
||||||
|
<node
|
||||||
|
pkg="oscillation_ctrl"
|
||||||
|
type="LinkState.py"
|
||||||
|
name="LinkStates"
|
||||||
|
launch-prefix="xterm -e"
|
||||||
|
/>
|
||||||
|
<node
|
||||||
|
pkg="oscillation_ctrl"
|
||||||
|
type="straight_line.py"
|
||||||
|
name="circle_acceleration"
|
||||||
|
|
||||||
|
/>
|
||||||
|
|
||||||
|
|
||||||
|
<node
|
||||||
|
pkg="oscillation_ctrl"
|
||||||
|
type="wpoint_tracker.py"
|
||||||
|
name="waypoints_server"
|
||||||
|
launch-prefix="xterm -e"
|
||||||
|
/>
|
||||||
|
|
||||||
|
<group if="$(eval test != 'none')">
|
||||||
|
<node
|
||||||
|
pkg="oscillation_ctrl"
|
||||||
|
type="perform_test.py"
|
||||||
|
name="test_node"
|
||||||
|
launch-prefix="xterm -e"
|
||||||
|
/>
|
||||||
|
</group>
|
||||||
|
<group if="$(eval turtle != 'none')">
|
||||||
|
<node
|
||||||
|
pkg="oscillation_ctrl"
|
||||||
|
type="turtle_test.py"
|
||||||
|
name="turtle_test"
|
||||||
|
/>
|
||||||
|
<node
|
||||||
|
pkg="turtlesim"
|
||||||
|
type="turtlesim_node"
|
||||||
|
name="turtlesim_node"
|
||||||
|
/>
|
||||||
|
</group>
|
||||||
|
<!-- PX4 LAUNCH -->
|
||||||
|
<include file="$(find px4)/launch/$(arg model).launch"/>
|
||||||
|
</launch>
|
||||||
|
|
@ -0,0 +1,158 @@
|
||||||
|
#!/usr/bin/env python2.7
|
||||||
|
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
import rospy
|
||||||
|
from geometry_msgs.msg import Point
|
||||||
|
from geometry_msgs.msg import PoseStamped
|
||||||
|
from geometry_msgs.msg import TwistStamped
|
||||||
|
from geometry_msgs.msg import Vector3Stamped
|
||||||
|
from mavros_msgs.msg import State
|
||||||
|
from mavros_msgs.srv import SetMode, SetModeRequest, CommandBool
|
||||||
|
#callback to update curent pose
|
||||||
|
def pose_callback(data):
|
||||||
|
global pose
|
||||||
|
pose = np.array([data.pose.position.x, data.pose.position.y, data.pose.position.z])
|
||||||
|
|
||||||
|
def velocity_callback(data):
|
||||||
|
global velocity
|
||||||
|
velocity = np.array([data.twist.linear.x,data.twist.linear.y,data.twist.linear.z])
|
||||||
|
|
||||||
|
def state_callback(data):
|
||||||
|
global state
|
||||||
|
state=data
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
G=9.8
|
||||||
|
def convert_to_cylindrical(point):
|
||||||
|
recentered = point-start_point
|
||||||
|
y=np.dot(recentered, unit_vector)
|
||||||
|
circle_portion = recentered-y*unit_vector
|
||||||
|
r=np.linalg.norm(circle_portion)
|
||||||
|
horizontal_position=np.dot(circle_portion, unit_vector_2)
|
||||||
|
theta=np.arccos(np.linalg.norm(horizontal_position)/r)
|
||||||
|
if(np.dot(circle_portion,unit_vector_3)<0):
|
||||||
|
theta=2*np.pi-theta
|
||||||
|
return y, r, theta, circle_portion
|
||||||
|
|
||||||
|
def get_error_acceleration(circle_position, r, velocity):
|
||||||
|
velocity_unit_2 = np.dot(velocity, unit_vector_2)
|
||||||
|
velocity_unit_3 = np.dot(velocity, unit_vector_3)
|
||||||
|
circle_unit_2 = np.dot(circle_position, unit_vector_2)
|
||||||
|
circle_unit_3 = np.dot(circle_position, unit_vector_3)
|
||||||
|
accel = -1*(b3*velocity_unit_2+k*circle_unit_2)*unit_vector_2
|
||||||
|
accel = accel - (b*velocity_unit_3+k*circle_unit_3)*unit_vector_3
|
||||||
|
#velocity_in_r = np.dot(circle_position, velocity)/r
|
||||||
|
|
||||||
|
#accel = -1*(b*velocity_in_r*circle_position/r + k*circle_position)/m
|
||||||
|
return accel
|
||||||
|
|
||||||
|
def get_forward_acceleration(y, velocity):
|
||||||
|
forward_velocity=np.dot(velocity,unit_vector)
|
||||||
|
forward_acceleration = 2*(9.5-forward_velocity)
|
||||||
|
forward_acceleration_2 = -1*(b2*forward_velocity+k2*(y-300))/m
|
||||||
|
return min(forward_acceleration, forward_acceleration_2)*unit_vector
|
||||||
|
|
||||||
|
return forward_acceleration
|
||||||
|
|
||||||
|
|
||||||
|
def accel_publisher():
|
||||||
|
|
||||||
|
pub = rospy.Publisher('mavros/setpoint_accel/accel',Vector3Stamped, queue_size=10)
|
||||||
|
|
||||||
|
setpointpub = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10)
|
||||||
|
rospy.wait_for_service('mavros/set_mode')
|
||||||
|
rospy.wait_for_service('mavros/cmd/arming')
|
||||||
|
arming_srv = rospy.ServiceProxy('mavros/cmd/arming',CommandBool)
|
||||||
|
set_mode_srv = rospy.ServiceProxy('mavros/set_mode', SetMode)
|
||||||
|
|
||||||
|
|
||||||
|
rate = rospy.Rate(20)
|
||||||
|
while not rospy.is_shutdown() and not state.connected:
|
||||||
|
|
||||||
|
rate.sleep()
|
||||||
|
rospy.loginfo('connected')
|
||||||
|
initial_pose=PoseStamped()
|
||||||
|
initial_pose.pose.position.x = 0
|
||||||
|
initial_pose.pose.position.y = 0
|
||||||
|
initial_pose.pose.position.z = 12
|
||||||
|
for i in range(100):
|
||||||
|
setpointpub.publish(initial_pose)
|
||||||
|
rate.sleep()
|
||||||
|
last_request = rospy.get_rostime().secs
|
||||||
|
offb_set_mode = SetModeRequest()
|
||||||
|
offb_set_mode.custom_mode="OFFBOARD"
|
||||||
|
arm = True
|
||||||
|
|
||||||
|
while not rospy.is_shutdown() and (pose[2]<9.5 or velocity[0]>1 or velocity[1]>1):
|
||||||
|
if(state.mode != "OFFBOARD" and (rospy.get_rostime().secs-last_request>5)):
|
||||||
|
rospy.loginfo('setting mode')
|
||||||
|
res=set_mode_srv(offb_set_mode)
|
||||||
|
|
||||||
|
if(not res.mode_sent):
|
||||||
|
rospy.loginfo('Mode not sent')
|
||||||
|
else:
|
||||||
|
rospy.loginfo('Mode sent')
|
||||||
|
last_request=rospy.get_rostime().secs
|
||||||
|
else:
|
||||||
|
if (not state.armed and (rospy.get_rostime().secs-last_request>3)):
|
||||||
|
rospy.loginfo('arming')
|
||||||
|
arming_srv(arm)
|
||||||
|
last_request=rospy.get_rostime().secs
|
||||||
|
initial_pose.pose.position.x =pose[0]
|
||||||
|
initial_pose.pose.position.y=pose[1]
|
||||||
|
setpointpub.publish(initial_pose)
|
||||||
|
global start_point
|
||||||
|
start_point=pose
|
||||||
|
rospy.logwarn('x: %f y: %f z: %f' %(pose[0], pose[1], pose[2]))
|
||||||
|
while not rospy.is_shutdown():
|
||||||
|
y,r,theta,circle_position=convert_to_cylindrical(pose)
|
||||||
|
forward_accel = get_forward_acceleration(y,velocity)
|
||||||
|
error_accel = get_error_acceleration(circle_position,r,velocity)
|
||||||
|
#accel = get_error_acceleration(circle_position, r, velocity) + get_forward_acceleration(y,velocity)
|
||||||
|
#accel = get_forward_acceleration(y,velocity)
|
||||||
|
#if(np.linalg.norm(velocity)>8) and (np.linalg.norm(forward_accel)>0):
|
||||||
|
# forward_accel = -0*unit_vector
|
||||||
|
accel = forward_accel+error_accel
|
||||||
|
magnitude_accel=np.linalg.norm(accel)
|
||||||
|
if magnitude_accel>2*G:
|
||||||
|
accel=accel/magnitude_accel*2*G
|
||||||
|
#if np.linalg.norm(velocity)>8:
|
||||||
|
# accel = [0,0,0]
|
||||||
|
#accel+=get_error_acceleration(circle_position,r,velocity)
|
||||||
|
|
||||||
|
to_publish=Vector3Stamped()
|
||||||
|
to_publish.vector.x = accel[0]
|
||||||
|
to_publish.vector.y=accel[1]
|
||||||
|
to_publish.vector.z=accel[2]+2
|
||||||
|
pub.publish(to_publish)
|
||||||
|
rate.sleep()
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
pose = None
|
||||||
|
velocity = None
|
||||||
|
k = 2
|
||||||
|
|
||||||
|
k2 = 0.5
|
||||||
|
|
||||||
|
m=1.5
|
||||||
|
b3 = 1*np.sqrt(m*k)
|
||||||
|
b = 0.4*np.sqrt(m*k)
|
||||||
|
b2 = 2*np.sqrt(m*k2)
|
||||||
|
unit_vector = np.array([1,0,0])
|
||||||
|
unit_vector_2 = np.array([0,1,0])
|
||||||
|
unit_vector_3 = np.array([0,0,1])
|
||||||
|
unit_vector = unit_vector/np.linalg.norm(unit_vector)
|
||||||
|
unit_vector_2 = unit_vector_2/np.linalg.norm(unit_vector_2)
|
||||||
|
|
||||||
|
state = State(connected=False,armed=False,guided=False,mode="MANUAL",system_status=0)
|
||||||
|
rospy.init_node('circle_accelerations',anonymous=True)
|
||||||
|
rospy.Subscriber('mavros/local_position/velocity_body', TwistStamped, velocity_callback)
|
||||||
|
rospy.Subscriber('mavros/local_position/pose', PoseStamped, pose_callback)
|
||||||
|
rospy.Subscriber('mavros/state', State, state_callback)
|
||||||
|
try:
|
||||||
|
accel_publisher()
|
||||||
|
except rospy.ROSInterruptException:
|
||||||
|
pass
|
||||||
|
|
||||||
Loading…
Reference in New Issue