GenesisでUR5＋robotiq85のSimulationをした

RobogenのリポジトリにUR5のURDFがあったので、それを動かせるようにしました。

robogen-ai.github.io

使ったのはこのファイルです。

github.com

以前のUR5のSimulationから変更したのは三箇所です

EndEffectorのクオータニオン

サンプルのロボットや以前使ったUR5のURDFと違い、EndEffector周辺の定義が変わっています。なぜかee_linkが取得できないのでwrist_3_linkでIKを行います。EndEffecorの向きが90°変わっているので、下向きのクオータニオンも変更しています。このクオータニオンでgripperが下を向きます。

end_effector = ur.get_link('wrist_3_link')
goal_qpos = ur.inverse_kinematics(  
    link=end_effector,  
    pos=np.array([0.65, 0.0, 0.4]),  
    quat = np.array([0.70710678, -0.70710678, 0.0, 0.0]),  
    respect_joint_limit = True,  
    dofs_idx_local = motors_dof  
)

GripperのON/OFF

Robogenで使っているURDFはよくできていて、一つのjointを制御するだけで他の部分が連動して動きます。control_dofs_positionで動かすときは、jointの6だけに力を加えればON/OFFできます。

def gripper_open():  
    global finger_qpos  
    finger_qpos = [0.12, ]  
  
def gripper_close():  
    global finger_qpos  
    finger_qpos = [0.55, ]

fingers_dof = np.arange(6, 7)
ur.control_dofs_position(finger_qpos, fingers_dof)

一方OMPLで軌道を生成するときは、すべてのjointが必要です。jointの6以降のjonintから今のGripperの状態を取得し、IKの結果を上書きすると今のgripperの姿勢を維持したまま軌道生成ができます。

goal_qpos = list(goal_qpos)  
pos = ur.get_qpos()  
goal_qpos[6:12] = pos[6:12]

可動範囲

地面にめり込まないようにshoulder_lift_jointの値を修正。

<joint name="shoulder_lift_joint" type="revolute">  
  <parent link="shoulder_link"/>  
  <child link="upper_arm_link"/>  
  <origin rpy="0.0 1.5707963267948966 0.0" xyz="0.0 0.13585 0.0"/>  
  <axis xyz="0 1 0"/>  
  <limit effort="150.0" lower="-3.141592653589793" upper="-0.7" velocity="3.15"/>  
  <dynamics damping="0.0" friction="0.0"/>  
</joint>

robotiq_85_left_knuckle_jointの可動範囲が0.0～0.80285だったのを、-0.80285～0.80285に変更。EEの姿勢は連動していて、robotiq_85_left_knuckle_jointが元の範囲に入っていても連動する部分が負の値になってしまいます。それによって軌道生成ができなくなっていたので、範囲を緩めています。

<joint name="robotiq_85_left_knuckle_joint" type="revolute">  
  <parent link="robotiq_85_base_link"/>  
  <child link="robotiq_85_left_knuckle_link"/>  
  <axis xyz="0 0 1"/>  
  <origin rpy="3.141592653589793 0.0 0.0" xyz="0.05490451627 0.03060114443 0.0"/>  
  <limit effort="1000" lower="-0.80285" upper="0.80285" velocity="0.5"/>  
</joint>

動作結果

無事新しいURDFで動きました。

GWにGenesisが一歩前進した。圧倒的成長 pic.twitter.com/iBpd3hvGcb

— natsutan (@natsutan) 2025年5月5日

全ソース

Windows側

import os  
#os.environ['PYOPENGL_PLATFORM'] = 'glx'  
  
import genesis as gs  
import numpy as np  
import socket  
import json  
  
  
# WSL2 マシン(またはサーバー側)のIPアドレスとポートを合わせる  
# >wsl -- ip addr で確認できる  
HOST = '172.27.55.153'  # localhostで試す場合、WSL2/Windows間の通信は別IPになることも  
PORT = 50009  
  
########################## init ##########################  
gs.init(backend=gs.gpu)  
  
########################## create a scene ##########################  
scene = gs.Scene(  
    viewer_options=gs.options.ViewerOptions(  
        camera_pos=(3, -1, 1.5),  
        camera_lookat=(0.0, 0.0, 0.5),  
        camera_fov=30,  
        max_FPS=60,  
    ),  
    sim_options=gs.options.SimOptions(  
        dt=0.01,  
    ),  
    show_viewer=True,  
    rigid_options=gs.options.RigidOptions(  
        box_box_detection=True,  
    ),  
)  
  
########################## entities ##########################  
plane = scene.add_entity(  
    gs.morphs.Plane(),  
)  
cube = scene.add_entity(  
    gs.morphs.Box(  
        size=(0.04, 0.04, 0.04),  
        pos=(0.65, 0.0, 0.02),  
  
    )  
)  
  
ur = scene.add_entity(  
    gs.morphs.URDF(file='D:/home/myproj/genesis/UR5/asset/ur5/ur5_robotiq85.urdf', fixed=True),  
)  
  
cam = scene.add_camera(  
    res=(640, 480),  
    pos=(3, -1, 1.5),  
    lookat=(0, 0, 0.5),  
    fov=30,  
    GUI=False  
)  
  
finger_qpos = [0.00, ]  
  
def gripper_open():  
    global finger_qpos  
    finger_qpos = [0.12, ]  
  
def gripper_close():  
    global finger_qpos  
    finger_qpos = [0.55, ]  
  
  
  
  
  
def ompl_waypoints(start, goal, num_waypoint):  
    start_list = [float(x) for x in start]  
    goal_list = [float(x) for x in goal]  
  
    # 送信したいデータ  
    data_to_send = {  
        "qpos_start": start_list,  
        "qpos_goal":  goal_list,  
        "num_waypoint": num_waypoint  
    }  
  
    # ソケットを作ってサーバーに接続  
    with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:  
        print(f"Connecting to server {HOST}:{PORT} ...")  
        s.connect((HOST, PORT))  
  
        # JSONでエンコードして送信  
        message = json.dumps(data_to_send).encode()  
        s.sendall(message)  
  
        # レスポンス受信  
  
        # JSONデコードして結果を表示  
        response_data = recv_all(s)  
        response = json.loads(response_data.decode())  
        print("Received:", response)  
        return response.get("waypoint")  
  
  
def recv_all(sock):  
    """ サーバーが送信を完了 or ソケット閉じるまで、繰り返し受信する """    buffers = []  
    while True:  
        chunk = sock.recv(4096)  
        if not chunk:  
            # サーバー側が close した  
            break  
        buffers.append(chunk)  
    return b"".join(buffers)  
  
########################## build ##########################  
scene.build()  
  
motors_dof = np.arange(6)  
fingers_dof = np.arange(6, 7)  
first_qpos = np.array([  -0, -0.9,  -0.5,  -1.4,  -1.3,  -0.3, 0.04, 0.04, 0.04, 0.04, 0.04, 0.04])  
dofs_idx = list(np.arange(12))  
  
# from franka_cube.py  
kp = np.array([4500, 4500, 3500, 3500, 2000, 2000, 100, 100, 100, 100, 100, 100,])  
kv = np.array([450,   450,  350,  350,  200,  200, 10, 10, 10, 10, 10, 10])  
  
ur.set_dofs_kp(  
    kp,  
    dofs_idx  
)  
  
ur.set_dofs_kv(  
    kv,  
    dofs_idx  
)  
  
ur.set_dofs_force_range(  
    np.array([-87, -87, -87, -87, -87, -87, -12, -12, -12, -100, -100, -100]),  
    np.array([87, 87, 87, 87, 87, 87, 12, 12, 12, 100, 100, 100]),  
    dofs_idx  
)  
  
ur.set_qpos(first_qpos)  

gripper_open()  
for i in range(50):  
    ur.control_dofs_position(first_qpos[0:6], motors_dof)  
    ur.control_dofs_position(finger_qpos, fingers_dof)  
    scene.step()  
  
gripper_close()  
for i in range(50):  
    ur.control_dofs_position(first_qpos[0:6], motors_dof)  
    ur.control_dofs_position(finger_qpos, fingers_dof)  
    scene.step()  
  
  
gripper_open()  
for i in range(50):  
    ur.control_dofs_position(first_qpos[0:6], motors_dof)  
    ur.control_dofs_position(finger_qpos, fingers_dof)  
    scene.step()  
  
  
  
end_effector = ur.get_link('wrist_3_link')  
  
# workの真上に移動  
start_qpos = ur.get_qpos()  
goal_qpos = ur.inverse_kinematics(  
    link=end_effector,  
    pos=np.array([0.65, 0.0, 0.4]),  
    quat = np.array([0.70710678, -0.70710678, 0.0, 0.0]),  
    respect_joint_limit = True,  
    dofs_idx_local = motors_dof  
)  
  
goal_qpos = list(goal_qpos)  
pos = ur.get_qpos()  
goal_qpos[6:12] = pos[6:12]  
  
  
waypoints  = ompl_waypoints(start_qpos, goal_qpos, 200)  
for qpos in waypoints:  
    ur.control_dofs_position(qpos[0:6], motors_dof)  
    ur.control_dofs_position(finger_qpos, fingers_dof)  
    scene.step()  
  
  
# EEを下げる  
start_qpos = goal_qpos  
goal_qpos = ur.inverse_kinematics(  
    link=end_effector,  
    pos=np.array([0.65, 0.0, 0.15]),  
    quat=np.array([0.70710678, -0.70710678, 0.0, 0.0]),  
    dofs_idx_local = motors_dof  
)  
goal_qpos = list(goal_qpos)  
pos = ur.get_qpos()  
goal_qpos[6:12] = pos[6:12]  
  
waypoints  = ompl_waypoints(start_qpos, goal_qpos, 100)  
for qpos in waypoints:  
    ur.control_dofs_position(qpos[0:6], motors_dof)  
    ur.control_dofs_position(finger_qpos, fingers_dof)  
    scene.step()  
  
# grasp  
gripper_close()  
for i in range(100):  
    ur.control_dofs_position(goal_qpos[0:6], motors_dof)  
    ur.control_dofs_position(finger_qpos, fingers_dof)  
    scene.step()  
  
# lift  
start_qpos = goal_qpos  
goal_qpos = ur.inverse_kinematics(  
    link=end_effector,  
    pos=np.array([0.65, 0.0, 0.3]),  
    quat=np.array([0.70710678, -0.70710678, 0.0, 0.0]),  
    dofs_idx_local = motors_dof  
)  
  
goal_qpos = list(goal_qpos)  
pos = ur.get_qpos()  
goal_qpos[6:12] = pos[6:12]  
  
waypoints  = ompl_waypoints(start_qpos, goal_qpos, 100)  
  
for qpos in waypoints:  
    ur.control_dofs_position(qpos[0:6], motors_dof)  
    ur.control_dofs_position(finger_qpos, fingers_dof)  
    scene.step()  
  
# to the first position  
start_qpos = goal_qpos  
pos = ur.get_qpos()  
goal_qpos[6:12] = pos[6:12]  
  
waypoints  = ompl_waypoints(start_qpos, first_qpos, 200)  
for qpos in waypoints:  
    ur.control_dofs_position(qpos[0:6], motors_dof)  
    ur.control_dofs_position(finger_qpos, fingers_dof)  
    scene.step()

WSL2側

# server.py
import socket
import json

HOST = '0.0.0.0'  # どこからの接続でも受け付けたい場合は 0.0.0.0
PORT = 50009      # 適当なポート番号を指定


import os
#os.environ['PYOPENGL_PLATFORM'] = 'glx'

import genesis as gs
import numpy as np

########################## init ##########################
gs.init(backend=gs.cpu)

########################## create a scene ##########################
scene = gs.Scene(
    sim_options=gs.options.SimOptions(
        dt=0.01,
    ),
    show_viewer=False,
    rigid_options=gs.options.RigidOptions(
        box_box_detection=True,
    ),
)

########################## entities ##########################
plane = scene.add_entity(
    gs.morphs.Plane(),
)
cube = scene.add_entity(
    gs.morphs.Box(
        size=(0.04, 0.04, 0.04),
        pos=(0.65, 0.0, 0.02),

    )
)
ur = scene.add_entity(
    #gs.morphs.MJCF(file='/home/natu/myproj/genesis/pybullet_ur5_gripper/robots/xml/ur5e.xml'),
    gs.morphs.URDF(file='/mnt/d/home/myproj/genesis/UR5/asset/ur5/ur5_robotiq85.urdf', fixed=True),

    vis_mode='collision'
)

########################## build ##########################
scene.build()

motors_dof = np.arange(6)
fingers_dof = np.arange(6, 12)

# from franka_cube.py
kp = np.array([4500, 4500, 3500, 3500, 2000, 2000, 100, 100, 100, 100, 100, 100,])
kv = np.array([450,   450,  350,  350,  200,  200, 10, 10, 10, 10, 10, 10])

ur.set_dofs_kp(
    kp = kp,
)

ur.set_dofs_kv(
    kv = kv,
)

ur.set_dofs_force_range(
    np.array([-87, -87, -87, -87, -87, -87, -12, -12, -12, -100, -100, -100]),
    np.array([87, 87, 87, 87, 87, 87, 12, 12, 12, 100, 100, 100]),
)

def handle_client(conn):
    """
    クライアントから1回分のリクエストを受け取り、waypoint を返す処理
    """
    try:
        # 1) データ受信
        data = conn.recv(4096)
        if not data:
            return

        # 2) JSONをパース
        received = json.loads(data.decode())
        qpos_start = received["qpos_start"]
        qpos_goal  = received["qpos_goal"]
        num_waypoint = received["num_waypoint"]

        # 3) サンプルとして、開始姿勢から目標姿勢を線形補間した waypoint を作る
        try:
            # print("qpos_start", qpos_start)
            # print("qpos_goal", qpos_goal)
            waypoints_tensor = ur.plan_path(
                qpos_start=qpos_start,
                qpos_goal=qpos_goal,
                num_waypoints=num_waypoint,  # 2s duration
                ignore_collision=True,
                ignore_joint_limit=True,
            )
            waypoints = []
            for wp in waypoints_tensor:
                pose = []
                for p in wp:
                    pose.append(float(p))

                waypoints.append(pose)

        except Exception as e:
            print("[ERROR in generating waypoint]", e)
            waypoints = []

        # 4) JSONでレスポンスを作成して送信
        response = {"waypoint": waypoints}
        conn.sendall(json.dumps(response).encode())

    except Exception as e:
        print("[ERROR in handle_client]", e)
    finally:
        conn.close()

def main():
    # ソケットを作って待ち受けを開始
    with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
        s.bind((HOST, PORT))
        s.listen(5)  # 同時接続数(キュー)
        print(f"Server listening on {HOST}:{PORT}")

        while True:
            conn, addr = s.accept()
            print("Client connected:", addr)
            handle_client(conn)




if __name__ == "__main__":
    main()