pip install pybulletpython -m pip install pybulletimport pybullet as p
import pybullet_data
import time
# 1. 3D 뷰어 열기
physics_client = p.connect(p.GUI)
# 2. 내장 모델 경로 등록
p.setAdditionalSearchPath(pybullet_data.getDataPath())
# 3. 중력 설정 (z축 아래로 -9.81 m/s²)
p.setGravity(0, 0, -9.81)
# 4. 바닥 깔기
plane = p.loadURDF("plane.urdf")
# 5. 시뮬레이션 루프
for _ in range(10000):
p.stepSimulation()
time.sleep(1/240)
p.disconnect()p.GUI vs p.DIRECT 차이 (궁금한 사람만)p.GUI = 3D 창이 뜸. 눈으로 확인할 때 사용.
p.DIRECT = 창 없이 계산만. 10~50배 빠름. 나중에 AI 학습할 때 씀.
지금은 무조건 GUI. 나중에 강화학습(Phase 2)에서 DIRECT를 씁니다.
# 큐브 만들기
cube_shape = p.createCollisionShape(p.GEOM_BOX,
halfExtents=[0.5, 0.5, 0.5])
cube_visual = p.createVisualShape(p.GEOM_BOX,
halfExtents=[0.5, 0.5, 0.5],
rgbaColor=[1, 0, 0, 1]) # 빨간색
cube = p.createMultiBody(
baseMass=1, # 1kg
baseCollisionShapeIndex=cube_shape,
baseVisualShapeIndex=cube_visual,
basePosition=[0, 0, 5] # 5m 높이에서 시작
)CollisionShape = 물리 엔진이 충돌을 계산하는 데 쓰는 모양. 단순해야 빠름.
VisualShape = 화면에 보이는 모양. 복잡해도 됨 (성능에 덜 영향).
# 파란 구
sphere_s = p.createCollisionShape(p.GEOM_SPHERE, radius=0.5)
sphere_v = p.createVisualShape(p.GEOM_SPHERE, radius=0.5,
rgbaColor=[0, 0, 1, 1])
p.createMultiBody(baseMass=1,
baseCollisionShapeIndex=sphere_s,
baseVisualShapeIndex=sphere_v,
basePosition=[2, 0, 5])
# 초록 원기둥
cyl_s = p.createCollisionShape(p.GEOM_CYLINDER,
radius=0.3, height=1.0)
cyl_v = p.createVisualShape(p.GEOM_CYLINDER,
radius=0.3, length=1.0,
rgbaColor=[0, 1, 0, 1])
p.createMultiBody(baseMass=1,
baseCollisionShapeIndex=cyl_s,
baseVisualShapeIndex=cyl_v,
basePosition=[-2, 0, 5])length, CollisionShape에서는 height. 이름이 다름 주의!# 가벼운 큐브 (1kg)
p.createMultiBody(baseMass=1, ..., basePosition=[-1,0,3])
# 무거운 큐브 (100kg)
p.createMultiBody(baseMass=100, ..., basePosition=[1,0,3])# 물체 만든 후에 물리 속성 변경
p.changeDynamics(cube, -1,
lateralFriction=0.0, # 마찰 0 = 빙판
restitution=0.9) # 반발 0.9 = 탱탱볼
# 비교용: 마찰 높고 반발 없는 큐브
p.changeDynamics(cube2, -1,
lateralFriction=1.0, # 마찰 1 = 고무
restitution=0.0) # 반발 0 = 찰흙자주 쓰는 것만:
rollingFriction을 0으로 두면 구가 영원히 굴러갑니다. 현실에선 있으니까 0.01 정도 줘야 자연스러움.
for i in range(2400): # 2400스텝 = 10초
p.stepSimulation()
time.sleep(1/240) # 실시간 재생
# 큐브 위치 확인 (매 1초마다)
if i % 240 == 0:
pos, orn = p.getBasePositionAndOrientation(cube)
print(f"t={i//240}s z={pos[2]:.2f}m")update()와 비교Ursina는 게임 엔진이 루프를 관리하지만, PyBullet은 우리가 직접 루프를 짭니다. 더 자유롭지만 더 책임도 큼.
time.sleep(1/240)을 빼면? → 2400스텝이 0.1초만에 끝남. 물리는 10초치를 계산하지만 화면은 순식간에 지나감.
# 시뮬 루프 안에서, 2초 시점에 큐브를 옆으로 밀기
if i == 480: # 480 step = 2초
p.applyExternalForce(
cube, -1,
forceObj=[500, 0, 0], # x방향 500N
posObj=[0, 0, 0],
flags=p.WORLD_FRAME
)import random
for i in range(20):
shape = p.createCollisionShape(p.GEOM_SPHERE, radius=0.2)
vis = p.createVisualShape(p.GEOM_SPHERE, radius=0.2,
rgbaColor=[random.random(), random.random(),
random.random(), 1])
p.createMultiBody(
baseMass=0.5,
baseCollisionShapeIndex=shape,
baseVisualShapeIndex=vis,
basePosition=[
random.uniform(-1, 1),
random.uniform(-1, 1),
3 + i*0.5 # 점점 높이
]
)import pybullet as p
import pybullet_data
import time
physics_client = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, -9.81)
plane = p.loadURDF("plane.urdf")
# 여기에 로봇을 불러올 겁니다
for _ in range(10000):
p.stepSimulation()
time.sleep(1/240)
p.disconnect()<robot name="my_robot">
<link name="base"> ← 몸통 (뼈대)
<visual> ... </visual> ← 보이는 모양
<collision> ... </collision> ← 충돌 영역
<inertial> ... </inertial> ← 질량/관성
</link>
<joint name="joint1" type="revolute"> ← 관절
<parent link="base"/> ← 부모 링크
<child link="arm"/> ← 자식 링크
<axis xyz="0 0 1"/> ← 회전축
</joint>
<link name="arm"> ... </link>
</robot>r2d2 = p.loadURDF("r2d2.urdf", basePosition=[0,0,0.5])num_joints = p.getNumJoints(r2d2)
print(f"관절 수: {num_joints}")
for i in range(num_joints):
info = p.getJointInfo(r2d2, i)
name = info[1].decode('utf-8')
joint_type = info[2]
lower = info[8] # 하한
upper = info[9] # 상한
type_name = {0:'회전', 1:'직선',
4:'고정'}.get(joint_type, '기타')
print(f" [{i}] {name:20s} 타입={type_name}"
f" 범위=[{lower:.2f}, {upper:.2f}]")대부분 로봇은 REVOLUTE(회전)와 FIXED(고정)로 구성됩니다.
kuka = p.loadURDF("kuka_iiwa/model.urdf",
basePosition=[0, 0, 0],
useFixedBase=True) # 바닥에 고정!useFixedBase=True 안 쓰면 로봇 팔이 바닥에 넘어져요. 실제로 공장 로봇 팔은 바닥에 볼트로 고정돼 있으니까.<?xml version="1.0"?>
<robot name="door">
<!-- 문틀 (고정) -->
<link name="frame">
<visual>
<geometry><box size="0.1 0.05 2.0"/></geometry>
<material name="gray">
<color rgba="0.5 0.5 0.5 1"/>
</material>
</visual>
<collision>
<geometry><box size="0.1 0.05 2.0"/></geometry>
</collision>
<inertial>
<mass value="0"/>
<inertia ixx="0" iyy="0" izz="0"
ixy="0" ixz="0" iyz="0"/>
</inertial>
</link>
<!-- 문짝 -->
<link name="door_panel">
<visual>
<origin xyz="0.4 0 0"/>
<geometry><box size="0.8 0.05 1.8"/></geometry>
<material name="brown">
<color rgba="0.6 0.3 0.1 1"/>
</material>
</visual>
<collision>
<origin xyz="0.4 0 0"/>
<geometry><box size="0.8 0.05 1.8"/></geometry>
</collision>
<inertial>
<mass value="10"/>
<origin xyz="0.4 0 0"/>
<inertia ixx="1" iyy="1" izz="1"
ixy="0" ixz="0" iyz="0"/>
</inertial>
</link>
<!-- 경첩 관절 -->
<joint name="hinge" type="revolute">
<parent link="frame"/>
<child link="door_panel"/>
<origin xyz="0 0 0"/>
<axis xyz="0 0 1"/>
<limit lower="0" upper="1.57"
effort="100" velocity="5"/>
</joint>
</robot># 문 로드하기
door = p.loadURDF("door.urdf",
basePosition=[0, 2, 1],
useFixedBase=True)# 여러 로봇 나란히
r2d2 = p.loadURDF("r2d2.urdf",
basePosition=[-2,0,0.5])
kuka = p.loadURDF("kuka_iiwa/model.urdf",
basePosition=[0,0,0], useFixedBase=True)
humanoid = p.loadURDF("humanoid/humanoid.urdf",
basePosition=[2,0,1], useFixedBase=True)
# 각각 관절 수 출력
for name, robot in [("R2D2",r2d2),("Kuka",kuka),("Human",humanoid)]:
print(f"{name}: {p.getNumJoints(robot)}개 관절")import pybullet as p
import pybullet_data
import time, math
physics_client = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, -9.81)
p.loadURDF("plane.urdf")
kuka = p.loadURDF("kuka_iiwa/model.urdf",
useFixedBase=True)
num_joints = p.getNumJoints(kuka)
print(f"Kuka 관절 수: {num_joints}") # 7# 0번 관절을 1.57 라디안(90도)으로 보내기
p.setJointMotorControl2(
bodyUniqueId=kuka,
jointIndex=0,
controlMode=p.POSITION_CONTROL,
targetPosition=1.57, # 목표 각도 (라디안)
force=200 # 최대 힘
)math.radians(90)으로 변환 가능.왜 라디안? 물리/수학 공식에서 삼각함수(sin, cos)가 라디안 기준이라 로봇 공학에서는 라디안이 표준.
# 7개 관절 한 번에 세팅
target_positions = [0.5, -0.3, 0.8, -1.2, 0.4, 1.0, 0.0]
for j in range(7):
p.setJointMotorControl2(
kuka, j,
controlMode=p.POSITION_CONTROL,
targetPosition=target_positions[j],
force=200
)# 0번 관절을 초속 1 라디안으로 계속 회전
p.setJointMotorControl2(
kuka, 0,
controlMode=p.VELOCITY_CONTROL,
targetVelocity=1.0, # 초속 1 라디안
force=200
)# 먼저 기존 모터 끄기 (중요!)
p.setJointMotorControl2(kuka, 0,
controlMode=p.VELOCITY_CONTROL,
targetVelocity=0, force=0)
# 그다음 토크 직접 적용
p.setJointMotorControl2(kuka, 0,
controlMode=p.TORQUE_CONTROL,
force=50) # 50 N·m 토크# 시뮬 루프 안에서
state = p.getJointState(kuka, 0)
pos = state[0] # 현재 각도 (라디안)
vel = state[1] # 현재 각속도
force = state[3] # 현재 적용 토크
print(f"각도={math.degrees(pos):.1f}° 속도={vel:.2f}")current_joint = 0 # 지금 제어 중인 관절
target = [0.0] * 7 # 7개 관절 목표값
for _ in range(100000):
p.stepSimulation()
time.sleep(1/240)
keys = p.getKeyboardEvents()
# 1~7 키로 관절 선택
for k in range(7):
if ord(str(k+1)) in keys:
current_joint = k
print(f"관절 {k} 선택")
# 좌/우 화살표로 각도 변경
if p.B3G_LEFT_ARROW in keys:
target[current_joint] -= 0.02
if p.B3G_RIGHT_ARROW in keys:
target[current_joint] += 0.02
# 모든 관절에 목표 적용
for j in range(7):
p.setJointMotorControl2(kuka, j,
p.POSITION_CONTROL,
targetPosition=target[j], force=200)p.B3G_LEFT_ARROW, p.B3G_RIGHT_ARROW는 PyBullet 내장 키코드. 일반 문자는 ord('a') 방식.t = 0
for _ in range(5000):
p.stepSimulation()
time.sleep(1/240)
t += 1/240
# 0번 관절: 사인파로 왔다갔다
p.setJointMotorControl2(kuka, 0,
p.POSITION_CONTROL,
targetPosition=math.sin(t * 2) * 1.0,
force=200)
# 2번 관절: 코사인파
p.setJointMotorControl2(kuka, 2,
p.POSITION_CONTROL,
targetPosition=math.cos(t * 2) * 0.8,
force=200)import pybullet as p
import pybullet_data
import time
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0,0,-9.81)
plane = p.loadURDF("plane.urdf")
# 빨간 큐브 (위에서 떨어짐)
box_s = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.3]*3)
box_v = p.createVisualShape(p.GEOM_BOX, halfExtents=[0.3]*3,
rgbaColor=[1,0,0,1])
box = p.createMultiBody(baseMass=1,
baseCollisionShapeIndex=box_s,
baseVisualShapeIndex=box_v,
basePosition=[0,0,3])
# 파란 테이블 (고정)
table_s = p.createCollisionShape(p.GEOM_BOX,
halfExtents=[1,1,0.1])
table_v = p.createVisualShape(p.GEOM_BOX,
halfExtents=[1,1,0.1],
rgbaColor=[0,0,1,1])
table = p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=table_s,
baseVisualShapeIndex=table_v,
basePosition=[0,0,0.5])for i in range(5000):
p.stepSimulation()
time.sleep(1/240)
# box와 table 사이 접촉 확인
contacts = p.getContactPoints(box, table)
if contacts:
for c in contacts:
pos = c[5] # 접촉 위치 (B측)
normal = c[7] # 접촉 법선
force = c[9] # 접촉력 크기
if i % 60 == 0:
print(f"접촉! 힘={force:.2f}N")# 시작점 → 끝점으로 직선 쏘기
start_pos = [0, 0, 2] # 위에서
end_pos = [0, 0, 0] # 아래로
result = p.rayTest(start_pos, end_pos)
hit_object = result[0][0] # 맞은 물체 ID (-1=없음)
hit_fraction = result[0][2] # 0~1 사이 비율
hit_pos = result[0][3] # 맞은 좌표
if hit_object >= 0:
distance = hit_fraction * 2 # start→end 거리 × fraction
print(f"물체 {hit_object}에 맞음! 거리={distance:.2f}m")fraction이 1.0이면 = 끝점까지 아무것도 없음 (hit_object = -1).
# 레이를 눈에 보이게
p.addUserDebugLine(
start_pos, end_pos,
lineColorRGB=[1,0,0], # 빨간 선
lineWidth=2,
lifeTime=0 # 0=영구
)import math
scan_origin = [0, 0, 0.5]
num_rays = 36 # 360도를 36등분 = 10도 간격
ray_len = 5
starts = []
ends = []
for i in range(num_rays):
angle = 2 * math.pi * i / num_rays
dx = math.cos(angle) * ray_len
dy = math.sin(angle) * ray_len
starts.append(scan_origin)
ends.append([scan_origin[0]+dx,
scan_origin[1]+dy,
scan_origin[2]])
results = p.rayTestBatch(starts, ends)
# 시각화
for i, r in enumerate(results):
color = [1,0,0] if r[0] >= 0 else [0,1,0]
p.addUserDebugLine(starts[i], ends[i],
lineColorRGB=color, lineWidth=1, lifeTime=0)# 장애물 3개 배치
for pos in [(2,0,0.5), (-1,2,0.5), (0,-3,0.5)]:
obs_s = p.createCollisionShape(p.GEOM_BOX,
halfExtents=[0.5,0.5,0.5])
obs_v = p.createVisualShape(p.GEOM_BOX,
halfExtents=[0.5,0.5,0.5],
rgbaColor=[0.8,0.2,0.2,1])
p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=obs_s,
baseVisualShapeIndex=obs_v,
basePosition=pos)kuka = p.loadURDF("kuka_iiwa/model.urdf",
useFixedBase=True)
# 앞에 큐브 하나
target_box = p.createMultiBody(baseMass=0.5, ...,
basePosition=[0.5,0,0.5])
stopped = False
for _ in range(10000):
p.stepSimulation()
time.sleep(1/240)
if not stopped:
# 팔을 앞으로 뻗기
p.setJointMotorControl2(kuka, 1,
p.POSITION_CONTROL,
targetPosition=-1.0, force=200)
# 닿았나?
contacts = p.getContactPoints(kuka, target_box)
if contacts:
print("접촉! 정지!")
stopped = True
# 현재 위치에서 고정
state = p.getJointState(kuka, 1)
p.setJointMotorControl2(kuka, 1,
p.POSITION_CONTROL,
targetPosition=state[0], force=200)import numpy as np
# 카메라 설정
width, height = 320, 240
view_matrix = p.computeViewMatrix(
cameraEyePosition=[1, 1, 1], # 카메라 위치
cameraTargetPosition=[0, 0, 0.5], # 바라보는 곳
cameraUpVector=[0, 0, 1] # 위쪽 방향
)
proj_matrix = p.computeProjectionMatrixFOV(
fov=60,
aspect=width/height,
nearVal=0.1,
farVal=10
)
# 사진 찍기!
img = p.getCameraImage(width, height,
viewMatrix=view_matrix,
projectionMatrix=proj_matrix)
# img[2] = RGB 데이터 (numpy로 변환)
rgb = np.array(img[2], dtype=np.uint8).reshape(height, width, 4)
rgb = rgb[:, :, :3] # RGBA → RGBimport matplotlib.pyplot as plt
plt.imshow(rgb)
plt.title("PyBullet Camera")
plt.axis('off')
plt.savefig("capture.png")
plt.show()pip install matplotlib# img[3] = depth buffer (0~1)
depth_raw = np.array(img[3], dtype=np.float32).reshape(height, width)
# 실제 거리(m)로 변환
near, far = 0.1, 10
depth_m = far * near / (far - (far - near) * depth_raw)
plt.imshow(depth_m, cmap='gray')
plt.colorbar(label='Distance (m)')
plt.title("Depth Map")
plt.savefig("depth.png")
plt.show()# img[4] = segmentation mask
seg = np.array(img[4], dtype=np.int32).reshape(height, width)
plt.imshow(seg, cmap='tab10')
plt.title("Segmentation (물체별 색상)")
plt.savefig("seg.png")
plt.show()kuka = p.loadURDF("kuka_iiwa/model.urdf", useFixedBase=True)
def get_robot_camera(robot, link_index=6):
# 끝단(end-effector) 위치와 방향 가져오기
state = p.getLinkState(robot, link_index)
pos = state[0] # 위치
orn = state[1] # 방향 (쿼터니언)
# 쿼터니언 → 회전행렬
rot = p.getMatrixFromQuaternion(orn)
# 카메라가 바라보는 방향 (z축 앞쪽)
forward = [rot[0], rot[3], rot[6]]
target = [pos[0]+forward[0],
pos[1]+forward[1],
pos[2]+forward[2]]
view = p.computeViewMatrix(pos, target, [0,0,1])
proj = p.computeProjectionMatrixFOV(60, 1, 0.1, 5)
return p.getCameraImage(320,240, view, proj)# 빨간 큐브를 씬에 추가하고 카메라로 촬영 후
# RGB 이미지에서 빨간 픽셀 찾기
red_mask = (rgb[:,:,0] > 150) & \
(rgb[:,:,1] < 80) & \
(rgb[:,:,2] < 80)
red_pixels = np.sum(red_mask)
total_pixels = width * height
ratio = red_pixels / total_pixels * 100
print(f"빨간 영역: {ratio:.1f}%")
if red_pixels > 100:
# 빨간 물체의 중심 좌표 (픽셀)
ys, xs = np.where(red_mask)
cx, cy = np.mean(xs), np.mean(ys)
print(f"빨간 물체 중심: ({cx:.0f}, {cy:.0f})")import pybullet as p
import pybullet_data
import time, math
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0,0,-9.81)
p.loadURDF("plane.urdf")
# 테이블
table = p.loadURDF("table/table.urdf",
basePosition=[0.5,0,-0.63])
# Panda 로봇 (그리퍼 내장)
panda = p.loadURDF("franka_panda/panda.urdf",
basePosition=[0,0,0], useFixedBase=True)
# 잡을 물체 (빨간 큐브)
obj_s = p.createCollisionShape(p.GEOM_BOX,
halfExtents=[0.02]*3)
obj_v = p.createVisualShape(p.GEOM_BOX,
halfExtents=[0.02]*3, rgbaColor=[1,0,0,1])
obj = p.createMultiBody(baseMass=0.1,
baseCollisionShapeIndex=obj_s,
baseVisualShapeIndex=obj_v,
basePosition=[0.5, 0, 0.05])def gripper_open():
p.setJointMotorControl2(panda, 9,
p.POSITION_CONTROL, targetPosition=0.04, force=20)
p.setJointMotorControl2(panda, 10,
p.POSITION_CONTROL, targetPosition=0.04, force=20)
def gripper_close():
p.setJointMotorControl2(panda, 9,
p.POSITION_CONTROL, targetPosition=0.0, force=20)
p.setJointMotorControl2(panda, 10,
p.POSITION_CONTROL, targetPosition=0.0, force=20)def move_to(target_pos, target_orn=None):
if target_orn is None:
target_orn = p.getQuaternionFromEuler(
[math.pi, 0, 0]) # 아래를 바라보는 자세
joint_angles = p.calculateInverseKinematics(
panda,
endEffectorLinkIndex=11, # 끝단 링크
targetPosition=target_pos,
targetOrientation=target_orn,
maxNumIterations=100
)
# 팔 관절(0~6)에만 적용
for j in range(7):
p.setJointMotorControl2(panda, j,
p.POSITION_CONTROL,
targetPosition=joint_angles[j],
force=240)
# 테스트: 물체 위로 이동
move_to([0.5, 0, 0.3])PyBullet의 calculateInverseKinematics는 수치적 방법으로 근사해를 구합니다. 완벽하진 않지만 대부분 잘 작동.
def wait(steps=240):
for _ in range(steps):
p.stepSimulation()
time.sleep(1/240)
obj_pos = [0.5, 0, 0.05]
# 1. 그리퍼 열기
gripper_open()
wait(120)
# 2. 물체 위 10cm로 이동
move_to([obj_pos[0], obj_pos[1], obj_pos[2]+0.15])
wait(240)
# 3. 물체 높이로 하강
move_to([obj_pos[0], obj_pos[1], obj_pos[2]+0.02])
wait(240)
# 4. 그리퍼 닫기 (잡기!)
gripper_close()
wait(120)
# 5. 들어올리기
move_to([obj_pos[0], obj_pos[1], 0.3])
wait(240)place_pos = [0.3, 0.3, 0.05]
# 6. 목표 위치 위로 이동
move_to([place_pos[0], place_pos[1], 0.3])
wait(240)
# 7. 내려놓기 높이로 하강
move_to([place_pos[0], place_pos[1], place_pos[2]+0.05])
wait(240)
# 8. 그리퍼 열기 (놓기!)
gripper_open()
wait(120)
# 9. 위로 후퇴
move_to([0, 0, 0.5])
wait(240)objects = []
positions = [(0.4,0.1), (0.5,-0.1), (0.6,0.0)]
for x, y in positions:
o = p.createMultiBody(baseMass=0.1,
baseCollisionShapeIndex=obj_s,
baseVisualShapeIndex=obj_v,
basePosition=[x, y, 0.05])
objects.append((o, [x, y, 0.05]))
# 자동 수거
for obj_id, obj_pos in objects:
gripper_open(); wait(60)
move_to([obj_pos[0], obj_pos[1], 0.2]); wait(200)
move_to([obj_pos[0], obj_pos[1], obj_pos[2]+0.02]); wait(200)
gripper_close(); wait(100)
move_to([0.3, 0.3, 0.2]); wait(200)
move_to([0.3, 0.3, 0.07]); wait(200)
gripper_open(); wait(60)import pybullet as p
import pybullet_data
import time, math
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0,0,-9.81)
p.loadURDF("plane.urdf")
car = p.loadURDF("racecar/racecar.urdf")
# 바퀴 관절 확인
for i in range(p.getNumJoints(car)):
info = p.getJointInfo(car, i)
print(f"[{i}] {info[1].decode()}")LEFT_WHEELS = [8, 15] # 왼쪽 바퀴 관절 (차량마다 다름!)
RIGHT_WHEELS = [2, 5] # 오른쪽 바퀴 관절
MAX_FORCE = 20
def drive(left_vel, right_vel):
for w in LEFT_WHEELS:
p.setJointMotorControl2(car, w,
p.VELOCITY_CONTROL,
targetVelocity=left_vel, force=MAX_FORCE)
for w in RIGHT_WHEELS:
p.setJointMotorControl2(car, w,
p.VELOCITY_CONTROL,
targetVelocity=right_vel, force=MAX_FORCE)탱크, 룸바 청소기, 대부분의 교육용 로봇이 이 방식!
speed = 10
turn = 5
for _ in range(100000):
p.stepSimulation()
time.sleep(1/240)
keys = p.getKeyboardEvents()
left_v, right_v = 0, 0
if p.B3G_UP_ARROW in keys: # 전진
left_v, right_v = speed, speed
if p.B3G_DOWN_ARROW in keys: # 후진
left_v, right_v = -speed, -speed
if p.B3G_LEFT_ARROW in keys: # 좌회전
left_v, right_v = -turn, turn
if p.B3G_RIGHT_ARROW in keys: # 우회전
left_v, right_v = turn, -turn
drive(left_v, right_v)goal = [5, 3, 0]
# 목표 지점 시각화
goal_v = p.createVisualShape(p.GEOM_SPHERE, radius=0.2,
rgbaColor=[0,1,0,1])
p.createMultiBody(baseMass=0, baseVisualShapeIndex=goal_v,
basePosition=goal)
for _ in range(50000):
p.stepSimulation()
time.sleep(1/240)
# 현재 위치 & 방향
pos, orn = p.getBasePositionAndOrientation(car)
euler = p.getEulerFromQuaternion(orn)
yaw = euler[2] # z축 회전 = 차량 방향
# 목표까지 각도
dx = goal[0] - pos[0]
dy = goal[1] - pos[1]
target_angle = math.atan2(dy, dx)
dist = math.sqrt(dx**2 + dy**2)
# 각도 차이
angle_diff = target_angle - yaw
# -π ~ π 범위로 정규화
angle_diff = math.atan2(math.sin(angle_diff),
math.cos(angle_diff))
if dist < 0.5:
drive(0, 0) # 도착!
print("목표 도착!")
break
elif abs(angle_diff) > 0.3:
# 먼저 방향 맞추기
turn_dir = 5 if angle_diff > 0 else -5
drive(-turn_dir, turn_dir)
else:
# 방향 맞으면 직진
drive(10, 10)# 장애물 배치
for obs_pos in [(2,1,0.5), (3,2,0.5), (4,1.5,0.5)]:
s = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.5]*3)
v = p.createVisualShape(p.GEOM_BOX, halfExtents=[0.5]*3,
rgbaColor=[0.8,0.2,0.2,1])
p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=s, baseVisualShapeIndex=v,
basePosition=obs_pos)
# 시뮬 루프 안에서: 전방 레이캐스트
pos, orn = p.getBasePositionAndOrientation(car)
euler = p.getEulerFromQuaternion(orn)
yaw = euler[2]
# 전방 3m 레이
front = [pos[0]+math.cos(yaw)*3,
pos[1]+math.sin(yaw)*3, 0.3]
ray = p.rayTest([pos[0],pos[1],0.3], front)
obstacle_ahead = ray[0][0] >= 0 and ray[0][2] < 0.8
if obstacle_ahead:
drive(-5, 5) # 장애물 → 회전
else:
# Go-to-Goal 로직 (위 코드)
...A. 물체 분류 로봇
카메라로 색상 감지 → 빨간/파란 분류 → Pick & Place
(5주차 + 6주차 통합)
B. 순찰 + 수거 로봇
바퀴 로봇이 웨이포인트 순회
→ 레이캐스트로 물체 발견 → 위치 기록
(4주차 + 7주차 통합)import pybullet as p
import pybullet_data
import numpy as np
import time, math
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0,0,-9.81)
p.loadURDF("plane.urdf")
p.loadURDF("table/table.urdf", [0.5,0,-0.63])
panda = p.loadURDF("franka_panda/panda.urdf",
useFixedBase=True)
# 빨간 큐브 2개 + 파란 큐브 2개
results = {'red': 0, 'blue': 0, 'fail': 0}
objects = []
for color, rgba, x in [
('red', [1,0,0,1], 0.4),
('blue',[0,0,1,1], 0.5),
('red', [1,0,0,1], 0.6),
('blue',[0,0,1,1], 0.45),
]:
s = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.02]*3)
v = p.createVisualShape(p.GEOM_BOX, halfExtents=[0.02]*3,
rgbaColor=rgba)
obj_id = p.createMultiBody(baseMass=0.1,
baseCollisionShapeIndex=s, baseVisualShapeIndex=v,
basePosition=[x, 0, 0.05])
objects.append((obj_id, color, [x, 0, 0.05]))
# move_to, gripper_open, gripper_close, wait는 6주차 그대로
# ... (함수 정의 생략)
# 분류 실행
RED_BOX = [0.3, 0.3, 0.05]
BLUE_BOX = [0.3, -0.3, 0.05]
for obj_id, color, pos in objects:
target = RED_BOX if color == 'red' else BLUE_BOX
gripper_open(); wait(60)
move_to([pos[0], pos[1], 0.2]); wait(200)
move_to([pos[0], pos[1], pos[2]+0.02]); wait(200)
gripper_close(); wait(100)
move_to([target[0], target[1], 0.2]); wait(200)
move_to(target); wait(200)
gripper_open(); wait(60)
results[color] += 1
print(f"{color} 분류 완료! {results}")
print(f"최종 결과: {results}")import requests
import json
# 시뮬 완료 후 결과 전송
data = {
'student': '홍길동',
'project': '물체 분류',
'red_sorted': results['red'],
'blue_sorted': results['blue'],
'failures': results['fail'],
'total_time': 42.5, # 초
}
try:
res = requests.post(
'https://apptulz.com/api/robotics/result/',
json=data, timeout=5
)
print(f"서버 응답: {res.status_code}")
except:
print("서버 연결 실패 (로컬 저장)")
with open('result.json', 'w') as f:
json.dump(data, f)