def simulate(structure, trajectory_function, t_step=0.005, tmax=5, loc=[1., .0, .0]):
"""
:param structure:
:param trajectory_function:
:param t_step: time step
:param tmax: maximum time
"""
state_vector = init_state(loc, 0)
state_log = []
forces_log = []
# For every time step
for t in np.arange(0, tmax, t_step):
# print t, state_vector
##### Trajectory
desired_state = trajectory_function(t % 10, tmax)
# Position controller for a single robot
[thrust_newtons, roll, pitch, yaw] = position_controller(state_vector, desired_state)
F, M = attitude_controller((thrust_newtons, roll, pitch, yaw), state_vector)
# Structure control
F_structure, M_structure, rotor_forces = modquad_torque_control(F, M, structure)
forces_log.append(rotor_forces)
# Simulate
new_state_vector = simulation_step(structure, state_vector, F_structure, M_structure, t_step)
if new_state_vector is None:
break
state_vector = new_state_vector
state_log.append(np.copy(state_vector))
print "end simulation"
state_log = np.array(state_log)
# Show trajectory x-y
plt.plot(state_log[:, 0], state_log[:, 1])
plt.grid()
plt.show()
# sum of the squared forces
plt.plot(np.sum(np.array(forces_log) ** 2, axis=1))
plt.grid()
plt.show()
print "total integral=", np.sum(np.array(forces_log) ** 2) * t_step
def simulate():
global dislocation_srv
rospy.init_node('modrotor_simulator', anonymous=True)
robot_id = rospy.get_param('~robot_id', 'crazy01')
init_x = rospy.get_param('~init_x', 0.)
init_y = rospy.get_param('~init_y', 0.)
init_z = rospy.get_param('~init_z', 0.)
odom_topic = rospy.get_param('~odom_topic', '/odom') # '/odom2'
# cmd_vel_topic = rospy.get_param('~cmd_vel_topic', '/cmd_vel') # '/cmd_vel2'
# viewer = rospy.get_param('~viewer', False)
# service for dislocate the robot
rospy.Service('dislocate_robot', Dislocation, dislocate)
# Subscribe to control input
rospy.Subscriber('/' + robot_id + '/cmd_vel', Twist,
control_input_listenener)
# Odom publisher
odom_pub = rospy.Publisher('/' + robot_id + odom_topic,
Odometry,
queue_size=0)
# TF publisher
tf_broadcaster = tf2_ros.TransformBroadcaster()
########### Simulator ##############
loc = [init_x, init_y, init_z]
x = init_state(loc, 0)
freq = 100 # 100hz
rate = rospy.Rate(freq)
while not rospy.is_shutdown():
rate.sleep()
# Dislocate based on request
x[0] += dislocation_srv[0]
x[1] += dislocation_srv[1]
dislocation_srv = (0., 0.)
# Publish odometry
publish_odom(x, odom_pub)
publish_transform_stamped(robot_id, x, tf_broadcaster)
# Control output
F, M = attitude_controller((thrust_pwm, roll, pitch, yaw), x)
# Derivative of the robot dynamics
f_dot = lambda t1, s: state_derivative(s, F, M)
# Solve the differential equation of motion
r = ode(f_dot).set_integrator('dopri5')
r.set_initial_value(x, 0)
r.integrate(r.t + 1. / freq, step=True)
if not r.successful():
return
x = r.y
# Simulate floor
if x[2] < 0:
x[2] = 0.
# Velocity towards the floor
if x[5] < 0:
x[5] = 0.
def simulate():
global dislocation_srv, thrust_newtons, roll, pitch, yaw
rospy.init_node('modrotor_simulator', anonymous=True)
robot_id = rospy.get_param('~robot_id', 'modquad01')
init_x = rospy.get_param('~init_x', 1.)
init_y = rospy.get_param('~init_y', 0.)
init_z = rospy.get_param('~init_z', 0.)
demo_trajectory = rospy.get_param('~demo_trajectory', False)
odom_topic = rospy.get_param('~odom_topic', '/odom') # '/odom2'
# cmd_vel_topic = rospy.get_param('~cmd_vel_topic', '/cmd_vel') # '/cmd_vel2'
# service for dislocate the robot
rospy.Service('dislocate_robot', Dislocation, dislocate)
# TODO read structure and create a service to change it.
structure4 = Structure(ids=['modquad01', 'modquad02'],
xx=[0, params.cage_width, 0, params.cage_width],
yy=[0, 0, params.cage_width, params.cage_width],
motor_failure=[])
structure4fail = Structure(ids=['modquad01', 'modquad02'],
xx=[0, params.cage_width, 0, params.cage_width],
yy=[0, 0, params.cage_width, params.cage_width],
motor_failure=[(1, 0)])
structure1 = Structure(ids=[robot_id], xx=[0], yy=[0])
structure = structure1
# Subscribe to control input
rospy.Subscriber('/' + robot_id + '/cmd_vel', Twist,
control_input_listener)
# Odom publisher
odom_publishers = {
id_robot: rospy.Publisher('/' + id_robot + odom_topic,
Odometry,
queue_size=0)
for id_robot in structure.ids
}
# TF publisher
tf_broadcaster = tf2_ros.TransformBroadcaster()
########### Simulator ##############
loc = [init_x, init_y, init_z]
state_vector = init_state(loc, 0)
freq = 100 # 100hz
rate = rospy.Rate(freq)
t = 0
waypts = np.array([[0, 0, 0], [0, 0.5, 0], [0.5, 0.5, 0], [0.5, 0, 0],
[0, 0, 0]])
traj_vars = min_snap_trajectory(0, 10, None, waypts)
while not rospy.is_shutdown():
rate.sleep()
t += 1. / freq
## Dislocate based on request
state_vector[0] += dislocation_srv[0]
state_vector[1] += dislocation_srv[1]
dislocation_srv = (0., 0.)
# Publish odometry
publish_structure_odometry(structure, state_vector, odom_publishers,
tf_broadcaster)
if demo_trajectory:
# F, M = control_output( state_vector,
# min_snap_trajectory(t % 10, 30, traj_vars), control_handle)
# F, M = control_output( state_vector,
# simple_waypt_trajectory(waypts, t % 10, 30), control_handle)
# F, M = control_output( state_vector,
# circular_trajectory(t % 10, 10), control_handle)
# Overwrite the control input with the demo trajectory
[thrust_newtons, roll, pitch,
yaw] = position_controller(state_vector,
circular_trajectory(t % 10, 10))
# Control output based on crazyflie input
F_single, M_single = attitude_controller(
(thrust_newtons, roll, pitch, yaw), state_vector)
# Control of Moments and thrust
F_structure, M_structure, rotor_forces = modquad_torque_control(
F_single, M_single, structure, motor_sat=False)
# Simulate
state_vector = simulation_step(structure, state_vector, F_structure,
M_structure, 1. / freq)