Alla fine, usando AI,sono arrivato ad avere una versione funzionante del pendolo doppio con PyBullet. In questo caso non si devono scrivere le equazioni di moto .... ci pensa il motore fisico una volta impostato il modello e le condizioni iniziali
Alla fine del post lo stesso pendolo doppio risolto pero' in modo analitico con le equazioni di moto del sistema
<?xml version="1.0"?>
<robot name="double_pendulum_2d">
<!-- Base link (fixed to world) -->
<link name="base">
<visual>
<geometry>
<sphere radius="0.05"/>
</geometry>
<material name="gray">
<color rgba="0.5 0.5 0.5 1"/>
</material>
</visual>
<collision>
<geometry>
<sphere radius="0.05"/>
</geometry>
</collision>
<inertial>
<mass value="0.001"/>
<inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
</inertial>
</link>
<!-- First rod -->
<link name="rod1">
<visual>
<origin xyz="0 0 -0.25" rpy="0 0 0"/>
<geometry>
<box size="0.04 0.01 0.5"/>
</geometry>
<material name="red">
<color rgba="0.8 0.2 0.2 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 -0.25" rpy="0 0 0"/>
<geometry>
<box size="0.04 0.01 0.5"/>
</geometry>
</collision>
<inertial>
<origin xyz="0 0 -0.25" rpy="0 0 0"/>
<mass value="0.1"/>
<inertia ixx="0.002" ixy="0" ixz="0" iyy="0.002" iyz="0" izz="0.00001"/>
</inertial>
</link>
<!-- First bob -->
<link name="bob1">
<visual>
<geometry>
<sphere radius="0.08"/>
</geometry>
<material name="bright_red">
<color rgba="1 0 0 1"/>
</material>
</visual>
<collision>
<geometry>
<sphere radius="0.08"/>
</geometry>
</collision>
<inertial>
<mass value="0.5"/>
<inertia ixx="0.00128" ixy="0" ixz="0" iyy="0.00128" iyz="0" izz="0.00128"/>
</inertial>
</link>
<!-- Second rod -->
<link name="rod2">
<visual>
<origin xyz="0 0 -0.25" rpy="0 0 0"/>
<geometry>
<box size="0.04 0.01 0.5"/>
</geometry>
<material name="blue">
<color rgba="0.2 0.2 0.8 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 -0.25" rpy="0 0 0"/>
<geometry>
<box size="0.04 0.01 0.5"/>
</geometry>
</collision>
<inertial>
<origin xyz="0 0 -0.25" rpy="0 0 0"/>
<mass value="0.1"/>
<inertia ixx="0.002" ixy="0" ixz="0" iyy="0.002" iyz="0" izz="0.00001"/>
</inertial>
</link>
<!-- Second bob -->
<link name="bob2">
<visual>
<geometry>
<sphere radius="0.08"/>
</geometry>
<material name="bright_blue">
<color rgba="0 0 1 1"/>
</material>
</visual>
<collision>
<geometry>
<sphere radius="0.08"/>
</geometry>
</collision>
<inertial>
<mass value="0.5"/>
<inertia ixx="0.00128" ixy="0" ixz="0" iyy="0.00128" iyz="0" izz="0.00128"/>
</inertial>
</link>
<!-- Joint 1: Base to Rod1 (revolute around Y-axis for XZ plane motion) -->
<joint name="joint1" type="continuous">
<parent link="base"/>
<child link="rod1"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.000" friction="0.0"/>
</joint>
<!-- Fixed joint: Rod1 to Bob1 -->
<joint name="rod1_to_bob1" type="fixed">
<parent link="rod1"/>
<child link="bob1"/>
<origin xyz="0 0 -0.5" rpy="0 0 0"/>
</joint>
<!-- Joint 2: Bob1 to Rod2 (revolute around Y-axis for XZ plane motion) -->
<joint name="joint2" type="continuous">
<parent link="bob1"/>
<child link="rod2"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.000" friction="0.0"/>
</joint>
<!-- Fixed joint: Rod2 to Bob2 -->
<joint name="rod2_to_bob2" type="fixed">
<parent link="rod2"/>
<child link="bob2"/>
<origin xyz="0 0 -0.5" rpy="0 0 0"/>
</joint>
</robot>
import pybullet as p
import pybullet_data
import numpy as np
import time
import os
import xml.etree.ElementTree as ET
def parse_urdf(urdf_path):
"""Parse URDF file and extract relevant parameters"""
tree = ET.parse(urdf_path)
root = tree.getroot()
params = {
'links': {},
'joints': {},
'robot_name': root.get('name', 'unknown')
}
# Parse links
for link in root.findall('link'):
link_name = link.get('name')
params['links'][link_name] = {}
# Get mass
inertial = link.find('inertial')
if inertial is not None:
mass_elem = inertial.find('mass')
if mass_elem is not None:
params['links'][link_name]['mass'] = float(mass_elem.get('value'))
# Get visual geometry
visual = link.find('visual')
if visual is not None:
geom = visual.find('geometry')
if geom is not None:
# Check for sphere
sphere = geom.find('sphere')
if sphere is not None:
params['links'][link_name]['geometry'] = {
'type': 'sphere',
'radius': float(sphere.get('radius'))
}
# Check for cylinder
cylinder = geom.find('cylinder')
if cylinder is not None:
params['links'][link_name]['geometry'] = {
'type': 'cylinder',
'radius': float(cylinder.get('radius')),
'length': float(cylinder.get('length'))
}
# Get color
material = visual.find('material')
if material is not None:
color = material.find('color')
if color is not None:
rgba_str = color.get('rgba')
params['links'][link_name]['color'] = [float(x) for x in rgba_str.split()]
# Parse joints
for joint in root.findall('joint'):
joint_name = joint.get('name')
params['joints'][joint_name] = {
'type': joint.get('type'),
'parent': joint.find('parent').get('link'),
'child': joint.find('child').get('link')
}
# Get axis
axis = joint.find('axis')
if axis is not None:
xyz_str = axis.get('xyz')
params['joints'][joint_name]['axis'] = [float(x) for x in xyz_str.split()]
# Get dynamics
dynamics = joint.find('dynamics')
if dynamics is not None:
params['joints'][joint_name]['damping'] = float(dynamics.get('damping', 0))
params['joints'][joint_name]['friction'] = float(dynamics.get('friction', 0))
return params
# Initialize PyBullet
physics_client = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, -9.81)
# Camera setup for 2D side view (looking at XZ plane)
p.resetDebugVisualizerCamera(
cameraDistance=3.0,
cameraYaw=90, # Look from the side
cameraPitch=0, # Straight horizontal view
cameraTargetPosition=[0.5, 1.3, 0.5]
)
# Disable rendering during setup for speed
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 1)
# Add a ground plane for reference
p.loadURDF("plane.urdf", basePosition=[0, 0, -1])
# Load and parse the URDF file
urdf_path = "double_pendulum_2d.urdf"
if not os.path.exists(urdf_path):
print(f"ERROR: URDF file not found at {urdf_path}")
print("Please make sure 'double_pendulum.urdf' is in the same directory as this script")
p.disconnect()
exit(1)
# Parse URDF to get parameters
print("Parsing URDF file...")
urdf_params = parse_urdf(urdf_path)
print(f"\nRobot: {urdf_params['robot_name']}")
print(f"Found {len(urdf_params['links'])} links:")
for link_name, link_data in urdf_params['links'].items():
print(f" - {link_name}:", end=" ")
if 'mass' in link_data:
print(f"mass={link_data['mass']}kg", end=" ")
if 'geometry' in link_data:
geom = link_data['geometry']
if geom['type'] == 'sphere':
print(f"[sphere r={geom['radius']}m]", end=" ")
elif geom['type'] == 'cylinder':
print(f"[cylinder r={geom['radius']}m l={geom['length']}m]", end=" ")
print()
print(f"\nFound {len(urdf_params['joints'])} joints:")
for joint_name, joint_data in urdf_params['joints'].items():
print(f" - {joint_name}: {joint_data['type']}")
print(f" {joint_data['parent']} -> {joint_data['child']}")
if 'damping' in joint_data:
print(f" damping={joint_data['damping']}, friction={joint_data['friction']}")
# Load the double pendulum
base_position = [0, 0, 1]
pendulum_id = p.loadURDF(
urdf_path,
basePosition=base_position,
useFixedBase=True,
flags=p.URDF_USE_INERTIA_FROM_FILE
)
print(f"\nLoaded URDF with {p.getNumJoints(pendulum_id)} joints in PyBullet")
#sblocca i motori
for i in range(p.getNumJoints(pendulum_id)):
info = p.getJointInfo(pendulum_id, i)
joint_type = info[2]
if joint_type in [p.JOINT_REVOLUTE, getattr(p, "JOINT_CONTINUOUS", p.JOINT_REVOLUTE)]: p.setJointMotorControl2(
bodyUniqueId=pendulum_id,
jointIndex=i,
controlMode=p.VELOCITY_CONTROL,
targetVelocity=0,
force=0
)
# Get joint information from PyBullet
num_joints = p.getNumJoints(pendulum_id)
joint_map = {} # Map joint names to indices
link_map = {} # Map link names to indices
for i in range(num_joints):
info = p.getJointInfo(pendulum_id, i)
joint_name = info[1].decode('utf-8')
link_name = info[12].decode('utf-8')
joint_map[joint_name] = i
link_map[link_name] = i
# Set initial angles for the revolute joints
initial_angle1 = np.pi / 3 # 60 degrees
initial_angle2 = np.pi / 2 # 45 degrees
print("\nSetting initial joint angles:")
if 'joint1' in joint_map:
p.resetJointState(pendulum_id, joint_map['joint1'], initial_angle1)
print(f" joint1: {np.degrees(initial_angle1):.1f}°")
if 'joint2' in joint_map:
p.resetJointState(pendulum_id, joint_map['joint2'], initial_angle2)
print(f" joint2: {np.degrees(initial_angle2):.1f}°")
# Re-enable rendering
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
# Trail tracking
trail_bob1 = []
trail_bob2 = []
trail_line_ids_1 = []
trail_line_ids_2 = []
max_trail_length = 200
print("\n" + "="*50)
print("Double Pendulum Simulation Started")
print("="*50)
print("Close the window to exit")
# Simulation parameters
dt = 1./240.
p.setTimeStep(dt)
frame_skip = 4
frame_count = 0
# Get bob link indices
bob1_index = link_map.get('bob1')
bob2_index = link_map.get('bob2')
p.resetDebugVisualizerCamera(
cameraDistance=1.0, # how far the camera is
cameraYaw=180, # rotate around Z axis so we look along +Y
cameraPitch=-10, # tilt downward a bit
cameraTargetPosition=[0, 3, 0] # center the camera on the pendulum
)
try:
while True:
p.stepSimulation()
frame_count += 1
# Only update visuals every few frames
if frame_count % frame_skip == 0:
# Get bob positions
pos1, pos2 = None, None
if bob1_index is not None:
link_state1 = p.getLinkState(pendulum_id, bob1_index)
pos1 = link_state1[0]
if bob2_index is not None:
link_state2 = p.getLinkState(pendulum_id, bob2_index)
pos2 = link_state2[0]
# Add to trails
if pos1:
trail_bob1.append(pos1)
if pos2:
trail_bob2.append(pos2)
# Limit trail length
if len(trail_bob1) > max_trail_length:
trail_bob1.pop(0)
if trail_line_ids_1:
p.removeUserDebugItem(trail_line_ids_1.pop(0))
if len(trail_bob2) > max_trail_length:
trail_bob2.pop(0)
if trail_line_ids_2:
p.removeUserDebugItem(trail_line_ids_2.pop(0))
# Draw new trail segments
if len(trail_bob1) > 1:
line_id = p.addUserDebugLine(
trail_bob1[-2], trail_bob1[-1],
lineColorRGB=[1, 0.5, 0.5],
lineWidth=2
)
trail_line_ids_1.append(line_id)
if len(trail_bob2) > 1:
line_id = p.addUserDebugLine(
trail_bob2[-2], trail_bob2[-1],
lineColorRGB=[0.5, 0.5, 1],
lineWidth=2
)
trail_line_ids_2.append(line_id)
time.sleep(dt)
except KeyboardInterrupt:
print("\nSimulation stopped by user")
p.disconnect()
print("PyBullet disconnected. Goodbye!")
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
# --- Parametri fisici ---
g = 9.81
L1 = L2 = 1.0
m1 = m2 = 1.0
# --- Equazioni del moto ---
def derivs(state, t):
th1, w1, th2, w2 = state
delta = th2 - th1
den1 = (m1 + m2) * L1 - m2 * L1 * np.cos(delta)**2
den2 = (L2 / L1) * den1
dw1 = (m2 * L1 * w1**2 * np.sin(delta) * np.cos(delta)
+ m2 * g * np.sin(th2) * np.cos(delta)
+ m2 * L2 * w2**2 * np.sin(delta)
- (m1 + m2) * g * np.sin(th1)) / den1
dw2 = (-L2 / L1 * m2 * w2**2 * np.sin(delta) * np.cos(delta)
+ (m1 + m2) * (g * np.sin(th1) * np.cos(delta)
- L1 * w1**2 * np.sin(delta) - g * np.sin(th2))) / den2
return np.array([w1, dw1, w2, dw2])
# --- Integrazione numerica (RK4) ---
def rk4_step(f, y, t, dt):
k1 = f(y, t)
k2 = f(y + 0.5 * dt * k1, t + 0.5 * dt)
k3 = f(y + 0.5 * dt * k2, t + 0.5 * dt)
k4 = f(y + dt * k3, t + dt)
return y + (dt / 6) * (k1 + 2*k2 + 2*k3 + k4)
# --- Simulazione ---
dt = 0.01
t = np.arange(0, 20, dt)
# Due condizioni iniziali quasi identiche
y1 = np.array([np.pi/2, 0, np.pi/2 + 0.01, 0])
y2 = np.array([np.pi/2, 0, np.pi/2 + 0.010001, 0])
y1s, y2s = [], []
for _ in t:
y1 = rk4_step(derivs, y1, 0, dt)
y2 = rk4_step(derivs, y2, 0, dt)
y1s.append(y1.copy())
y2s.append(y2.copy())
y1s, y2s = np.array(y1s), np.array(y2s)
# --- Conversione in coordinate cartesiane ---
x1_1 = L1 * np.sin(y1s[:,0])
y1_1 = -L1 * np.cos(y1s[:,0])
x2_1 = x1_1 + L2 * np.sin(y1s[:,2])
y2_1 = y1_1 - L2 * np.cos(y1s[:,2])
x1_2 = L1 * np.sin(y2s[:,0])
y1_2 = -L1 * np.cos(y2s[:,0])
x2_2 = x1_2 + L2 * np.sin(y2s[:,2])
y2_2 = y1_2 - L2 * np.cos(y2s[:,2])
# --- Animazione ---
fig, ax = plt.subplots()
ax.set_xlim(-2.2, 2.2)
ax.set_ylim(-2.2, 2.2)
ax.set_aspect('equal')
ax.set_title("Double Pendulum Chaos")
(line1,) = ax.plot([], [], 'o-', lw=2, color='blue', label='Pendulum A')
(line2,) = ax.plot([], [], 'o-', lw=2, color='red', alpha=0.6, label='Pendulum B')
ax.legend()
def update(i):
line1.set_data([0, x1_1[i], x2_1[i]], [0, y1_1[i], y2_1[i]])
line2.set_data([0, x1_2[i], x2_2[i]], [0, y1_2[i], y2_2[i]])
return line1, line2
ani = FuncAnimation(fig, update, frames=len(t), interval=10, blit=True)
plt.show()
