diff --git a/libraries/SITL/examples/JSON/python/default.parm b/libraries/SITL/examples/JSON/python/default.parm new file mode 100644 index 00000000000000..8fb0200495478c --- /dev/null +++ b/libraries/SITL/examples/JSON/python/default.parm @@ -0,0 +1,28 @@ +# Rover parameters for velocity_controlled_robot.py +# +# The defaults below assume the model defaults: 1.5 m/s at full throttle and +# 1.2 rad/s at full steering. Change them together with the script arguments. + +# Skid steering: left and right throttle on SERVO1 and SERVO3. +SERVO1_FUNCTION 73 +SERVO3_FUNCTION 74 + +# Speed feed forward: half throttle gives half of the maximum speed. +CRUISE_SPEED 0.75 +CRUISE_THROTTLE 50 + +# Steering feed forward is the steering fraction needed for 1 rad/s, which is +# 1 / max turn rate. Leaving this at the default under states the gain by a +# factor of four, and the vehicle yaws back and forth without making progress. +ATC_STR_RAT_FF 0.83 + +WP_SPEED 1.0 +WP_RADIUS 2 + +# A vehicle of this class can turn on the spot, so pivot rather than arc when +# the heading error is large. +WP_PIVOT_ANGLE 30 +WP_PIVOT_RATE 60 + +# Lateral acceleration available at mission speed: 1.0 m/s * 1.2 rad/s. +TURN_MAX_G 0.15 diff --git a/libraries/SITL/examples/JSON/python/readme.md b/libraries/SITL/examples/JSON/python/readme.md new file mode 100644 index 00000000000000..b15ccf1e158444 --- /dev/null +++ b/libraries/SITL/examples/JSON/python/readme.md @@ -0,0 +1,65 @@ +# Velocity controlled robot + +A JSON backend for vehicles that ArduPilot commands by velocity rather than by +joint angle. It needs nothing outside the Python standard library. + +Walking robots that are sold as complete products keep their gait controller on +board and do not expose their joints. The autopilot asks for a forward speed and +a turn rate, and the on-board controller decides how to walk. Unitree's Go2 is +one such vehicle: its SDK takes a body velocity and walks. The same shape of +interface appears on tracked and differential platforms that take a velocity +command over a serial or network link. This example models that vehicle so +Rover's navigation, mission and failsafe code can be exercised without a physics +engine. + +The model here is a plain unicycle with a first order lag. It is not a model of +any particular robot, and no hardware has been driven from it. + +`../pybullet/walking_robot.py` covers the other case, where ArduPilot drives the +leg joints itself through twelve servo outputs. + +## Running + +```sh +python3 velocity_controlled_robot.py +``` + +then, in another terminal: + +```sh +sim_vehicle.py -v Rover --model JSON:127.0.0.1 --add-param-file=default.parm --map --console +``` + +Arm, upload a mission and switch to AUTO. The vehicle drives it. + +Options: + +```text +--max-speed forward speed at full throttle, default 1.5 m/s +--max-turn-rate turn rate at full steering, default 1.2 rad/s +--tau first order lag of the locomotion controller, default 0.2 s +--port UDP port to listen on, default 9002 +``` + +## Parameters + +`default.parm` holds the matching Rover parameters. Two of them are easy to get +wrong: + +* `ATC_STR_RAT_FF` is the steering fraction required for one radian per second, + so it is `1 / max turn rate`. Leaving it at the default while the model turns + at 1.2 rad/s under states the gain by a factor of four; the vehicle then yaws + back and forth about the target heading and makes almost no forward progress. +* `CRUISE_SPEED` and `CRUISE_THROTTLE` must describe the same vehicle as + `--max-speed`, otherwise the speed controller fights its own feed forward. + +## Notes for anyone writing a similar backend + +* The skid mixer in `AP_MotorsUGV` computes `motor_left = throttle + steering` + and `motor_right = throttle - steering`, so a positive turn rate means the + left motor runs faster. If this sign is inverted the vehicle settles pointing + 180 degrees away from the target and yaws forever, which is hard to recognise + as a sign error. +* ArduPilot sends 0 on servo channels it is not driving. Scaling that as if it + were a PWM value gives full reverse, so values outside a sane band are treated + as neutral. diff --git a/libraries/SITL/examples/JSON/python/velocity_controlled_robot.py b/libraries/SITL/examples/JSON/python/velocity_controlled_robot.py new file mode 100755 index 00000000000000..bbc81265e4af15 --- /dev/null +++ b/libraries/SITL/examples/JSON/python/velocity_controlled_robot.py @@ -0,0 +1,173 @@ +#!/usr/bin/env python3 +"""JSON backend for a vehicle that is commanded by velocity, not by joint angles. + +Many walking robots (and a lot of tracked/differential platforms) ship with their +own locomotion controller. ArduPilot does not drive their joints; it asks for a +forward speed and a turn rate, and the on-board controller works out how to +achieve it. This example models that class of vehicle so Rover's navigation and +mission code can be exercised without any physics engine. + +Contrast with ``../pybullet/walking_robot.py``, which drives twelve leg joints +directly from twelve servo outputs. + +Only the Python standard library is used. + +Usage: + python3 velocity_controlled_robot.py + ardurover --model JSON:127.0.0.1 --defaults default.parm + +See readme.md for the parameters Rover needs for this vehicle model. +""" + +import argparse +import json +import math +import socket +import struct + +MAGIC = 18458 +PACKET = struct.Struct(" PWM_MAX: + return 0.0 + return clamp((pwm - PWM_NEUTRAL) / 500.0, -1.0, 1.0) + + +def pwm_to_command(pwm, max_speed, max_turn_rate): + """Convert skid steering outputs to a forward speed and a turn rate. + + The skid mixer in AP_MotorsUGV computes:: + + motor_left = throttle + steering + motor_right = throttle - steering + + so a positive turn rate (clockwise, NED) means the left motor runs faster. + Getting this sign backwards is not obvious from the servo output alone: the + vehicle settles pointing 180 degrees away from the target and yaws forever. + """ + left = normalise(pwm[0]) # SERVO1_FUNCTION = 73, throttleLeft + right = normalise(pwm[2]) # SERVO3_FUNCTION = 74, throttleRight + speed = (left + right) * 0.5 * max_speed + turn_rate = (left - right) * 0.5 * max_turn_rate + return speed, turn_rate + + +class Vehicle(object): + """Planar unicycle with a first order lag on the commanded velocity. + + The lag stands in for however long the on-board locomotion controller takes + to reach a newly commanded velocity. + """ + + def __init__(self, tau): + self.tau = tau + self.reset() + + def reset(self): + self.timestamp = 0.0 + self.north = 0.0 + self.east = 0.0 + self.yaw = 0.0 + self.speed = 0.0 + self.turn_rate = 0.0 + self.accel_forward = 0.0 + + def update(self, speed_cmd, turn_rate_cmd, dt): + previous_speed = self.speed + gain = 1.0 if dt >= self.tau else dt / self.tau + self.speed += (speed_cmd - self.speed) * gain + self.turn_rate += (turn_rate_cmd - self.turn_rate) * gain + + self.accel_forward = (self.speed - previous_speed) / dt + self.yaw += self.turn_rate * dt + self.yaw = (self.yaw + math.pi) % (2.0 * math.pi) - math.pi + self.north += self.speed * math.cos(self.yaw) * dt + self.east += self.speed * math.sin(self.yaw) * dt + self.timestamp += dt + + def state(self): + return { + "timestamp": self.timestamp, + "imu": { + "gyro": [0.0, 0.0, self.turn_rate], + # x is forward acceleration, y is centripetal, z is the + # reaction to gravity for a vehicle sitting level. + "accel_body": [ + self.accel_forward, + self.speed * self.turn_rate, + -9.80665, + ], + }, + "position": [self.north, self.east, 0.0], + "attitude": [0.0, 0.0, self.yaw], + "velocity": [ + self.speed * math.cos(self.yaw), + self.speed * math.sin(self.yaw), + 0.0, + ], + } + + +def main(): + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--port", type=int, default=9002) + parser.add_argument("--max-speed", type=float, default=1.5, + help="forward speed at full throttle (m/s)") + parser.add_argument("--max-turn-rate", type=float, default=1.2, + help="turn rate at full steering (rad/s)") + parser.add_argument("--tau", type=float, default=0.2, + help="first order lag of the locomotion controller (s)") + args = parser.parse_args() + + vehicle = Vehicle(args.tau) + last_frame = None + reply = ("\n" + json.dumps(vehicle.state()) + "\n").encode() + + sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + sock.bind(("0.0.0.0", args.port)) + print("listening for ArduPilot on UDP %d" % args.port) + + while True: + data, address = sock.recvfrom(2048) + if len(data) != PACKET.size: + continue + values = PACKET.unpack(data) + if values[0] != MAGIC: + continue + frame_rate, frame_count, pwm = values[1], values[2], values[3:] + if frame_rate == 0: + continue + + # A frame count that goes backwards means SITL restarted. + if last_frame is not None and frame_count < last_frame: + print("ArduPilot restarted, resetting vehicle") + vehicle.reset() + last_frame = None + + # Repeated frame counts are retransmissions; do not advance time twice. + if frame_count != last_frame: + speed, turn_rate = pwm_to_command( + pwm, args.max_speed, args.max_turn_rate) + vehicle.update(speed, turn_rate, 1.0 / frame_rate) + reply = ("\n" + json.dumps(vehicle.state()) + "\n").encode() + last_frame = frame_count + + sock.sendto(reply, address) + + +if __name__ == "__main__": + main()