30 lines
795 B
Python
30 lines
795 B
Python
import pybullet as p
|
|
|
|
# Start PyBullet simulation
|
|
physicsClient = p.connect(p.DIRECT) # or p.GUI for graphical interface
|
|
|
|
# Load your robot URDF
|
|
robot_id = p.loadURDF("/ur10e.urdf", useFixedBase=True)
|
|
|
|
# Set joint angles
|
|
joint_angles = [0, 0, 3.141/2, 0, 0, 0]
|
|
for i, angle in enumerate(joint_angles):
|
|
p.setJointMotorControl2(robot_id, i, p.POSITION_CONTROL, targetPosition=angle)
|
|
|
|
# Step the simulation to update the robot's state
|
|
p.stepSimulation()
|
|
|
|
# Check if any link is colliding
|
|
collision = False
|
|
for i in range(p.getNumBodies()):
|
|
contact_points = p.getContactPoints(robot_id)
|
|
print(contact_points)
|
|
if len(contact_points) > 0:
|
|
collision = True
|
|
break
|
|
|
|
if collision:
|
|
print("The robot has collided with itself.")
|
|
else:
|
|
print("No collision detected.")
|