def default_action(self):
""" Run attitude controller and apply resulting force and torque to the parent robot. """
# Get the the parent robot
robot = self.robot_parent
if self.local_data['thrust'] > 0:
# yaw_rate and yaw_setpoint in NED
self.yaw_setpoint += self.local_data['yaw'] / self.frequency
# wrap angle
self.yaw_setpoint = normalise_angle(self.yaw_setpoint)
logger.debug("yaw setpoint: %.3f", degrees(self.yaw_setpoint))
logger.debug("yaw current: %.3f setpoint: %.3f",
-degrees(self.position_3d.yaw),
degrees(self.yaw_setpoint))
# Compute errors
#
# e = att_sp - att = attitude error
# current angles to horizontal plane in NED
roll = self.position_3d.roll
pitch = -self.position_3d.pitch
yaw = -self.position_3d.yaw
roll_err = self.local_data['roll'] - roll
pitch_err = self.local_data['pitch'] - pitch
# wrapped yaw error
yaw_err = normalise_angle(self.yaw_setpoint - yaw)
err = Vector((roll_err, pitch_err, yaw_err))
logger.debug("attitude error: (% .3f % .3f % .3f)",
degrees(err[0]), degrees(err[1]), degrees(err[2]))
# derivative
we = (err - self.prev_err) * self.frequency
logger.debug("yaw rate error: %.3f", we[2])
kp = Vector((self._rp_pgain, self._rp_pgain, self._yaw_pgain))
kd = Vector((self._rp_dgain, self._rp_dgain, self._yaw_dgain))
# torque = self.inertia * (kp * err + kd * we)
t = []
for i in range(3):
t.append(self.inertia[i] * (kp[i] * err[i] + kd[i] * we[i]))
# convert to blender frame and scale with thrust
torque = Vector((t[0], -t[1], -t[2])) * self.local_data['thrust']
logger.debug("applied torques: (% .3f % .3f % .3f)", torque[0],
torque[1], torque[2])
force = Vector(
(0.0, 0.0, self.local_data['thrust'] * self._thrust_factor))
logger.debug("applied thrust force: %.3f", force[2])
self.prev_err = err.copy()
else:
force = Vector((0.0, 0.0, 0.0))
torque = Vector((0.0, 0.0, 0.0))
# directly apply local forces and torques to the blender object of the parent robot
robot.bge_object.applyForce(force, True)
robot.bge_object.applyTorque(torque, True)
def default_action(self):
""" Run attitude controller and apply resulting force and torque to the parent robot. """
# Get the the parent robot
robot = self.robot_parent
if self.local_data['thrust'] > 0:
if self._yaw_rate_control:
# yaw_rate and yaw_setpoint in NED
self.yaw_setpoint += self.local_data['yaw'] / self.frequency
# wrap angle
self.yaw_setpoint = normalise_angle(self.yaw_setpoint)
logger.debug("yaw setpoint: %.3f", degrees(self.yaw_setpoint))
logger.debug("yaw current: %.3f setpoint: %.3f", -degrees(self.position_3d.yaw), degrees(self.yaw_setpoint))
else:
self.yaw_setpoint = normalise_angle(self.local_data['yaw'])
# Compute errors
#
# e = att_sp - att = attitude error
# current angles to horizontal plane in NED
roll = self.position_3d.roll
pitch = -self.position_3d.pitch
yaw = -self.position_3d.yaw
roll_err = self.local_data['roll'] - roll
pitch_err = self.local_data['pitch'] - pitch
# wrapped yaw error
yaw_err = normalise_angle(self.yaw_setpoint - yaw)
err = Vector((roll_err, pitch_err, yaw_err))
logger.debug("attitude error: (% .3f % .3f % .3f)", degrees(err[0]), degrees(err[1]), degrees(err[2]))
# derivative
we = (err - self.prev_err) * self.frequency
logger.debug("yaw rate error: %.3f", we[2])
kp = Vector((self._rp_pgain, self._rp_pgain, self._yaw_pgain))
kd = Vector((self._rp_dgain, self._rp_dgain, self._yaw_dgain))
# torque = self.inertia * (kp * err + kd * we)
t = []
for i in range(3):
t.append(self.inertia[i] * (kp[i] * err[i] + kd[i] * we[i]))
# convert to blender frame and scale with thrust
torque = Vector((t[0], -t[1], -t[2])) * self.local_data['thrust']
logger.debug("applied torques: (% .3f % .3f % .3f)", torque[0], torque[1], torque[2])
force = Vector((0.0, 0.0, self.local_data['thrust'] * self._thrust_factor))
logger.debug("applied thrust force: %.3f", force[2])
self.prev_err = err.copy()
else:
force = Vector((0.0, 0.0, 0.0))
torque = Vector((0.0, 0.0, 0.0))
# directly apply local forces and torques to the blender object of the parent robot
robot.bge_object.applyForce(force, True)
robot.bge_object.applyTorque(torque, True)
def default_action(self):
""" Move the object towards the destination. """
robot = self.robot_parent
if self._pos_initalized:
self._destination = Vector((self.local_data["x"], self.local_data["y"], self.local_data["z"]))
else:
self._destination = self.robot_parent.bge_object.worldPosition
self.local_data["x"] = self._destination[0]
self.local_data["y"] = self._destination[1]
self.local_data["z"] = self._destination[2]
self.local_data["yaw"] = self.robot_parent.position_3d.yaw
self._pos_initalized = True
# logger.debug("Robot %s move status: '%s'", robot.bge_object.name, robot.move_status)
# Place the target marker where the robot should go
if self._wp_object:
self._wp_object.worldPosition = self._destination
self._wp_object.worldOrientation = Matrix.Rotation(self.local_data["yaw"], 3, "Z")
# current angles to horizontal plane (not quite, but approx good enough)
roll = self.position_3d.roll
pitch = self.position_3d.pitch
yaw = self.position_3d.yaw
# current position and velocity of robot
pos_blender = robot.bge_object.worldPosition
vel_blender = robot.bge_object.worldLinearVelocity
pos_error = self._destination - pos_blender
# zero velocity setpoint for now
vel_error = -vel_blender
logger.debug(
"pos current: (% .3f % .3f % .3f) setpoint: (% .3f % .3f % .3f)",
pos_blender[0],
pos_blender[1],
pos_blender[2],
self._destination[0],
self._destination[1],
self._destination[2],
)
logger.debug("velocity: (% .3f % .3f % .3f)", vel_blender[0], vel_blender[1], vel_blender[2])
# simple PD controller on horizontal position
command_world_x = self._h_pgain * pos_error[0] + self._h_dgain * vel_error[0]
command_world_y = self._h_pgain * pos_error[1] + self._h_dgain * vel_error[1]
# setpoints in body frame
self.roll_setpoint = sin(yaw) * command_world_x - cos(yaw) * command_world_y
self.pitch_setpoint = cos(yaw) * command_world_x + sin(yaw) * command_world_y
self.yaw_setpoint = self.local_data["yaw"]
# saturate max roll/pitch angles
if fabs(self.roll_setpoint) > self._max_bank_angle:
self.roll_setpoint = copysign(self._max_bank_angle, self.roll_setpoint)
if fabs(self.pitch_setpoint) > self._max_bank_angle:
self.pitch_setpoint = copysign(self._max_bank_angle, self.pitch_setpoint)
# wrap yaw setpoint
self.yaw_setpoint = normalise_angle(self.yaw_setpoint)
logger.debug("roll current: % 2.3f setpoint: % 2.3f", degrees(roll), degrees(self.roll_setpoint))
logger.debug("pitch current: % 2.3f setpoint: % 2.3f", degrees(pitch), degrees(self.pitch_setpoint))
logger.debug("yaw current: % 2.3f setpoint: % 2.3f", degrees(yaw), degrees(self.yaw_setpoint))
# compute thrust
# nominal command to keep altitude (feed forward)
thrust_attitude_compensation = max(self._attitude_compensation_limit, cos(roll) * cos(pitch))
thrust_ff = self.nominal_thrust / thrust_attitude_compensation
# feedback to correct altitude
thrust_fb = self._v_pgain * pos_error[2] + self._v_dgain * vel_error[2]
self.thrust = max(0, thrust_ff + thrust_fb)
# Compute attitude errors
#
# e = att_sp - att = attitude error
roll_err = normalise_angle(self.roll_setpoint - roll)
pitch_err = normalise_angle(self.pitch_setpoint - pitch)
yaw_err = normalise_angle(self.yaw_setpoint - yaw)
err = Vector((roll_err, pitch_err, yaw_err))
# derivative
we = (err - self.prev_err) * self.frequency
# we = mathutils.Vector((0.0, 0.0, 0.0))
# logger.debug("yaw rate error: %.3f", we[2])
kp = Vector((self._rp_pgain, self._rp_pgain, self._yaw_pgain))
kd = Vector((self._rp_dgain, self._rp_pgain, self._yaw_dgain))
# torque = self.inertia * (kp * err + kd * we)
t = []
for i in range(3):
t.append(self.inertia[i] * (kp[i] * err[i] + kd[i] * we[i]))
# scale with thrust
torque = Vector((t[0], t[1], t[2])) * self.thrust / self.nominal_thrust
logger.debug("applied torques: (% .3f % .3f % .3f)", torque[0], torque[1], torque[2])
force = Vector((0.0, 0.0, self.thrust))
logger.debug("applied thrust force: %.3f", force[2])
self.prev_err = err.copy()
# directly apply local forces and torques to the blender object of the parent robot
robot.bge_object.applyForce(force, True)
robot.bge_object.applyTorque(torque, True)
# Vectors returned are already normalized
wp_distance, global_vector, local_vector = self.bge_object.getVectTo(self._destination)
# logger.debug("GOT DISTANCE: xyz: %.4f", wp_distance)
# If the target has been reached, change the status
if wp_distance - self.local_data["tolerance"] <= 0:
robot.move_status = "Arrived"
# Do we have a running request? if yes, notify the completion
self.completed(status.SUCCESS, robot.move_status)
# logger.debug("TARGET REACHED")
# logger.debug("Robot %s move status: '%s'", robot.bge_object.name, robot.move_status)
else:
robot.move_status = "Transit"
def default_action(self):
""" Move the object towards the destination. """
robot = self.robot_parent
if self._pos_initalized:
self._destination = Vector(
(self.local_data['x'], self.local_data['y'],
self.local_data['z']))
else:
self._destination = self.robot_parent.bge_object.worldPosition
self.local_data['x'] = self._destination[0]
self.local_data['y'] = self._destination[1]
self.local_data['z'] = self._destination[2]
self.local_data['yaw'] = self.robot_parent.position_3d.yaw
self._pos_initalized = True
#logger.debug("Robot %s move status: '%s'", robot.bge_object.name, robot.move_status)
# Place the target marker where the robot should go
if self._wp_object:
self._wp_object.worldPosition = self._destination
self._wp_object.worldOrientation = Matrix.Rotation(
self.local_data['yaw'], 3, 'Z')
# current angles to horizontal plane (not quite, but approx good enough)
roll = self.position_3d.roll
pitch = self.position_3d.pitch
yaw = self.position_3d.yaw
# current position and velocity of robot
pos_blender = robot.bge_object.worldPosition
vel_blender = robot.bge_object.worldLinearVelocity
pos_error = self._destination - pos_blender
# zero velocity setpoint for now
vel_error = -vel_blender
logger.debug("pos current: (% .3f % .3f % .3f) setpoint: (% .3f % .3f % .3f)", \
pos_blender[0], pos_blender[1], pos_blender[2], \
self._destination[0], self._destination[1], self._destination[2])
logger.debug("velocity: (% .3f % .3f % .3f)", vel_blender[0],
vel_blender[1], vel_blender[2])
# simple PD controller on horizontal position
command_world_x = self._h_pgain * pos_error[
0] + self._h_dgain * vel_error[0]
command_world_y = self._h_pgain * pos_error[
1] + self._h_dgain * vel_error[1]
# setpoints in body frame
self.roll_setpoint = sin(yaw) * command_world_x - cos(
yaw) * command_world_y
self.pitch_setpoint = cos(yaw) * command_world_x + sin(
yaw) * command_world_y
self.yaw_setpoint = self.local_data['yaw']
# saturate max roll/pitch angles
if fabs(self.roll_setpoint) > self._max_bank_angle:
self.roll_setpoint = copysign(self._max_bank_angle,
self.roll_setpoint)
if fabs(self.pitch_setpoint) > self._max_bank_angle:
self.pitch_setpoint = copysign(self._max_bank_angle,
self.pitch_setpoint)
# wrap yaw setpoint
self.yaw_setpoint = normalise_angle(self.yaw_setpoint)
logger.debug("roll current: % 2.3f setpoint: % 2.3f", \
degrees(roll), degrees(self.roll_setpoint))
logger.debug("pitch current: % 2.3f setpoint: % 2.3f", \
degrees(pitch), degrees(self.pitch_setpoint))
logger.debug("yaw current: % 2.3f setpoint: % 2.3f", \
degrees(yaw), degrees(self.yaw_setpoint))
# compute thrust
# nominal command to keep altitude (feed forward)
thrust_attitude_compensation = max(self._attitude_compensation_limit,
cos(roll) * cos(pitch))
thrust_ff = self.nominal_thrust / thrust_attitude_compensation
# feedback to correct altitude
thrust_fb = self._v_pgain * pos_error[2] + self._v_dgain * vel_error[2]
self.thrust = max(0, thrust_ff + thrust_fb)
# Compute attitude errors
#
# e = att_sp - att = attitude error
roll_err = normalise_angle(self.roll_setpoint - roll)
pitch_err = normalise_angle(self.pitch_setpoint - pitch)
yaw_err = normalise_angle(self.yaw_setpoint - yaw)
err = Vector((roll_err, pitch_err, yaw_err))
# derivative
we = (err - self.prev_err) * self.frequency
#we = mathutils.Vector((0.0, 0.0, 0.0))
#logger.debug("yaw rate error: %.3f", we[2])
kp = Vector((self._rp_pgain, self._rp_pgain, self._yaw_pgain))
kd = Vector((self._rp_dgain, self._rp_dgain, self._yaw_dgain))
#torque = self.inertia * (kp * err + kd * we)
t = []
for i in range(3):
t.append(self.inertia[i] * (kp[i] * err[i] + kd[i] * we[i]))
# scale with thrust
torque = Vector((t[0], t[1], t[2])) * self.thrust / self.nominal_thrust
logger.debug("applied torques: (% .3f % .3f % .3f)", torque[0],
torque[1], torque[2])
force = Vector((0.0, 0.0, self.thrust))
logger.debug("applied thrust force: %.3f", force[2])
self.prev_err = err.copy()
# directly apply local forces and torques to the blender object of the parent robot
robot.bge_object.applyForce(force, True)
robot.bge_object.applyTorque(torque, True)
# Vectors returned are already normalized
wp_distance, global_vector, local_vector = self.bge_object.getVectTo(
self._destination)
#logger.debug("GOT DISTANCE: xyz: %.4f", wp_distance)
# If the target has been reached, change the status
if wp_distance - self.local_data['tolerance'] <= 0:
robot.move_status = "Arrived"
#Do we have a running request? if yes, notify the completion
self.completed(status.SUCCESS, robot.move_status)
#logger.debug("TARGET REACHED")
#logger.debug("Robot %s move status: '%s'", robot.bge_object.name, robot.move_status)
else:
robot.move_status = "Transit"
def default_action(self):
""" Move the robot. """
robot_obj = self.robot_parent.bge_object
vel_body_sp = Vector((self.local_data['vx'], self.local_data['vy'],
self.local_data['vz']))
# TODO: limit max_velocity setpoint
# current angles to horizontal plane in NED
# (not quite, but approx good enough)
roll = self.position_3d.roll
pitch = self.position_3d.pitch
yaw = self.position_3d.yaw
# convert the commands in body frame to blender frame
# in which frame do we want to command??
#body2blender = Matrix.Rotation(math.pi, 3, 'X') * Matrix.Rotation(yaw, 3, 'Z')
body2blender = Matrix.Rotation(yaw, 3, 'Z')
vel_blender_sp = body2blender * vel_body_sp
# current velocity of robot in Blender world frame
vel_blender = robot_obj.worldLinearVelocity
# errors in blender frame
vel_error = vel_blender_sp - vel_blender
# zero desired acceleration for now... no reference...
accel_error = -(vel_blender - self._prev_vel_blender) * self.frequency
logger.debug("velocity in blender frame: (% .3f % .3f % .3f)",
vel_blender[0], vel_blender[1], vel_blender[2])
logger.debug("velocity setpoint:: (% .3f % .3f % .3f)",
vel_blender_sp[0], vel_blender_sp[1], vel_blender_sp[2])
logger.debug("velocity error: (% .3f % .3f % .3f)", vel_error[0],
vel_error[1], vel_error[2])
logger.debug("acceleration error: (% .3f % .3f % .3f)", accel_error[0],
accel_error[1], accel_error[2])
cmd_blender_x = self._h_pgain * vel_error[
0] + self._h_dgain * accel_error[0]
cmd_blender_y = self._h_pgain * vel_error[
1] + self._h_dgain * accel_error[1]
self.roll_setpoint = sin(yaw) * cmd_blender_x - cos(
yaw) * cmd_blender_y
self.pitch_setpoint = cos(yaw) * cmd_blender_x + sin(
yaw) * cmd_blender_y
self.yaw_setpoint = self._prev_yaw - (self.local_data['vyaw'] /
self.frequency)
# saturate max roll/pitch angles
if fabs(self.roll_setpoint) > self._max_bank_angle:
self.roll_setpoint = copysign(self._max_bank_angle,
self.roll_setpoint)
if fabs(self.pitch_setpoint) > self._max_bank_angle:
self.pitch_setpoint = copysign(self._max_bank_angle,
self.pitch_setpoint)
# wrap yaw setpoint
self.yaw_setpoint = normalise_angle(self.yaw_setpoint)
logger.debug("roll current: % 2.3f setpoint: % 2.3f", degrees(roll),
degrees(self.roll_setpoint))
logger.debug("pitch current: % 2.3f setpoint: % 2.3f",
degrees(pitch), degrees(self.pitch_setpoint))
logger.debug("yaw current: % 2.3f setpoint: % 2.3f", degrees(yaw),
degrees(self.yaw_setpoint))
# compute thrust
# nominal command to keep altitude (feed forward)
thrust_attitude_compensation = max(self._attitude_compensation_limit,
cos(roll) * cos(pitch))
thrust_ff = self.nominal_thrust / thrust_attitude_compensation
# feedback to correct vertical speed
thrust_fb = self._v_pgain * vel_error[
2] # + self._v_dgain * accel_error[2]
self.thrust = max(0, thrust_ff + thrust_fb)
# Compute attitude errors
#
# e = att_sp - att = attitude error
roll_err = normalise_angle(self.roll_setpoint - roll)
pitch_err = normalise_angle(self.pitch_setpoint - pitch)
yaw_err = normalise_angle(self.yaw_setpoint - yaw)
err = Vector((roll_err, pitch_err, yaw_err))
# derivative
we = (err - self._prev_err) * self.frequency
# we = Vector((0.0, 0.0, 0.0))
# logger.debug("yaw rate error: %.3f", we[2])
kp = Vector((self._rp_pgain, self._rp_pgain, self._yaw_pgain))
kd = Vector((self._rp_dgain, self._rp_dgain, self._yaw_dgain))
# torque = self.inertia * (kp * err + kd * we)
t = []
for i in range(3):
t.append(self.inertia[i] * (kp[i] * err[i] + kd[i] * we[i]))
# convert to blender frame and scale with thrust
torque = Vector((t[0], t[1], t[2])) * self.thrust / self.nominal_thrust
logger.debug("applied torques: (% .3f % .3f % .3f)", torque[0],
torque[1], torque[2])
force = Vector((0.0, 0.0, self.thrust))
logger.debug("applied thrust force: %.3f", force[2])
self._prev_err = err.copy()
self._prev_vel_blender = vel_blender.copy()
self._prev_yaw = yaw
# directly apply local forces and torques to the blender object of the parent robot
robot_obj.applyForce(force, True)
robot_obj.applyTorque(torque, True)
