rospy.init_node('fly_labirint', anonymous=True)
if toFly:
print "takeoff"
cf1 = crazyflie.Crazyflie(cf_names[0],
'/vicon/' + cf_names[0] + '/' + cf_names[0])
cf1.setParam("commander/enHighLevel", 1)
cf1.takeoff(targetHeight=TAKEOFFHEIGHT, duration=2.0)
time.sleep(TakeoffTime)
# Objects init
obstacles = np.array([])
for i in range(len(obstacle_names)):
obstacles = np.append(
obstacles, swarmlib.Obstacle(obstacle_names[i], i, R_obstacles))
drone1 = swarmlib.Drone(cf_names[0], obstacles)
drone2 = swarmlib.Drone(cf_names[1], obstacles)
# drone3 = swarmlib.Drone(cf_names[2], obstacles)
drone1.start = obstacles[0].position() + np.array([-1.0, 0, TAKEOFFHEIGHT])
drone1.goal = obstacles[0].position() + np.array([0.5, 0, TAKEOFFHEIGHT])
xs, ys, zs = drone1.start
xg, yg, zg = drone1.goal
N_samples = ViconRate * TimeFlightSec
# [X] [Y] [Z]
drone1.traj = np.array([
np.linspace(xs, xg, N_samples),
np.linspace(ys, yg, N_samples),
np.linspace(zs, zg, N_samples)
]).T
drone2.traj = np.array([
# lp1 = swarmlib.Mocap_object(lp1_name)
# lp2 = swarmlib.Mocap_object(lp2_name)
# lp_list = [lp1, lp2]
# cf_names = ['cf1', 'cf2', 'cf3', 'cf4']
# cf_names = ['cf1', 'cf2', 'cf3']
cf_names = ['cf1', 'cf2']
# cf_names = ['cf1']
lp_names = []
# lp_names = ['lp2', 'lp1', 'lp3', 'lp4']
# lp_names = ['lp2', 'lp1']
# lp_names = ['lp2']
drone_list = []
for name in cf_names:
drone = swarmlib.Drone(name)
drone_list.append(drone)
lp_list = []
for lp_name in lp_names:
lp_list.append(swarmlib.Mocap_object(lp_name))
# cf1_name = 'cf1'
# cf2_name = 'cf2'
# cf3_name = 'cf3'
# cf4_name = 'cf4'
# cf1 = crazyflie.Crazyflie(cf1_name, '/vicon/'+cf1_name+'/'+cf1_name)
# cf2 = crazyflie.Crazyflie(cf2_name, '/vicon/'+cf2_name+'/'+cf2_name)
# cf3 = crazyflie.Crazyflie(cf3_name, '/vicon/'+cf3_name+'/'+cf3_name)
# cf4 = crazyflie.Crazyflie(cf4_name, '/vicon/'+cf4_name+'/'+cf4_name)
# cf_list = [cf1, cf2, cf3, cf4]
# cf_list = [cf1, cf2, cf3]
subject_name = "Evgeny"
# Variables #########################
initialized = False
imp_pose_prev = np.array([0, 0, 0])
imp_vel_prev = np.array([0, 0, 0])
imp_time_prev = time.time()
# SetUp
if tacile_glove_on:
swarmlib.startXbee()
# START ##########################################################################################################
rospy.init_node('follow_multiple', anonymous=True)
# Objects init TODO: swarmlib.swarm_manager(drone_name_list)
human = swarmlib.Mocap_object(human_name)
drone1 = swarmlib.Drone(cf1_name, leader=True)
drone2 = swarmlib.Drone(cf2_name)
drone3 = swarmlib.Drone(cf3_name)
swarm = [drone1, drone2, drone3]
for drone in swarm:
print "drone_name: ", drone.name
obstacle_objects_list = []
for obstacle in obstacle_name_list:
obstacle_objects_list.append(swarmlib.Mocap_object(obstacle))
if toFly:
print "takeoff"
cf1 = crazyflie.Crazyflie(cf1_name, '/vicon/' + cf1_name + '/' + cf1_name)
# cf2 = crazyflie.Crazyflie(cf2_name, '/vicon/'+cf2_name+'/'+cf2_name)
# cf3 = crazyflie.Crazyflie(cf3_name, '/vicon/'+cf3_name+'/'+cf3_name)
# cf_list = [cf1, cf2, cf3]
initialized = False
prev_pattern_time = time.time()
imp_pose_prev = np.array([0, 0, 0])
imp_vel_prev = np.array([0, 0, 0])
imp_time_prev = time.time()
updated_1 = 0
# SetUp
if tacile_glove_on:
swarmlib.startXbee()
if __name__ == '__main__':
rospy.init_node('convexhull', anonymous=True)
# Objects init
drone1 = swarmlib.Drone(cf_names[0], leader=True)
drone2 = swarmlib.Drone(cf_names[1])
drone3 = swarmlib.Drone(cf_names[2])
human = swarmlib.Mocap_object(human_name)
obstacle = np.array([])
points = np.zeros((len(obstacle_names), 2))
rate = rospy.Rate(60)
while not rospy.is_shutdown():
# TODO: update current position of all objects every loop
# print '\nhuman_pose', human.position()
# OBSTACLEs
# TODO: make it to look good, by Ruslan
# for i in range(len(obstacle)):
cf2 = crazyflie.Crazyflie(cf_names[1],
'/vicon/' + cf_names[1] + '/' + cf_names[1])
cf2.setParam("commander/enHighLevel", 1)
cf2.takeoff(targetHeight=TAKEOFFHEIGHT, duration=2.0)
# cf3 = crazyflie.Crazyflie(cf_names[2], '/vicon/'+cf_names[2]+'/'+cf_names[2])
# cf3.setParam("commander/enHighLevel", 1)
# cf3.takeoff(targetHeight = TAKEOFFHEIGHT, duration = 2.0)
#time to takeoff and select position for human
time.sleep(TakeoffTime)
# Objects init
obstacle = np.array([])
for i in range(len(obstacle_names)):
obstacle = np.append(
obstacle, swarmlib.Obstacle(obstacle_names[i], i, R_obstacles))
drone1 = swarmlib.Drone(cf_names[0], obstacle, leader=True)
drone2 = swarmlib.Drone(cf_names[1], obstacle)
drone3 = swarmlib.Drone(cf_names[2], obstacle)
human = swarmlib.Mocap_object(human_name)
rate = rospy.Rate(60)
while not rospy.is_shutdown():
for i in range(len(obstacle)):
obstacle[i].publish_position()
if human_imp:
# HUMAN IMPEDANCE
hum_vel = swarmlib.hum_vel(human_pose)
imp_pose, imp_vel, imp_time_prev = swarmlib.impedance_human(
hum_vel, imp_pose_prev, imp_vel_prev, imp_time_prev)
imp_pose_prev = imp_pose
else:
obstacles_poses = np.array([[-1, 1], [1.0, 0.5], [-1.0, 0.3], [0.1, 0.1],
[1, -0.3], [-0.8,
-0.8]]) # 2D - coordinates [m]
obstacles_goal_poses = np.array([[-0, 0], [0.0, 0.0], [0.0,
0.0], [0.0, 0.0],
[0, 0], [0.0,
0.0]]) # for moving obstacles
if __name__ == '__main__':
rospy.init_node('gradient_interactive', anonymous=True)
# Objects inititalization
human = swarmlib.Mocap_object(human_name)
swarm = []
for name in cf_names:
swarm.append(swarmlib.Drone(name))
swarm[0].leader = True
# Obstacles init
obstacles_poses = []
for name in obstacles_names:
obstacles_poses.append(swarmlib.Mocap_object(name).position()[:2])
obstacles_poses = np.array(obstacles_poses)
obstacles_array = []
for ind in range(len(obstacles_poses)):
obstacles_array.append(swarmlib.Obstacle('obstacle_%d' % ind))
obstacles_array[-1].pose = obstacles_poses[ind].tolist() + [
TakeoffHeight
]
obstacles_array[-1].R = R_obstacles
ind += 1
imp_time_prev = time.time()
if __name__ == '__main__':
rospy.init_node('impedance_landing', anonymous=True)
if toFly:
print "takeoff"
cf = crazyflie.Crazyflie(cf_name, '/vicon/'+cf_name+'/'+cf_name)
cf.setParam("commander/enHighLevel", 1)
cf.setParam("stabilizer/estimator", 2) # Use EKF
cf.setParam("stabilizer/controller", 2) # Use mellinger controller
cf.takeoff(targetHeight=TakeoffHeight, duration=TakeoffTime)
time.sleep(TakeoffTime)
# Objects init
drone1 = swarmlib.Drone(cf_name)
start_landing_point = drone1.position()[:2].tolist() + [TakeoffHeight]
drone1.sp = start_landing_point if toFly else np.array([0,0,TakeoffHeight])
rate = rospy.Rate(60)
print "Landing..."
z_array = []; vz_array = []
while not rospy.is_shutdown():
drone1.sp[2] -= 0.01
""" impedance model """
####################################################
drone_vel = swarmlib.velocity(drone1.sp)
if land_imp:
""" ipedance terms calculation """
imp_pose, imp_vel, imp_time_prev = velocity_impedance(drone_vel, imp_pose_prev, imp_vel_prev, imp_time_prev)