ROS & Robotics
MuJoCo-python 환경에서 body 에 외력 주기 (xfrc_applied)
- -
요새 계속 MuJoCo 관련 프로젝트를 진행하고 있는데, mujoco 의 python wrapper 인 mujoco-py 는 한국어 자료도 거의 없고, 참고할 만한 자료가 거의 공식 도큐먼트 밖에 없는 것 같았습니다. 프로젝트를 진행하면서 mujoco-py 를 사용하는 방법에 대해서 포스팅을 해 보려고 합니다.
mujoco 에서는 xfrc_applied 라는 외력을 사용할 수 있는데, 이를 이용하면 body 에 힘과 토크를 적용할 수 있습니다.
https://www.youtube.com/watch?v=N2E-qnsyn1I
다음과 같이 body 에 외력을 줄 수 있습니다.
0. 개발 환경
- Ubuntu 20.04
- Mujoco-py
1. xfrc_applied
xfrc_applied 의 [0:3] 요소는 힘을, [3:6] 까지의 요소는 토크를 의미합니다. 다음 코드는 동영상처럼 원을 따라서 외력을 가하는 코드입니다.
외력 자체를 적용하는 것은
self.sim.data.xfrc_applied[body_index] = self.force
딱 이 한 줄로 할 수 있지만, 외력을 시각화하는 코드가 추가로 포함되어 있습니다.
Mujoco-py 는 화살표를 지원하지 않아, 실린더에 끝 점에 구 모양을 붙여서 화살표와 같은 기능을 하도록 했습니다.
- xfrc_applied.py
# 단순한 사각형에 외력을 주고 이를 시각화하는 테스트 코드
import mujoco_py
import numpy as np
import time
import math
class Robot:
def __init__(self, mjcf_path, body_name):
self.model = mujoco_py.load_model_from_path(mjcf_path)
self.sim = mujoco_py.MjSim(self.model)
self.viewer = mujoco_py.MjViewer(self.sim)
self.body_name = body_name
self.last_update_time = time.time()
self.body_index = self.sim.model.body_name2id(body_name)
self.force = np.zeros(6) # 초기 외력 값
self.random_force = np.zeros(6)
def rotation_quaternion(self, from_vec, to_vec):
v = np.cross(from_vec, to_vec)
w = np.sqrt((np.linalg.norm(from_vec) ** 2) * (np.linalg.norm(to_vec) ** 2)) + np.dot(from_vec, to_vec)
return np.concatenate(([w], v))
def apply_external_force(self):
# body name 가져오기
body_index = self.sim.model.body_name2id(self.body_name)
# 외력 가하기
self.sim.data.xfrc_applied[body_index] = self.force
force_vector = self.force[:3]
# print("force_vector :", force_vector)
# for force vector visualization
start_point = self.sim.data.body_xpos[body_index]
distance = 0.2
end_point = start_point + force_vector / (np.linalg.norm(force_vector) + 1.0E-6) * distance
arrow_id = self.sim.model.geom_name2id("force_visualization")
arrow_tip_id = self.sim.model.geom_name2id("force_arrow_tip")
self.sim.model.geom_pos[arrow_id] = end_point
default_direction = np.array([0, 0, 1])
quat = self.rotation_quaternion(default_direction, force_vector / np.linalg.norm(force_vector) + 1.0E-6)
print("start point is ",start_point)
print("end point is ",end_point)
self.sim.model.geom_pos[arrow_id] = start_point
self.sim.model.geom_quat[arrow_id] = quat
self.sim.model.geom_pos[arrow_tip_id] = end_point
def update_force_random(self):
self.random_force = np.random.uniform(-0.001, 0.001, 2) # 크기 조정
self.force = np.concatenate([self.random_force, [0], np.zeros(3)])
def update_force_circle(self, time):
radius = 0.01
omega = 5
x_force = radius * math.cos(omega * time)
y_force = radius * math.sin(omega * time)
self.force = np.array([x_force, y_force, 0, 0, 0, 0])
def run_simulation(self):
while True:
current_time = time.time()
# if current_time - self.last_update_time > 0.01:
self.update_force_circle(current_time - self.last_update_time)
# self.last_update_time = current_time
self.apply_external_force()
self.sim.step()
self.viewer.render()
time.sleep(0.01)
mjcf_path = "model.xml"
body_name = "box"
robot = Robot(mjcf_path, body_name)
robot.run_simulation()
update_force 함수를 고쳐서 원하는 궤적으로 외력이 작용하도록 할 수 있습니다.
- model.xml
<?xml version="1.0" ?>
<mujoco>
<worldbody>
<geom name="force_visualization" type="cylinder" rgba="1 0 0 1" size="0.01 0.1" quat="0.707 0 0.707 0"/>
<geom name="force_arrow_tip" pos="0.5 0.5 1" type="sphere" size="0.05" rgba="1 0 0 1"/>
<body name="box" pos="0 0 0.2">
<geom size="0.15 0.15 0.15" rgba="1 1 1 0.7" type="box"/>
<joint axis="1 0 0" name="box:x" type="slide"/>
<joint axis="0 1 0" name="box:y" type="slide"/>
</body>
<body name="floor" pos="0 0 0.025">
<geom size="10 10 0.02" rgba="0 1 0 1" type="box"/>
</body>
</worldbody>
</mujoco>Contents
소중한 공감 감사합니다