Sempre nella sperimentazione di robot senza avere un robot fisico ho trovato interessante la libreria Bullet3 che effettua simulazioni fisiche
In particolare e' possibile caricare un file .urdf che descrive fisicamente il robot...in particolare Thor, un robot opensurce il cui progetto si trova qui http://thor.angel-lm.com/ mentre il file urdf e le meshes sono
La libreria Bullet3 e' interessante perche' risolve anche i problemi di cinematica inversa ma e' documentata in maniera molto scarsa (il programma di test sottostante e' stato scritto da Claude.ai ma non sono riusciuto a scrivere da solo un programma per un doppio pendolo caotico)
import pybullet as p
import pybullet_data
import time
import math
import numpy as np
class ThorArmSimulation:
def __init__(self, urdf_path="thor_robot.urdf"):
"""Initialize PyBullet simulation with THOR arm"""
# Connect to PyBullet
self.physics_client = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
# Set up the simulation
p.setGravity(0, 0, -9.81)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 1)
p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS, 1)
# Load ground plane
self.plane_id = p.loadURDF("plane.urdf")
# Load THOR robot arm
self.robot_id = p.loadURDF(urdf_path,
basePosition=[0, 0, 0],
baseOrientation=p.getQuaternionFromEuler([0, 0, 0]),
useFixedBase=True)
# Get joint information
self.num_joints = p.getNumJoints(self.robot_id)
self.joint_indices = []
self.joint_names = []
self.joint_limits = []
print(f"\nTHOR Robot Arm loaded with {self.num_joints} joints:")
print("-" * 60)
for i in range(self.num_joints):
joint_info = p.getJointInfo(self.robot_id, i)
joint_name = joint_info[1].decode('utf-8')
joint_type = joint_info[2]
if joint_type == p.JOINT_REVOLUTE:
self.joint_indices.append(i)
self.joint_names.append(joint_name)
lower_limit = joint_info[8]
upper_limit = joint_info[9]
self.joint_limits.append((lower_limit, upper_limit))
print(f"Joint {i}: {joint_name}")
print(f" Limits: [{lower_limit:.3f}, {upper_limit:.3f}] rad")
print(f" Limits: [{math.degrees(lower_limit):.1f}°, {math.degrees(upper_limit):.1f}°]")
print("-" * 60)
print(f"Active joints: {len(self.joint_indices)}\n")
# Set camera
p.resetDebugVisualizerCamera(
cameraDistance=0.8,
cameraYaw=45,
cameraPitch=-30,
cameraTargetPosition=[0, 0, 0.2]
)
# Enable joint control
self.setup_joint_control()
def setup_joint_control(self):
"""Set up position control for all joints"""
for i in self.joint_indices:
p.setJointMotorControl2(
self.robot_id,
i,
p.POSITION_CONTROL,
targetPosition=0,
force=100
)
def set_joint_positions(self, positions):
"""Set target positions for all joints"""
for i, pos in enumerate(positions):
if i < len(self.joint_indices):
p.setJointMotorControl2(
self.robot_id,
self.joint_indices[i],
p.POSITION_CONTROL,
targetPosition=pos,
force=100,
maxVelocity=1.0
)
def get_joint_positions(self):
"""Get current joint positions"""
positions = []
for i in self.joint_indices:
state = p.getJointState(self.robot_id, i)
positions.append(state[0])
return positions
def get_end_effector_pose(self):
"""Get end effector position and orientation"""
# Get the last link state
link_state = p.getLinkState(self.robot_id, self.num_joints - 1)
position = link_state[4] # world position
orientation = link_state[5] # world orientation
return position, orientation
def animation_wave(self, t):
"""Wave animation - moves joints in a wave pattern"""
positions = [
0.8 * math.sin(t), # art1_yaw
0.5 * math.sin(t + 0.5), # art2_pitch
0.8 * math.sin(t + 1.0), # art3_pitch
1.5 * math.sin(t + 1.5), # art4_roll
0.6 * math.sin(t + 2.0), # art5_pitch
2.0 * math.sin(t + 2.5), # art6_roll
]
return positions
def animation_reach(self, t):
"""Reaching animation - simulates reaching forward and back"""
phase = (math.sin(t * 0.5) + 1) / 2 # 0 to 1
positions = [
0.0, # art1_yaw - straight ahead
-0.5 + phase * 1.0, # art2_pitch - shoulder
-1.0 + phase * 2.0, # art3_pitch - elbow
0.0, # art4_roll
0.5 - phase * 1.0, # art5_pitch - wrist
0.0, # art6_roll
]
return positions
def animation_circle(self, t):
"""Circle drawing animation"""
positions = [
0.0, # Keep base stationary
-0.3 + 0.3 * math.sin(t), # Shoulder pitch
-0.6 + 0.3 * math.cos(t), # Elbow pitch
0.0,
0.2,
t, # End effector rotation
]
return positions
def animation_full_rotation(self, t):
"""Full rotation test - tests joint limits"""
speed = 0.5
positions = [
3.0 * math.sin(t * speed), # art1_yaw - full rotation
1.2 * math.sin(t * speed + 1), # art2_pitch
1.2 * math.sin(t * speed + 2), # art3_pitch
3.0 * math.sin(t * speed + 3), # art4_roll - full rotation
1.2 * math.sin(t * speed + 4), # art5_pitch
3.0 * math.sin(t * speed + 5), # art6_roll - full rotation
]
return positions
def animation_pick_place(self, t):
"""Pick and place animation"""
cycle_time = 8.0
phase = (t % cycle_time) / cycle_time
if phase < 0.25: # Move to pick position
progress = phase / 0.25
positions = [
0.3 * progress,
-0.8 * progress,
-1.2 * progress,
0.0,
0.5 * progress,
0.0
]
elif phase < 0.5: # Grasp and lift
progress = (phase - 0.25) / 0.25
positions = [
0.3,
-0.8 + 0.6 * progress,
-1.2 + 0.8 * progress,
0.0,
0.5 - 0.3 * progress,
math.pi * progress
]
elif phase < 0.75: # Move to place position
progress = (phase - 0.5) / 0.25
positions = [
0.3 - 0.6 * progress,
-0.2 - 0.4 * progress,
-0.4 - 0.6 * progress,
0.0,
0.2 + 0.3 * progress,
math.pi
]
else: # Return to home
progress = (phase - 0.75) / 0.25
positions = [
-0.3 + 0.3 * progress,
-0.6 + 0.6 * progress,
-1.0 + 1.0 * progress,
0.0,
0.5 - 0.5 * progress,
math.pi - math.pi * progress
]
return positions
def run_animation(self, animation_type="wave", duration=20.0):
"""Run a specific animation"""
print(f"\nRunning '{animation_type}' animation for {duration}s...")
print("Press Ctrl+C to stop\n")
animations = {
"wave": self.animation_wave,
"reach": self.animation_reach,
"circle": self.animation_circle,
"rotation": self.animation_full_rotation,
"pick_place": self.animation_pick_place,
}
if animation_type not in animations:
print(f"Unknown animation: {animation_type}")
print(f"Available: {list(animations.keys())}")
return
animation_func = animations[animation_type]
start_time = time.time()
try:
while time.time() - start_time < duration:
t = time.time() - start_time
# Get target positions from animation
positions = animation_func(t)
# Clamp to joint limits
clamped_positions = []
for i, pos in enumerate(positions):
if i < len(self.joint_limits):
lower, upper = self.joint_limits[i]
clamped_pos = max(lower, min(upper, pos))
clamped_positions.append(clamped_pos)
# Set joint positions
self.set_joint_positions(clamped_positions)
# Step simulation
p.stepSimulation()
time.sleep(1./240.)
# Print status every 2 seconds
if int(t) % 2 == 0 and t - int(t) < 0.01:
ee_pos, ee_orn = self.get_end_effector_pose()
print(f"Time: {t:.1f}s | End Effector: [{ee_pos[0]:.3f}, {ee_pos[1]:.3f}, {ee_pos[2]:.3f}]")
except KeyboardInterrupt:
print("\nAnimation stopped by user")
def run_interactive(self):
"""Run interactive mode with joint sliders"""
print("\nInteractive Mode - Use sliders to control joints")
# Create sliders for each joint
sliders = []
for i, (name, limits) in enumerate(zip(self.joint_names, self.joint_limits)):
slider = p.addUserDebugParameter(
name,
limits[0],
limits[1],
0.0
)
sliders.append(slider)
print("Control the robot using the sliders. Press Ctrl+C to exit.\n")
try:
while True:
# Read slider values
positions = [p.readUserDebugParameter(slider) for slider in sliders]
# Set joint positions
self.set_joint_positions(positions)
# Step simulation
p.stepSimulation()
time.sleep(1./240.)
except KeyboardInterrupt:
print("\nInteractive mode stopped")
def disconnect(self):
"""Disconnect from PyBullet"""
p.disconnect()
def main():
"""Main function to run the simulation"""
import sys
# Check if URDF path is provided
urdf_path = "thor_robot.urdf"
if len(sys.argv) > 1:
urdf_path = sys.argv[1]
print("=" * 60)
print("THOR Robot Arm - PyBullet Visualization")
print("=" * 60)
# Create simulation
sim = ThorArmSimulation(urdf_path)
# Menu
print("\nAvailable animations:")
print("1. Wave - Sinusoidal wave motion")
print("2. Reach - Forward reaching motion")
print("3. Circle - Draw a circle")
print("4. Rotation - Full joint rotation test")
print("5. Pick/Place - Pick and place sequence")
print("6. Interactive - Manual control with sliders")
print("7. All - Run all animations sequentially")
choice = input("\nSelect animation (1-7) [default: 1]: ").strip() or "1"
try:
if choice == "1":
sim.run_animation("wave", duration=20)
elif choice == "2":
sim.run_animation("reach", duration=20)
elif choice == "3":
sim.run_animation("circle", duration=20)
elif choice == "4":
sim.run_animation("rotation", duration=20)
elif choice == "5":
sim.run_animation("pick_place", duration=20)
elif choice == "6":
sim.run_interactive()
elif choice == "7":
for anim in ["wave", "reach", "circle", "rotation", "pick_place"]:
sim.run_animation(anim, duration=10)
else:
print("Invalid choice, running wave animation")
sim.run_animation("wave", duration=20)
finally:
print("\nClosing simulation...")
sim.disconnect()
if __name__ == "__main__":
main()
