実際のところ
公式サンプルをベースに、メソッドなどを切り分けて実装してみます
import numpy as np import genesis as gs class FrankaController: def __init__(self, scene): self.scene = scene # エンティティの追加のみを行う self.setup_robot_entity() self.motors_dof = np.arange(7) self.fingers_dof = np.arange(7, 9) self.end_effector = None # ビルド後に設定 def setup_robot_entity(self): """ロボットのエンティティをシーンに追加""" self.franka = self.scene.add_entity( gs.morphs.MJCF(file='xml/franka_emika_panda/panda.xml'), ) def setup_robot_controls(self): """ロボットの制御パラメータを設定（シーンビルド後に呼び出し）""" # エンドエフェクタのリンクを取得 self.end_effector = self.franka.get_link('hand') # 制御ゲインの設定 self.franka.set_dofs_kp( np.array([4500, 4500, 3500, 3500, 2000, 2000, 2000, 100, 100]), ) self.franka.set_dofs_kv( np.array([450, 450, 350, 350, 200, 200, 200, 10, 10]), ) self.franka.set_dofs_force_range( np.array([-87, -87, -87, -87, -12, -12, -12, -100, -100]), np.array([ 87, 87, 87, 87, 12, 12, 12, 100, 100]), ) def move_to_position(self, pos, quat=(0, 1, 0, 0), gripper_width=None, num_steps=100): """指定された位置と姿勢にロボットを移動させる""" qpos = self.franka.inverse_kinematics( link = self.end_effector, pos = np.array(pos), quat = np.array(quat), ) if gripper_width is not None: qpos[-2:] = gripper_width # パスプランニングと実行 path = self.franka.plan_path( qpos_goal = qpos, num_waypoints = num_steps, ) for waypoint in path: self.franka.control_dofs_position(waypoint) self.scene.step() # 到達を確実にするための待機 for _ in range(50): self.scene.step() def grasp_object(self, force=(-0.5, -0.5)): """オブジェクトを把持する""" self.franka.control_dofs_force(np.array(force), self.fingers_dof) for _ in range(100): self.scene.step() def main(): # 初期化 gs.init(backend=gs.gpu) # シーンの設定 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, ) # コントローラの初期化（この時点でFrankaエンティティが追加される） controller = FrankaController(scene) # 環境の設定 scene.add_entity(gs.morphs.Plane()) scene.add_entity( gs.morphs.Box( size = (0.04, 0.04, 0.04), pos = (0.65, 0.0, 0.02), ) ) # シーンのビルド scene.build() # ロボットの制御パラメータを設定 controller.setup_robot_controls() # 把持動作の実行 # 1. 把持前の位置へ移動 controller.move_to_position( pos=(0.65, 0.0, 0.25), gripper_width=0.04, num_steps=200 ) # 2. 把持位置へ降下 controller.move_to_position( pos=(0.65, 0.0, 0.135), num_steps=100 ) # 3. 把持実行 controller.grasp_object() # 4. オブジェクトを持ち上げ controller.move_to_position( pos=(0.65, 0.0, 0.3), num_steps=200 ) if __name__ == "__main__": main()
前回のサンプルとちがい、ちゃんとアームがゼロ点で固定された状態でちゃんと固定され
で、物体をピックしにいく
ピックする
