def default_action(self):
""" Compute the relative position and rotation of the robot
The measurements are taken with respect to the previous position
and orientation of the robot
"""
# Compute the position of the sensor within the original frame
current_pos = self.original_pos.transformation3d_with(self.position_3d)
# Compute the difference in positions with the previous loop
self._dx = current_pos.x - self.previous_pos.x
self._dy = current_pos.y - self.previous_pos.y
self.local_data['dx'] = self._dx
self.local_data['dy'] = self._dy
self.local_data['dz'] = current_pos.z - self.previous_pos.z
# Compute the difference in orientation with the previous loop
dyaw = current_pos.yaw - self.previous_pos.yaw
dpitch = current_pos.pitch - self.previous_pos.pitch
droll = current_pos.roll - self.previous_pos.roll
self.local_data['dyaw'] = normalise_angle(dyaw)
self.local_data['dpitch'] = normalise_angle(dpitch)
self.local_data['droll'] = normalise_angle(droll)
# Store the 'new' previous data
self.previous_pos = current_pos
def default_action(self):
""" Compute the relative position and rotation of the robot
The measurements are taken with respect to the previous position
and orientation of the robot
"""
# Compute the position of the sensor within the original frame
current_pos = self.original_pos.transformation3d_with(self.position_3d)
# Compute the difference in positions with the previous loop
self._dx = current_pos.x - self.previous_pos.x
self._dy = current_pos.y - self.previous_pos.y
self.local_data['dx'] = self._dx
self.local_data['dy'] = self._dy
self.local_data['dz'] = current_pos.z - self.previous_pos.z
# Compute the difference in orientation with the previous loop
dyaw = current_pos.yaw - self.previous_pos.yaw
dpitch = current_pos.pitch - self.previous_pos.pitch
droll = current_pos.roll - self.previous_pos.roll
self.local_data['dyaw'] = normalise_angle(dyaw)
self.local_data['dpitch'] = normalise_angle(dpitch)
self.local_data['droll'] = normalise_angle(droll)
# Store the 'new' previous data
self.previous_pos = current_pos
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):
""" Change the parent robot orientation. """
if self._speed == float('inf'):
# New parent orientation
orientation = mathutils.Euler([self.local_data['roll'],
self.local_data['pitch'],
self.local_data['yaw']])
self.robot_parent.force_pose(None, orientation.to_matrix())
else:
goal = [self.local_data['roll'], self.local_data['pitch'], self.local_data['yaw']]
current_rot = [self.position_3d.roll, self.position_3d.pitch, self.position_3d.yaw]
cmd = [0.0, 0.0, 0.0]
for i in range(0, 3):
diff = goal[i] - current_rot[i]
diff = normalise_angle(diff)
diff_abs = abs(diff)
if diff_abs < self._tolerance:
cmd[i] = 0.0
else:
sign = diff_abs / diff
if diff_abs > self._speed / self._frequency:
cmd[i] = sign * self._speed
else:
cmd[i] = diff_abs * self._frequency
if self._type == 'Position':
cmd[i] /= self._frequency
self.robot_parent.apply_speed(self._type, [0.0, 0.0, 0.0], cmd)
def default_action(self):
# Compute the position of the sensor within the original frame
current_pos = self.original_pos.transformation3d_with(self.position_3d)
# Integrated version
self._dx = current_pos.x - self.previous_pos.x
self._dy = current_pos.y - self.previous_pos.y
self._dyaw = normalise_angle(current_pos.yaw - self.previous_pos.yaw)
self.local_data['x'] = current_pos.x
self.local_data['y'] = current_pos.y
self.local_data['z'] = current_pos.z
self.local_data['yaw'] = current_pos.yaw
self.local_data['pitch'] = current_pos.pitch
self.local_data['roll'] = current_pos.roll
# speed in the sensor frame, related to the robot pose
lin_vel = linear_velocities(self.previous_pos, current_pos, self.frequency)
ang_vel = angular_velocities(self.previous_pos, current_pos, 1/self.frequency)
self.local_data['vx'] = lin_vel[0]
self.local_data['vy'] = lin_vel[1]
self.local_data['vz'] = lin_vel[2]
self.local_data['wx'] = ang_vel[0]
self.local_data['wy'] = ang_vel[1]
self.local_data['wz'] = ang_vel[2]
# Store the 'new' previous data
self.previous_pos = current_pos
def default_action(self):
# Compute the position of the sensor within the original frame
current_pos = copy.copy(self.position_3d)
# Integrated version
self._dx = current_pos.x - self.previous_pos.x
self._dy = current_pos.y - self.previous_pos.y
self._dyaw = normalise_angle(current_pos.yaw - self.previous_pos.yaw)
self.local_data['x'] = current_pos.x
self.local_data['y'] = current_pos.y
self.local_data['z'] = current_pos.z
self.local_data['yaw'] = current_pos.yaw
self.local_data['pitch'] = current_pos.pitch
self.local_data['roll'] = current_pos.roll
self.delta_pos = self.previous_pos.transformation3d_with(current_pos)
self.local_data['vx'] = self.delta_pos.x * self.frequency
self.local_data['vy'] = self.delta_pos.y * self.frequency
self.local_data['vz'] = self.delta_pos.z * self.frequency
self.local_data['wz'] = self.delta_pos.yaw * self.frequency
self.local_data['wy'] = self.delta_pos.pitch * self.frequency
self.local_data['wx'] = self.delta_pos.roll * self.frequency
# Store the 'new' previous data
self.previous_pos = current_pos
def default_action(self):
# Compute the position of the sensor within the original frame
current_pos = self.original_pos.transformation3d_with(self.position_3d)
# Integrated version
self._dx = current_pos.x - self.previous_pos.x
self._dy = current_pos.y - self.previous_pos.y
self._dyaw = normalise_angle(current_pos.yaw - self.previous_pos.yaw)
self.local_data['x'] = current_pos.x
self.local_data['y'] = current_pos.y
self.local_data['z'] = current_pos.z
self.local_data['yaw'] = current_pos.yaw
self.local_data['pitch'] = current_pos.pitch
self.local_data['roll'] = current_pos.roll
# speed in the sensor frame, related to the robot pose
lin_vel = linear_velocities(self.previous_pos, current_pos, 1/self.frequency)
ang_vel = angular_velocities(self.previous_pos, current_pos,1/self.frequency)
self.local_data['vx'] = lin_vel[0]
self.local_data['vy'] = lin_vel[1]
self.local_data['vz'] = lin_vel[2]
self.local_data['wx'] = ang_vel[0]
self.local_data['wy'] = ang_vel[1]
self.local_data['wz'] = ang_vel[2]
# Store the 'new' previous data
self.previous_pos = current_pos
def default_action(self):
# Compute the position of the sensor within the original frame
current_pos = self.original_pos.transformation3d_with(self.position_3d)
# Integrated version
self._dx = current_pos.x - self.previous_pos.x
self._dy = current_pos.y - self.previous_pos.y
self._dyaw = normalise_angle(current_pos.yaw - self.previous_pos.yaw)
self.local_data['x'] = current_pos.x
self.local_data['y'] = current_pos.y
self.local_data['z'] = current_pos.z
self.local_data['yaw'] = current_pos.yaw
self.local_data['pitch'] = current_pos.pitch
self.local_data['roll'] = current_pos.roll
# speed in the sensor frame, related to the robot pose
self.delta_pos = self.previous_pos.transformation3d_with(current_pos)
self.local_data['vx'] = self.delta_pos.x * self.frequency
self.local_data['vy'] = self.delta_pos.y * self.frequency
self.local_data['vz'] = self.delta_pos.z * self.frequency
self.local_data['wz'] = self.delta_pos.yaw * self.frequency
self.local_data['wy'] = self.delta_pos.pitch * self.frequency
self.local_data['wx'] = self.delta_pos.roll * self.frequency
# Store the 'new' previous data
self.previous_pos = current_pos
def default_action(self):
""" Change the parent robot orientation. """
if self._speed == float('inf'):
# New parent orientation
orientation = mathutils.Euler([self.local_data['roll'],
self.local_data['pitch'],
self.local_data['yaw']])
self.robot_parent.force_pose(None, orientation.to_matrix())
else:
goal = [self.local_data['roll'], self.local_data['pitch'], self.local_data['yaw']]
current_rot = [self.position_3d.roll, self.position_3d.pitch, self.position_3d.yaw]
cmd = [0.0, 0.0, 0.0]
for i in range(0, 3):
diff = goal[i] - current_rot[i]
diff = normalise_angle(diff)
diff_abs = abs(diff)
if diff_abs < self._tolerance:
cmd[i] = 0.0
else:
sign = diff_abs / diff
if diff_abs > self._speed / self._frequency:
cmd[i] = sign * self._speed
else:
cmd[i] = diff_abs * self._frequency
if self._type == 'Position':
cmd[i] /= self._frequency
self.robot_parent.apply_speed(self._type, [0.0, 0.0, 0.0], cmd)
def default_action(self):
""" Compute the relative position and rotation of the robot
The measurements are taken with respect to the previous position
and orientation of the robot
"""
# Compute the position of the sensor within the original frame
current_pos = self.original_pos.transformation3d_with(self.position_3d)
# Compute the difference in positions with the previous loop
self.local_data['dS'] = current_pos.distance(self.previous_pos)
self.local_data['dx'] = current_pos.x - self.previous_pos.x
self.local_data['dy'] = current_pos.y - self.previous_pos.y
self.local_data['dz'] = current_pos.z - self.previous_pos.z
# Compute the difference in orientation with the previous loop
dyaw = current_pos.yaw - self.previous_pos.yaw
dpitch = current_pos.pitch - self.previous_pos.pitch
droll = current_pos.roll - self.previous_pos.roll
self.local_data['dyaw'] = normalise_angle(dyaw)
self.local_data['dpitch'] = normalise_angle(dpitch)
self.local_data['droll'] = normalise_angle(droll)
# Integrated version
self.local_data['x'] = current_pos.x
self.local_data['y'] = current_pos.y
self.local_data['z'] = current_pos.z
self.local_data['yaw'] = current_pos.yaw
self.local_data['pitch'] = current_pos.pitch
self.local_data['roll'] = current_pos.roll
# speed in the sensor frame, related to the robot pose
self.delta_pos = self.previous_pos.transformation3d_with(current_pos)
self.local_data['vx'] = self.delta_pos.x * self.frequency
self.local_data['vy'] = self.delta_pos.y * self.frequency
self.local_data['vz'] = self.delta_pos.z * self.frequency
self.local_data['wz'] = self.delta_pos.yaw * self.frequency
self.local_data['wy'] = self.delta_pos.pitch * self.frequency
self.local_data['wx'] = self.delta_pos.roll * self.frequency
# Store the 'new' previous data
self.previous_pos = current_pos
def default_action(self):
""" Apply rotation to the platine unit """
# Reset movement variables
ry, rz = 0.0, 0.0
if self._is_manual_mode:
return
try:
normal_speed = self._speed / self.frequency
# For the moment ignoring the division by zero
# It happens apparently when the simulation starts
except ZeroDivisionError:
pass
self._current_pan = self._pan_orientation.to_euler().z
self._current_tilt = self._tilt_orientation.to_euler().y
logger.debug("PTU: pan=%.4f, tilt=%.4f" %
(self._current_pan, self._current_tilt))
# Get the angles in a range of -PI, PI
target_pan = normalise_angle(self.local_data['pan'])
target_tilt = normalise_angle(self.local_data['tilt'])
logger.debug("Targets: pan=%.4f, tilt=%.4f" %
(target_pan, target_tilt))
if (abs(target_pan - self._current_pan) < self._tolerance
and abs(target_tilt - self._current_tilt) < self._tolerance):
self.completed(status.SUCCESS)
# Determine the direction of the rotation, if any
ry = rotation_direction(self._current_tilt, target_tilt,
self._tolerance, normal_speed)
rz = rotation_direction(self._current_pan, target_pan, self._tolerance,
normal_speed)
# Give the rotation instructions directly to the parent
# The second parameter specifies a "local" movement
self._pan_base.applyRotation([0.0, 0.0, rz], True)
self._tilt_base.applyRotation([0.0, ry, 0.0], True)
def default_action(self):
""" Apply rotation to the platine unit """
# Reset movement variables
ry, rz = 0.0, 0.0
if self._is_manual_mode:
return
try:
normal_speed = self._speed / self.frequency
# For the moment ignoring the division by zero
# It happens apparently when the simulation starts
except ZeroDivisionError:
pass
self._current_pan = self._pan_orientation.to_euler().z
self._current_tilt = self._tilt_orientation.to_euler().y
logger.debug("PTU: pan=%.4f, tilt=%.4f" % (self._current_pan,
self._current_tilt))
# Get the angles in a range of -PI, PI
target_pan = normalise_angle(self.local_data['pan'])
target_tilt = normalise_angle(self.local_data['tilt'])
logger.debug("Targets: pan=%.4f, tilt=%.4f" % (target_pan, target_tilt))
if (abs(target_pan - self._current_pan) < self._tolerance and
abs(target_tilt - self._current_tilt) < self._tolerance):
self.completed(status.SUCCESS)
# Determine the direction of the rotation, if any
ry = rotation_direction(self._current_tilt, target_tilt,
self._tolerance, normal_speed)
rz = rotation_direction(self._current_pan, target_pan,
self._tolerance, normal_speed)
# Give the rotation instructions directly to the parent
# The second parameter specifies a "local" movement
self._pan_base.applyRotation([0.0, 0.0, rz], True)
self._tilt_base.applyRotation([0.0, ry, 0.0], True)
def default_action(self):
if self._type == 'Velocity':
rates = self.sim_attitude_with_physics()
else:
rates = self.sim_attitude_simple()
# Store the important data
self.local_data['rotation'] = self.position_3d.euler
if self._use_angle_against_north:
self.local_data['rotation'][2] = \
normalise_angle(
- self._coord_converter.angle_against_geographic_north(self.position_3d.euler))
self.local_data['angular_velocity'] = rates
def default_action(self):
""" Apply rotation angles to the segments of the arm """
armature = self.blender_obj
logger.debug("The armature is: '%s' (%s)" % (armature, type(armature)))
for channel in armature.channels:
segment_angle = channel.joint_rotation
logger.debug("Channel '%s' st: [%.4f, %.4f, %.4f]" % (channel, segment_angle[0], segment_angle[1], segment_angle[2]))
# Get the normalised angle for this segment
target_angle = normalise_angle(self.local_data[channel.name])
# Use the corresponding direction for each rotation
if self._dofs[channel.name][1] == 1:
segment_angle[1] = target_angle
elif self._dofs[channel.name][2] == 1:
segment_angle[2] = target_angle
logger.debug("Channel '%s' fn: [%.4f, %.4f, %.4f]" % (channel, segment_angle[0], segment_angle[1], segment_angle[2]))
channel.joint_rotation = segment_angle
armature.update()
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)
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):
""" Apply rotation to the arm segments """
# Get the reference to the Sound actuator
if self._sound == None:
logger.debug ("ACTIVATING THE SOUND ACTUATOR")
contr = blenderapi.controller()
self._sound = contr.actuators['Sound']
contr.activate(self._sound)
self._sound.stopSound()
# Reset movement variables
rx, ry, rz = 0.0, 0.0, 0.0
# Scale the speeds to the time used by Blender
try:
rotation = self._speed / self.frequency
# For the moment ignoring the division by zero
# It happens apparently when the simulation starts
except ZeroDivisionError:
pass
self._moving = False
for i in range(6):
key = ('seg%d' % i)
target_angle = normalise_angle(self.local_data[key])
# Get the next segment
segment = self._segments[i]
# Extract the angles
rot_matrix = segment.localOrientation
segment_matrix = mathutils.Matrix((rot_matrix[0], rot_matrix[1], rot_matrix[2]))
segment_euler = segment_matrix.to_euler()
# Use the corresponding direction for each rotation
if self._dofs[i] == 'y':
ry = rotation_direction(segment_euler[1], target_angle, self._tolerance, rotation)
#logger.debug("PARAMETERS Y: %.4f, %.4f, %.4f, %.4f = %.4f" % (segment_euler[1], target_angle, _tolerance, rotation, ry))
elif self._dofs[i] == 'z':
rz = rotation_direction(segment_euler[2], target_angle, self._tolerance, rotation)
#logger.debug("PARAMETERS Z: %.4f, %.4f, %.4f, %.4f = %.4f" % (segment_euler[2], target_angle, _tolerance, rotation, rz))
logger.debug("ry = %.4f, rz = %.4f" % (ry, rz))
# Give the movement instructions directly to the parent
# The second parameter specifies a "local" movement
segment.applyRotation([rx, ry, rz], True)
if ry != 0.0 or rz != 0.0:
self._moving = True
# Reset the rotations for the next segment
ry = rz = 0
if self._moving:
self._sound.startSound()
logger.debug("STARTING SOUND")
else:
self._sound.stopSound()
logger.debug("STOPPING SOUND")
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):
""" Apply rotation to the arm segments """
# Get the reference to the Sound actuator
if self._sound is None:
logger.debug ("ACTIVATING THE SOUND ACTUATOR")
contr = blenderapi.controller()
self._sound = contr.actuators['Sound']
contr.activate(self._sound)
self._sound.stopSound()
# Reset movement variables
rx, ry, rz = 0.0, 0.0, 0.0
# Scale the speeds to the time used by Blender
try:
rotation = self._speed / self.frequency
# For the moment ignoring the division by zero
# It happens apparently when the simulation starts
except ZeroDivisionError:
pass
self._moving = False
for i in range(6):
key = ('seg%d' % i)
target_angle = normalise_angle(self.local_data[key])
# Get the next segment
segment = self._segments[i]
# Extract the angles
rot_matrix = segment.localOrientation
segment_matrix = mathutils.Matrix((rot_matrix[0], rot_matrix[1], rot_matrix[2]))
segment_euler = segment_matrix.to_euler()
# Use the corresponding direction for each rotation
if self._dofs[i] == 'y':
ry = rotation_direction(segment_euler[1], target_angle, self._tolerance, rotation)
#logger.debug("PARAMETERS Y: %.4f, %.4f, %.4f, %.4f = %.4f" % (segment_euler[1], target_angle, _tolerance, rotation, ry))
elif self._dofs[i] == 'z':
rz = rotation_direction(segment_euler[2], target_angle, self._tolerance, rotation)
#logger.debug("PARAMETERS Z: %.4f, %.4f, %.4f, %.4f = %.4f" % (segment_euler[2], target_angle, _tolerance, rotation, rz))
logger.debug("ry = %.4f, rz = %.4f" % (ry, rz))
# Give the movement instructions directly to the parent
# The second parameter specifies a "local" movement
segment.applyRotation([rx, ry, rz], True)
if ry != 0.0 or rz != 0.0:
self._moving = True
# Reset the rotations for the next segment
ry = rz = 0
if self._moving:
self._sound.startSound()
logger.debug("STARTING SOUND")
else:
self._sound.stopSound()
logger.debug("STOPPING SOUND")
