def __init__(self, obj, parent=None): logger.info("%s initialization" % obj.name) # Call the constructor of the parent class morse.core.actuator.Actuator.__init__(self, obj, parent) # a few references to ease matrix writing d = self._drag_factor b = self._thrust_factor l = self._lever bl = b * l bl_sq = bl * sqrt(2) / 2 if self._configuration == '+': self.transformation = Matrix( ([b, b, b, b], [0.0, -bl, 0.0, bl], [bl, 0.0, -bl, 0.0], [-d, d, -d, d])) elif self._configuration == 'x': self.transformation = Matrix( ([b, b, b, b], [bl_sq, -bl_sq, -bl_sq, bl_sq], [-bl_sq, -bl_sq, bl_sq, bl_sq], [-d, d, -d, d])) else: logger.error("Invalid configuration %s" % self._configuration) logger.info("Component initialized, runs at %.2f Hz ", self.frequency)
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)