Skip to content

Commit

Permalink
Merge pull request #53 from WBK-Robotics/agv_support
Browse files Browse the repository at this point in the history 
added simplistic Agv support
  • Loading branch information
@liquidcronos
liquidcronos authored Sep 3, 2024
2 parents fc4d1f4 + 9214191 commit 001da11
Show file tree
Hide file tree
Showing 5 changed files with 604 additions and 0 deletions.
81 changes: 81 additions & 0 deletions examples/agv_robot.py
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,81 @@
"""This is an example for an AGV following a set path using a custom controller."""
import pybullet as p
import os


import pybullet_industrial as pi
import pybullet_data
import time
import os

def trajectory_follower_controller(distance,angle,target_angle_error):
""" A simple trajectory following controller that neglects the target_angle_error and simply
orients the robot towards the current target position.
The controller is a simple proportional controller that
takes the distance and angle to the target
and returns a linear and angular velocity command to the robot.
"""
kp_lin=1
kp_ang=1

linear_velocity = kp_lin*distance,
angular_velocity = -1*kp_ang*angle,

return [linear_velocity[0], angular_velocity[0]]

if __name__ == "__main__":

pysics_client = p.connect(p.GUI, options='--background_color_red=1 ' +
'--background_color_green=1 ' +
'--background_color_blue=1')

p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.loadURDF("plane.urdf")
p.setGravity(0, 0, -10)

diff_drive_params = {"wheel_radius": 0.2,
"track_width": 0.3,
"max_linear_velocity": 0.8,
"max_angular_velocity": 0.8}
dirname = os.path.dirname(__file__)
urdf_file = os.path.join(dirname,
'robot_descriptions', 'diff_drive_agv.urdf')

agv = pi.DiffDriveAGV(urdf_file, [0, 0, 0.3], [0, 0, 0, 1],
"left_wheel_joint",
"right_wheel_joint",
diff_drive_params,
position_controller=trajectory_follower_controller)

agv.set_world_state([2.25,0,1], [0,0,0,1])


test_path = pi.build_box_path(
[0,0,0], [4.5, 6.6], 0.8, [0, 0, 0, 1], 200)

test_path.draw()

#spawn a sphere mulitbody that highlights the target position
sphere_visual = p.createVisualShape(p.GEOM_SPHERE,
radius=0.1,
rgbaColor=[1, 0, 0, 1])

sphere = p.createMultiBody(baseMass=0,
baseVisualShapeIndex=sphere_visual,
basePosition=[0, 0, 0.1])

while True:
positions,orientations = [0,0,0.1],[0,0,0,1]
for positions, orientations, _ in test_path:
p.resetBasePositionAndOrientation(sphere, positions, orientations)
agv.set_target_pose(positions,orientations)
for _ in range(50):
agv.update_position_loop()
p.stepSimulation()

actual_pos, actual_ori = agv.get_world_state()

time.sleep(0.01)



99 changes: 99 additions & 0 deletions examples/robot_descriptions/diff_drive_agv.urdf
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,99 @@
<?xml version="1.0"?>
<robot name="simple_diff_drive_agv">

<!-- Base Link -->
<link name="base_link">
<visual>
<geometry>
<box size="0.5 0.3 0.2"/> <!-- dimensions of the base -->
</geometry>
</visual>
<collision>
<geometry>
<box size="0.5 0.3 0.2"/>
</geometry>
</collision>
</link>

<!-- Left Wheel -->
<link name="left_wheel">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<damping value="1000"/>
</contact>
<visual>
<geometry>
<cylinder radius="0.2" length="0.05"/> <!-- wheel dimensions -->
</geometry>
</visual>
<collision>
<geometry>
<cylinder radius="0.2" length="0.05"/>
</geometry>
</collision>
</link>

<!-- Right Wheel -->
<link name="right_wheel">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<damping value="1000"/>
</contact>
<visual>
<geometry>
<cylinder radius="0.2" length="0.05"/>
</geometry>
</visual>
<collision>
<geometry>
<cylinder radius="0.2" length="0.05"/>
</geometry>
</collision>
</link>

<!-- Caster Wheel -->
<link name="caster_wheel">
<contact>
<lateral_friction value="0.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<damping value="1000"/>
</contact>
<visual>
<geometry>
<sphere radius="0.05"/> <!-- caster wheel size -->
</geometry>
</visual>
<collision>
<geometry>
<sphere radius="0.05"/>
</geometry>
</collision>
</link>

<!-- Joints -->
<joint name="left_wheel_joint" type="continuous">
<parent link="base_link"/>
<child link="left_wheel"/>
<origin xyz="0.2 -0.15 0" rpy="-1.57079632679 0 0"/> <!-- position and orientation -->
<axis xyz="0 0 1"/> <!-- rotation axis -->
</joint>

<joint name="right_wheel_joint" type="continuous">
<parent link="base_link"/>
<child link="right_wheel"/>
<origin xyz="0.2 0.15 0" rpy="-1.57079632679 0 0"/>
<axis xyz="0 0 1"/>
</joint>

<joint name="caster_wheel_joint" type="fixed">
<parent link="base_link"/>
<child link="caster_wheel"/>
<origin xyz="-0.2 0 -0.1" rpy="0 0 0"/> <!-- rear-mounted caster wheel -->
</joint>

</robot>
2 changes: 2 additions & 0 deletions src/pybullet_industrial/__init__.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,4 +12,6 @@
from pybullet_industrial.path_builders import *
from pybullet_industrial.milling_tool import *
from pybullet_industrial.g_code_processor import *
from pybullet_industrial.agv_robots import *
from pybullet_industrial.g_code_logger import *

Loading

0 comments on commit 001da11

Please sign in to comment.