Merge pull request #245 from maxspahn/ft-generic-bicycle

Ft generic bicycle

examples/prius.py

@@ -1,11 +1,21 @@
 import gymnasium as gym
-from urdfenvs.robots.prius import Prius
 import numpy as np
 
+from urdfenvs.urdf_common.bicycle_model import BicycleModel
+
 
 def run_prius(n_steps=1000, render=False, goal=True, obstacles=True):
     robots = [
-        Prius(mode="vel"),
+        BicycleModel(
+            urdf='prius.urdf',
+            mode="vel",
+            scaling=0.3,
+            wheel_radius = 0.31265,
+            wheel_distance = 0.494,
+            spawn_offset = np.array([-0.435, 0.0, 0.05]),
+            actuated_wheels=['front_right_wheel_joint', 'front_left_wheel_joint', 'rear_right_wheel_joint', 'rear_left_wheel_joint'],
+            steering_links=['front_right_steer_joint', 'front_left_steer_joint'],
+        )
     ]
     env = gym.make(
         "urdf-env-v0",

pyproject.toml

@@ -1,6 +1,6 @@
 [tool.poetry]
 name = "urdfenvs"
-version = "0.8.18"
+version = "0.8.19"
 description = "Simple simulation environment for robots, based on the urdf files."
 authors = ["Max Spahn <[email protected]>"]
 maintainers = [

tests/test_acc_envs.py

@@ -110,10 +110,19 @@ def albertReacherEnv():
 
 @pytest.fixture
 def priusEnv():
-    from urdfenvs.robots.prius import Prius
+    from urdfenvs.urdf_common.bicycle_model import BicycleModel
     initPos = np.zeros(3)
     initVel = np.zeros(2)
-    robot = Prius(mode="acc")
+    robot = BicycleModel(
+        urdf='prius.urdf',
+        mode="acc",
+        scaling=0.3,
+        wheel_radius = 0.31265,
+        wheel_distance = 0.494,
+        spawn_offset = np.array([-0.435, 0.0, 0.05]),
+        actuated_wheels=['front_right_wheel_joint', 'front_left_wheel_joint', 'rear_right_wheel_joint', 'rear_left_wheel_joint'],
+        steering_links=['front_right_steer_joint', 'front_left_steer_joint'],
+    )
     return (robot, initPos, initVel)
 
 @pytest.fixture

tests/test_vel_envs.py

@@ -103,10 +103,19 @@ def albertReacherEnv():
 
 @pytest.fixture
 def priusEnv():
-    from urdfenvs.robots.prius import Prius
+    from urdfenvs.urdf_common.bicycle_model import BicycleModel
     initPos = np.zeros(3)
     initVel = np.zeros(2)
-    robot = Prius(mode="vel")
+    robot = BicycleModel(
+        urdf='prius.urdf',
+        mode="vel",
+        scaling=0.3,
+        wheel_radius = 0.31265,
+        wheel_distance = 0.494,
+        spawn_offset = np.array([-0.435, 0.0, 0.05]),
+        actuated_wheels=['front_right_wheel_joint', 'front_left_wheel_joint', 'rear_right_wheel_joint', 'rear_left_wheel_joint'],
+        steering_links=['front_right_steer_joint', 'front_left_steer_joint'],
+    )
     return (robot, initPos, initVel)
 
 @pytest.fixture

urdfenvs/robots/prius/prius.urdf → urdfenvs/assets/prius/prius.urdf

@@ -276,10 +276,6 @@
     <origin xyz="0 0.4 1.8" rpy="0 0 -1.5707"/>
   </joint>
 
-  <gazebo reference="base_link_connection">
-    <disableFixedJointLumping>true</disableFixedJointLumping>
-  </gazebo>
-
   <link name="front_camera_link">
     <inertial>
       <mass value="1"/>
@@ -292,123 +288,5 @@
     <origin xyz="0 -0.4 1.4" rpy="0 0.05 -1.5707"/>
   </joint>
 
-  <gazebo>
-    <plugin name="pruis_hybrid_drive" filename="libPriusHybridPlugin.so">
-      <chassis>chassis</chassis>
-      <front_left_wheel>front_left_wheel_joint</front_left_wheel>
-      <front_right_wheel>front_right_wheel_joint</front_right_wheel>
-      <front_left_wheel_steering>front_left_steer_joint</front_left_wheel_steering>
-      <front_right_wheel_steering>front_right_steer_joint</front_right_wheel_steering>
-      <back_left_wheel>rear_left_wheel_joint</back_left_wheel>
-      <back_right_wheel>rear_right_wheel_joint</back_right_wheel>
-      <steering_wheel>steering_joint</steering_wheel>
-      <chassis_aero_force_gain>0.63045</chassis_aero_force_gain>
-      <front_torque>859.4004393000001</front_torque>
-      <back_torque>0</back_torque>
-      <front_brake_torque>1031.28052716</front_brake_torque>
-      <back_brake_torque>687.5203514400001</back_brake_torque>
-      <max_speed>37.998337013956565</max_speed>
-      <min_gas_flow>8.981854013171626e-05</min_gas_flow>
-      <gas_efficiency>0.371</gas_efficiency>
-      <battery_charge_watt_hours>291</battery_charge_watt_hours>
-      <battery_discharge_watt_hours>214</battery_discharge_watt_hours>
-      <max_steer>0.6458</max_steer>
-      <flwheel_steering_p_gain>1e5</flwheel_steering_p_gain>
-      <frwheel_steering_p_gain>1e5</frwheel_steering_p_gain>
-      <flwheel_steering_i_gain>0</flwheel_steering_i_gain>
-      <frwheel_steering_i_gain>0</frwheel_steering_i_gain>
-      <flwheel_steering_d_gain>3e1</flwheel_steering_d_gain>
-      <frwheel_steering_d_gain>3e1</frwheel_steering_d_gain>
-    </plugin>
-  </gazebo>
-<!-- d gain was 3e2! -->
-  <gazebo>
-    <plugin name="joint_state_publisher" filename="libgazebo_ros_joint_state_publisher.so">
-      <!-- <robotNamespace>/prius</robotNamespace> -->
-      <jointName>rear_right_wheel_joint, rear_left_wheel_joint, front_right_wheel_joint, front_left_wheel_joint, front_right_steer_joint, front_left_steer_joint, steering_joint</jointName>
-      <updateRate>100.0</updateRate>
-      <alwaysOn>true</alwaysOn>
-    </plugin>
-  </gazebo>
-
-  <gazebo>
-    <plugin name="p3d" filename="libgazebo_ros_p3d.so">
-      <!-- <robotNamespace>/prius</robotNamespace> -->
-      <bodyName>base_link</bodyName>
-      <topicName>base_pose_ground_truth</topicName>
-      <frameName>map</frameName>
-      <updateRate>100.0</updateRate>
-    </plugin>
-  </gazebo>
-
-  <gazebo reference="center_laser_link">
-    <sensor name='center_laser_sensor' type='ray'>
-      <ray>
-        <scan>
-          <horizontal>
-            <samples>512</samples>
-            <resolution>1</resolution>
-            <min_angle>-3.14</min_angle>
-            <max_angle>3.14</max_angle>
-          </horizontal>
-          <vertical>
-            <samples>16</samples>
-            <min_angle>-0.1</min_angle>
-            <max_angle>-0.35</max_angle>
-          </vertical>
-        </scan>
-        <range>
-          <min>0.2</min>
-          <max>30</max>
-          <resolution>0.01</resolution>
-        </range>
-      </ray>
-      <plugin name='center_laser' filename='libgazebo_ros_block_laser.so'>
-        <topicName>/prius/center_laser/scan</topicName>
-        <frameName>center_laser_link</frameName>
-        <gaussianNoise>0.01</gaussianNoise>
-      </plugin>
-      <always_on>1</always_on>
-      <update_rate>30</update_rate>
-      <visualize>0</visualize>
-    </sensor>
-  </gazebo>
-
-  <gazebo reference="front_camera_link">
-    <sensor type="camera" name="front_camera_sensor">
-      <update_rate>30.0</update_rate>
-      <camera name="front_camera">
-        <horizontal_fov>1.3962634</horizontal_fov>
-        <image>
-          <width>800</width>
-          <height>800</height>
-          <format>R8G8B8</format>
-        </image>
-        <clip>
-          <near>0.02</near>
-          <far>300</far>
-        </clip>
-        <noise>
-          <type>gaussian</type>
-          <mean>0.0</mean>
-          <stddev>0.007</stddev>
-        </noise>
-      </camera>
-      <plugin name="front_camera_controller" filename="libgazebo_ros_camera.so">
-        <alwaysOn>true</alwaysOn>
-        <updateRate>0.0</updateRate>
-        <cameraName>/prius/front_camera</cameraName>
-        <imageTopicName>image_raw</imageTopicName>
-        <cameraInfoTopicName>/prius/front_camera_info</cameraInfoTopicName>
-        <frameName>camera_link</frameName>
-        <hackBaseline>0.07</hackBaseline>
-        <distortionK1>0.0</distortionK1>
-        <distortionK2>0.0</distortionK2>
-        <distortionK3>0.0</distortionK3>
-        <distortionT1>0.0</distortionT1>
-        <distortionT2>0.0</distortionT2>
-      </plugin>
-    </sensor>
-  </gazebo>
 
 </robot>