Adds python demo examples (gazebosim#2044)

This PR creates the python version of a couple of examples. It is basically the same code as the C++ version but using gz-transport's python bindings. --------- Signed-off-by: Voldivh <[email protected]> Co-authored-by: Addisu Z. Taddese <[email protected]>
rudislabs · Aug 31, 2023 · 92158db · 92158db
1 parent a84d66a
commit 92158db
Show file tree

Hide file tree

Showing 2 changed files with 298 additions and 0 deletions.
diff --git a/examples/standalone/lrauv_control/lrauv_control.py b/examples/standalone/lrauv_control/lrauv_control.py
@@ -0,0 +1,202 @@
+#
+# Copyright (C) 2023 Open Source Robotics Foundation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+#
+
+#
+# Check README for detailed instructions.
+# Note: You need to update the PYTHONPATH variable if it is not set
+# before for other python examples. You can use then following:
+# $ export PYTHONPATH=$PYTHONPATH:<path_to_gazebo_ws>/install/lib/python
+# Usage:
+#   $ gz sim -r worlds/lrauv_control_demo.sdf
+#   $ # python3 lrauv_control.py speed_m_s yaw_rad pitch_rad
+#   $ python3 lrauv_control.py 0.5 0.78 0.174
+#
+
+from gz.msgs10.double_pb2 import Double
+from gz.msgs10.odometry_pb2 import Odometry
+from gz.math7 import Quaterniond, Vector3d
+from gz.transport13 import Node
+
+from threading import Lock
+
+import math
+import sys
+import time
+
+
+# Helper class for the PID controller for
+# linear speed, pitch and yaw angles.
+class Controller:
+    def __init__(self):
+        # Mutex to synchronize internal variables.
+        self.controllerMutex = Lock()
+
+        # Desired state.
+        self.targetSpeed = 0
+        self.targetYawAngle = 0
+        self.targetPitchAngle = 0
+
+        # Errors
+        self.errorSpeed = 0
+        self.errorSpeedIntegral = 0
+        self.errorYawAngle = 0
+        self.errorPitchAngle = 0
+
+        # States to be tracked and controlled.
+        self.speed = 0
+        self.yawAngle = 0
+        self.pitchAngle = 0
+
+        # PID gains and error limits.
+        # PI for speed.
+        self.kSpeed = -30
+        self.kISpeed = -0.5
+        self.errorSpeedIntegralMax = 10
+
+        # P for yaw and pitch control.
+        self.kYawAngle = -0.5
+        self.kPitchAngle = 0.6
+
+    # Set the target states to be tracked,
+    # i.e. linear speed (m/s), pitch and yaw angles (rad).
+    def SetTargets(self, _speed, _yaw, _pitch):
+        with self.controllerMutex:
+            if _speed == 0 and (_yaw != 0 or _pitch != 0):
+                print("Speed needs to be non zero for non zero pitch and yaw angles")
+                return
+            self.targetSpeed = float(_speed)
+            self.targetYawAngle = float(_yaw)
+            self.targetPitchAngle = float(_pitch)
+
+    # Update the state of the vehicle.
+    def UpdateState(self, _speed, _yaw, _pitch):
+        with self.controllerMutex:
+            self.speed = _speed
+            self.yawAngle = _yaw
+            self.pitchAngle = _pitch
+
+            self.errorSpeed = self.targetSpeed - self.speed
+            self.errorSpeedIntegral = min(
+                self.errorSpeedIntegral + self.errorSpeed, self.errorSpeedIntegralMax
+            )
+            self.errorYawAngle = self.targetYawAngle - self.yawAngle
+            self.errorPitchAngle = self.targetPitchAngle - self.pitchAngle
+
+    # Generate control input to be applied to the thruster.
+    def SpeedControl(self):
+        return self.errorSpeed * self.kSpeed + self.errorSpeedIntegral * self.kISpeed
+
+    # Generate control input to be supplied to the yaw rudders.
+    def YawControl(self):
+        return self.errorYawAngle * self.kYawAngle
+
+    # Generate control input to be supplied to the pitch rudders.
+    def PitchControl(self):
+        return self.errorPitchAngle * self.kPitchAngle
+
+
+def main():
+    control = Controller()
+    argc = len(sys.argv)
+    if argc == 4:
+        targetSpeed = sys.argv[1]
+        targetYaw = sys.argv[2]
+        targetPitch = sys.argv[3]
+
+        # Target state : speed (m/s), yaw angle, pitch angle (rad).
+        control.SetTargets(targetSpeed, targetYaw, targetPitch)
+
+    node = Node()
+
+    # Propeller command publisher.
+    propellerTopicTethys = "/model/tethys/joint/propeller_joint/cmd_thrust"
+    propellerPubTethys = node.advertise(propellerTopicTethys, Double)
+
+    # Subscriber for vehicle pose.
+    def cbPos(odometry_msg: Odometry):
+        orientation = odometry_msg.pose.orientation
+
+        q = Quaterniond(orientation.w, orientation.x, orientation.y, orientation.z)
+
+        # Get the velocity of the vehicle.
+        velocity = Vector3d(
+            odometry_msg.twist.linear.x,
+            odometry_msg.twist.linear.y,
+            odometry_msg.twist.linear.z,
+        )
+
+        control.UpdateState(velocity.length(), q.yaw(), q.pitch())
+
+    node.subscribe(Odometry, "/model/tethys/odometry", cbPos)
+
+    # Rudder command publisher.
+    rudderTopicTethys = "/model/tethys/joint/vertical_fins_joint/0/cmd_pos"
+    rudderPubTethys = node.advertise(rudderTopicTethys, Double)
+
+    # Fin command publisher.
+    finTopicTethys = "/model/tethys/joint/horizontal_fins_joint/0/cmd_pos"
+    finPubTethys = node.advertise(finTopicTethys, Double)
+
+    while True:
+        with control.controllerMutex:
+            # Publish propeller command for speed.
+            propellerMsg = Double()
+            propellerMsg.data = control.SpeedControl()
+            propellerPubTethys.publish(propellerMsg)
+
+            # Publish rudder command for yaw.
+            rudderMsg = Double()
+            rudderMsg.data = control.YawControl()
+            rudderPubTethys.publish(rudderMsg)
+
+            # Publish fin command for pitch.
+            finMsg = Double()
+            finMsg.data = control.PitchControl()
+            finPubTethys.publish(finMsg)
+
+            # Print the states.
+            print("-----------------------")
+            print("States            ( target, current, error) : ")
+            print(
+                "Speed (m/s)       :  ",
+                str(round(control.targetSpeed, 2)),
+                "  ",
+                str(round(float(control.speed), 2)),
+                "  ",
+                str(round(float(control.errorSpeed), 2)),
+            )
+            print(
+                "Yaw angle (deg)   : ",
+                str(round(control.targetYawAngle * 180 / math.pi, 2)),
+                " ",
+                str(round(control.yawAngle * 180 / math.pi, 2)),
+                "  ",
+                str(round(control.errorYawAngle * 180 / math.pi, 2)),
+            )
+            print(
+                "Pitch angle (deg) : ",
+                str(round(control.targetPitchAngle * 180 / math.pi, 2)),
+                " ",
+                str(round(control.pitchAngle * 180 / math.pi, 2)),
+                "  ",
+                str(round(control.errorPitchAngle * 180 / math.pi, 2)),
+            )
+        time.sleep(0.2)
+
+
+if __name__ == "__main__":
+    main()
diff --git a/examples/standalone/multi_lrauv_race/multi_lrauv_race.py b/examples/standalone/multi_lrauv_race/multi_lrauv_race.py
@@ -0,0 +1,96 @@
+#
+# Copyright (C) 2023 Open Source Robotics Foundation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+#
+#
+# In each iteration, for each vehicle, generate a random fin angle and thrust
+# within reasonable limits, and send the command to the vehicle.
+#
+# Usage:
+#   $ gz sim -r worlds/multi_lrauv_race.sdf
+#   $ python3 multi_lrauv_race.py
+#
+# Note: You need to update the PYTHONPATH variable if it is not set
+# before for other python examples. You can use then following:
+# $ export PYTHONPATH=$PYTHONPATH:<path_to_gazebo_ws>/install/lib/python
+
+from gz.msgs10.double_pb2 import Double
+from gz.transport13 import Node
+
+import random
+import time
+
+
+# Find joint limits from tethys model.sdf
+def random_angle_within_limits(min=-0.261799, max=0.261799):
+    return random.uniform(min, max)
+
+
+# Nominal speed is thruster 300 rpm ~ 31.4 radians per second ~ 6.14 Newtons
+def random_thrust_within_limits(min=-6.14, max=6.14):
+    return random.uniform(min, max)
+
+
+def main():
+    # Set up node
+    node = Node()
+
+    # Set up publishers
+    ns = ["tethys", "triton", "daphne"]
+    rudder_pubs = [
+        node.advertise(
+            "/model/" + name + "/joint/vertical_fins_joint/0/cmd_pos", Double
+        )
+        for name in ns
+    ]
+    propeller_pubs = [
+        node.advertise("/model/" + name + "/joint/propeller_joint/cmd_thrust", Double)
+        for name in ns
+    ]
+
+    artificial_speedup = 1
+
+    # Set up messages
+    rudder_msg = Double()
+    propeller_msg = Double()
+
+    try:
+        while True:
+            for rudder_pub, propelled_pub, name in zip(rudder_pubs, propeller_pubs, ns):
+                rudder_msg.data = random_angle_within_limits(-0.01, 0.01)
+                rudder_pub.publish(rudder_msg)
+                propeller_msg.data = random_thrust_within_limits(
+                    -6.14 * artificial_speedup, 0
+                )
+                propelled_pub.publish(propeller_msg)
+                print(
+                    "Commanding: "
+                    + name
+                    + " ruddder angle "
+                    + str(round(rudder_msg.data, 4))
+                    + " rad, thrust "
+                    + str(round(propeller_msg.data, 2))
+                    + " Newtons."
+                )
+            print(
+                "----------------------------------------------------------------------"
+            )
+            time.sleep(0.5)
+    except KeyboardInterrupt:
+        print("\nProcess terminated")
+
+
+if __name__ == "__main__":
+    main()