How to Move a Robot Arm in Genesis
I referred to control_your_robot.py in the official Tutorial and npaka's note article. I'm recording the key points so I don't forget them.
https://github.com/Genesis-Embodied-AI/Genesis/blob/main/examples/tutorials/control_your_robot.py
Introduction to Genesis (3) - Robot Manipulation
https://note.com/npaka/n/n423437e5b168
Preparation
This assumes that the settings such as scene = gs.Scene() explained last time have been completed.
scene.add_entity
Add a plane and the robot body (panda.xml) to the scene. Please see the previous article for details.
plane = scene.add_entity(
gs.morphs.Plane(),
)
franka = scene.add_entity(
gs.morphs.MJCF(
file="xml/franka_emika_panda/panda.xml",
),
)
joints_name = (... )
Defines a tuple of joint names for Franka Panda. "joint1" through "joint7" represent the 7-axis arm, and "finger_joint1" through "finger_joint2" represent the gripper. This is preparation for extracting joint names in a loop in the next line.
joints_name = (
"joint1",
"joint2",
"joint3",
"joint4",
"joint5",
"joint6",
"joint7",
"finger_joint1",
"finger_joint2",
)
motors_dof_idx = [franka.get_joint(name).dofs_idx_local[0] for name in joints_name]
Uses for name in joints_name to sequentially obtain the degree of freedom index corresponding to each joint name.
franka.get_joint(name)
Obtains the degree of freedom (DOF: degree of freedom) index that the joint has in the physics engine.
dofs_idx_local[0]
_local is attached to handle numbers within a specific entity (e.g., Franka).
Since each joint is 1 DOF, [0] is taken to make it a one-dimensional array. Ball joints and other multi-joints are handled differently.
motors_dof_idx = [franka.get_joint(name).dofs_idx_local[0] for name in joints_name]
Parameter Settings for PD (Proportional-Derivative) Control of Each Joint
Genesis doesn't use I (integral) control, so I'll explain PD control.
When it comes to control, PID (proportional-integral-derivative) control is standard, but integral control is not common in robot simulations including Genesis. The reasons are said to be as follows:
- I control (integral) accumulates errors, making the simulation prone to divergence
Noise, numerical errors, various fluctuations, and discretization errors in time steps tend to accumulate, easily causing excessive torque, joint oscillation, and simulation instability. - "Static errors" are originally small, so there's little need for integration
I control is used to reset steady-state deviation (small deviations from target position), but in simulation environments, the effects of friction, load, and backlash are small. - PD control is more compatible with robot control
Combining a spring-damper model with gravity control is more user-friendly
P (Proportional) Control
The variable kp corresponding to the proportional gain of P control is specified with set_dofs_kp() as follows. Kp physically corresponds to "the image of putting a spring in a joint," and larger values push back strongly toward the target value (image of high rigidity), while smaller values approach the target value slowly.
In the example below, the first seven joints (arm section) are set with higher rigidity, and the remaining two joints (gripper section) are set softer.
Note that dofs_idx_local = dofs_idx is the DOF index of each joint set with motors_dof_idx = [...].
franka.set_dofs_kp(
kp = np.array([4500, 4500, 3500, 3500, 2000, 2000, 2000, 100, 100]),
dofs_idx_local = dofs_idx,
)
kv corresponding to D control (derivative gain Kd) is defined with set_dofs_kv() as shown below.
kv is the image of a damping (damper) constant, providing damping proportional to the difference from the target velocity. With a damping ratio of 0.7 to 1.0 as a guide, set kv to about 1/10 of kp.
franka.set_dofs_kv(
kv=np.array([450, 450, 350, 350, 200, 200, 200, 10, 10]),
dofs_idx_local=motors_dof_idx,
)
Below, the "upper and lower limits of force (torque)" are set for each joint of the Franka robot with set_dofs_force_range.
franka.set_dofs_force_range(
lower=np.array([-87, -87, -87, -87, -12, -12, -12, -100, -100]),
upper=np.array([87, 87, 87, 87, 12, 12, 12, 100, 100]),
dofs_idx_local=motors_dof_idx,
)
Moving the Arm to Specified Positions in Simulation
Using set_dofs_position, each joint is set to a specified angle. In this case, PD control is not applied, and the angle changes instantaneously to the specified angle. The joint angles are changed when i in the for loop is 0-50, 50-100, and 100-150. Since there is scene.step(), the arm motion is displayed in the scene window.
# Position
for i in range(150):
if i < 50:
franka.set_dofs_position(np.array([1, 1, 0, 0, 0, 0, 0, 0.04, 0.04]), motors_dof_idx)
elif i < 100:
franka.set_dofs_position(np.array([-1, 0.8, 1, -2, 1, 0.5, -0.5, 0.04, 0.04]), motors_dof_idx)
else:
franka.set_dofs_position(np.array([0, 0, 0, 0, 0, 0, 0, 0, 0]), motors_dof_idx)
scene.step()
Controlling Arm Movement While Changing Camera Position and Recording to Video
Since angles are specified with control_dofs_position instead of set_dofs_position, PD control is applied. Note that velocity and force are specified with control_dofs_velocity and control_dofs_force partway through.
The force being controlled can be known with get_dofs_control_force(motors_dof_idx))
The force being generated can be known with get_dofs_force(motors_dof_idx)
cam.start_recording()
# PD control
for i in range(750):
if i == 0:
franka.control_dofs_position(
np.array([1, 1, 0, 0, 0, 0, 0, 0.04, 0.04]),
motors_dof_idx,
)
elif i == 250:
franka.control_dofs_position(
np.array([-1, 0.8, 1, -2, 1, 0.5, -0.5, 0.04, 0.04]),
motors_dof_idx,
)
elif i == 500:
franka.control_dofs_position(
np.array([0, 0, 0, 0, 0, 0, 0, 0, 0]),
motors_dof_idx,
)
elif i == 750:
# control first dof with velocity, and the rest with position
franka.control_dofs_position(
np.array([0, 0, 0, 0, 0, 0, 0, 0, 0])[1:],
motors_dof_idx[1:],
)
franka.control_dofs_velocity(
np.array([1.0, 0, 0, 0, 0, 0, 0, 0, 0])[:1],
motors_dof_idx[:1],
)
elif i == 1000:
franka.control_dofs_force(
np.array([0, 0, 0, 0, 0, 0, 0, 0, 0]),
motors_dof_idx,
)
# This is the control force computed based on the given control command
# If using force control, it's the same as the given control command
print("control force:", franka.get_dofs_control_force(motors_dof_idx))
# This is the actual force experienced by the dof
print("internal force:", franka.get_dofs_force(motors_dof_idx))
scene.step()
cam.set_pose(
pos=(3.0 * np.sin(i / 60), 3.0 * np.cos(i / 60), 2.5),
lookat=(0, 0, 0.5),
)
cam.render()
cam.stop_recording(save_to_filename="abc.mp4", fps=60)
A script that puts all of the above together is shown below. # Hard reset is rendered to preserve the original, but is not saved as a video. What is saved is the # PD control part. Note that to shorten the video, the maximum value of i in the for loop has been made smaller than in the Tutorial.
import numpy as np
import time
import genesis as gs
########################## init ##########################
gs.init(backend=gs.gpu)
########################## create a scene ##########################
scene = gs.Scene(
viewer_options=gs.options.ViewerOptions(
camera_pos=(0, -3.5, 2.5),
camera_lookat=(0.0, 0.0, 0.5),
camera_fov=30,
max_FPS=60,
),
vis_options=gs.options.VisOptions(
show_world_frame=True,
world_frame_size=1.0,
show_link_frame=False,
show_cameras=False,
plane_reflection=True,
ambient_light=(0.1, 0.1, 0.1),
),
# renderer=gs.renderers.RayTracer(),
renderer=gs.renderers.Rasterizer(),
sim_options=gs.options.SimOptions(
dt=0.01,
),
show_viewer=True,
)
########################## entities ##########################
plane = scene.add_entity(
gs.morphs.Plane(),
)
franka = scene.add_entity(
gs.morphs.MJCF(
file="xml/franka_emika_panda/panda.xml",
),
)
cam = scene.add_camera(
res=(640, 480),
pos=(3.5, 0.0, 2.5),
lookat=(0, 0, 0.5),
fov=30,
GUI=True,
)
########################## build ##########################
scene.build()
time.sleep(2)
joints_name = (
"joint1",
"joint2",
"joint3",
"joint4",
"joint5",
"joint6",
"joint7",
"finger_joint1",
"finger_joint2",
)
motors_dof_idx = [franka.get_joint(name).dofs_idx_local[0] for name in joints_name]
############ Optional: set control gains ############
# set positional gains
franka.set_dofs_kp(
kp=np.array([4500, 4500, 3500, 3500, 2000, 2000, 2000, 100, 100]),
dofs_idx_local=motors_dof_idx,
)
# set velocity gains
franka.set_dofs_kv(
kv=np.array([450, 450, 350, 350, 200, 200, 200, 10, 10]),
dofs_idx_local=motors_dof_idx,
)
# set force range for safety
franka.set_dofs_force_range(
lower=np.array([-87, -87, -87, -87, -12, -12, -12, -100, -100]),
upper=np.array([87, 87, 87, 87, 12, 12, 12, 100, 100]),
dofs_idx_local=motors_dof_idx,
)
# Hard reset
for i in range(150):
if i < 50:
franka.set_dofs_position(np.array([1, 1, 0, 0, 0, 0, 0, 0.04, 0.04]), motors_dof_idx)
elif i < 100:
franka.set_dofs_position(np.array([-1, 0.8, 1, -2, 1, 0.5, -0.5, 0.04, 0.04]), motors_dof_idx)
else:
franka.set_dofs_position(np.array([0, 0, 0, 0, 0, 0, 0, 0, 0]), motors_dof_idx)
scene.step()
cam.start_recording()
# PD control
for i in range(750):
if i == 0:
franka.control_dofs_position(
np.array([1, 1, 0, 0, 0, 0, 0, 0.04, 0.04]),
motors_dof_idx,
)
elif i == 250:
franka.control_dofs_position(
np.array([-1, 0.8, 1, -2, 1, 0.5, -0.5, 0.04, 0.04]),
motors_dof_idx,
)
elif i == 500:
franka.control_dofs_position(
np.array([0, 0, 0, 0, 0, 0, 0, 0, 0]),
motors_dof_idx,
)
elif i == 750:
# control first dof with velocity, and the rest with position
franka.control_dofs_position(
np.array([0, 0, 0, 0, 0, 0, 0, 0, 0])[1:],
motors_dof_idx[1:],
)
franka.control_dofs_velocity(
np.array([1.0, 0, 0, 0, 0, 0, 0, 0, 0])[:1],
motors_dof_idx[:1],
)
elif i == 1000:
franka.control_dofs_force(
np.array([0, 0, 0, 0, 0, 0, 0, 0, 0]),
motors_dof_idx,
)
# This is the control force computed based on the given control command
# If using force control, it's the same as the given control command
print("control force:", franka.get_dofs_control_force(motors_dof_idx))
# This is the actual force experienced by the dof
print("internal force:", franka.get_dofs_force(motors_dof_idx))
scene.step()
cam.set_pose(
pos=(3.0 * np.sin(i / 60), 3.0 * np.cos(i / 60), 2.5),
lookat=(0, 0, 0.5),
)
cam.render()
cam.stop_recording(save_to_filename="abc.mp4", fps=60)