Skip to content

Commit 3cd6708

Browse files
matthieuvigneduburcqa
matthieuvigne
and
duburcqa
authored
[FEATURE] Implement position-velocity controller. (#1948)
Co-authored-by: Alexis Duburcq <[email protected]>
1 parent 8c3d90a commit 3cd6708

File tree

6 files changed

+270
-45
lines changed

6 files changed

+270
-45
lines changed

‎examples/rigid/control_mesh.py‎

Lines changed: 4 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,5 @@
11
import argparse
22

3-
import numpy as np
4-
53
import genesis as gs
64

75

@@ -45,17 +43,13 @@ def main():
4543
scene.build()
4644

4745
dofs_idx = duck.base_joint.dofs_idx
46+
duck.set_dofs_kp((0.3,) * 6, dofs_idx)
47+
duck.set_dofs_kv((1.0,) * 6, dofs_idx)
4848

49-
duck.set_dofs_kv(
50-
np.array([1, 1, 1, 1, 1, 1]) * 50.0,
51-
dofs_idx,
52-
)
5349
pos = duck.get_dofs_position()
5450
pos[-1] = 1.0 # rotate around intrinsic z axis
55-
duck.control_dofs_position(
56-
pos,
57-
dofs_idx,
58-
)
51+
duck.control_dofs_position(pos, dofs_idx)
52+
5953
for i in range(1000):
6054
scene.step()
6155

‎examples/tutorials/position_control_comparison.py‎

Lines changed: 129 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,129 @@
1+
# This example compares the position control accuracy between 'control_dofs_position' and
2+
# 'control_dofs_position_velocity' when tracking a dynamic trajectory.
3+
# While both are equivalent in static, the former lacks the target velocity term of true PD controller in robotics,
4+
# making it underperform compared to 'control_dofs_position_velocity'.
5+
import argparse
6+
import math
7+
8+
import matplotlib.pyplot as plt
9+
10+
import genesis as gs
11+
12+
13+
def main():
14+
parser = argparse.ArgumentParser()
15+
parser.add_argument("-v", "--vis", action="store_true", default=False)
16+
parser.add_argument("-c", "--cpu", action="store_true", default=False)
17+
args = parser.parse_args()
18+
19+
########################## init ##########################
20+
gs.init(backend=gs.cpu if args.cpu else gs.gpu)
21+
22+
########################## create a scene ##########################
23+
scene = gs.Scene(
24+
viewer_options=gs.options.ViewerOptions(
25+
camera_pos=(0, -3.5, 2.5),
26+
camera_lookat=(0.0, 0.0, 0.5),
27+
camera_fov=30,
28+
max_FPS=None,
29+
),
30+
sim_options=gs.options.SimOptions(
31+
dt=0.005,
32+
),
33+
show_viewer=False,
34+
show_FPS=True,
35+
)
36+
37+
########################## entities ##########################
38+
plane = scene.add_entity(
39+
gs.morphs.Plane(),
40+
)
41+
franka = scene.add_entity(
42+
gs.morphs.MJCF(
43+
file="xml/franka_emika_panda/panda.xml",
44+
),
45+
)
46+
########################## build ##########################
47+
scene.build()
48+
49+
joints_name = (
50+
"joint1",
51+
"joint2",
52+
"joint3",
53+
"joint4",
54+
"joint5",
55+
"joint6",
56+
"joint7",
57+
"finger_joint1",
58+
"finger_joint2",
59+
)
60+
motors_dof_idx = [franka.get_joint(name).dofs_idx_local[0] for name in joints_name]
61+
62+
############ Optional: set control gains ############
63+
# set positional gains
64+
franka.set_dofs_kp(
65+
kp=[4500, 4500, 3500, 3500, 2000, 2000, 2000, 100, 100],
66+
dofs_idx_local=motors_dof_idx,
67+
)
68+
# set velocity gains
69+
franka.set_dofs_kv(
70+
kv=[450, 450, 350, 350, 200, 200, 200, 10, 10],
71+
dofs_idx_local=motors_dof_idx,
72+
)
73+
# set force range for safety
74+
franka.set_dofs_force_range(
75+
lower=[-87, -87, -87, -87, -12, -12, -12, -100, -100],
76+
upper=[87, 87, 87, 87, 12, 12, 12, 100, 100],
77+
dofs_idx_local=motors_dof_idx,
78+
)
79+
# Hard reset
80+
# Follow a sinusoid trajectory
81+
A = 0.5 # motion amplitude, rad
82+
f = 1.0 # motion frequency, Hz
83+
84+
# Use control_dofs_position
85+
pos_simulation_result = []
86+
franka.set_dofs_position([A, 0, 0, 0, 0, 0, 0, 0, 0], motors_dof_idx)
87+
t0 = scene.t
88+
while (t := (scene.t - t0) * scene.dt) < 2.0:
89+
target_position = A * (1 + math.sin(2 * math.pi * f * t))
90+
91+
current_position = float(franka.get_qpos()[0])
92+
pos_simulation_result.append([t, current_position, target_position])
93+
franka.control_dofs_position([target_position, 0, 0, 0, 0, 0, 0, 0, 0], motors_dof_idx)
94+
scene.step()
95+
96+
# Use control_dofs_position_velocity
97+
pos_vel_simulation_result = []
98+
franka.set_dofs_position([A, 0, 0, 0, 0, 0, 0, 0, 0], motors_dof_idx)
99+
t0 = scene.t
100+
while (t := (scene.t - t0) * scene.dt) < 2.0:
101+
target_position = A * (1 + math.sin(2 * math.pi * f * t))
102+
target_velocity = 2 * math.pi * f * A * math.cos(2 * math.pi * f * t)
103+
104+
current_position = float(franka.get_qpos()[0])
105+
pos_vel_simulation_result.append([t, current_position, target_position])
106+
franka.control_dofs_position_velocity(
107+
[target_position, 0, 0, 0, 0, 0, 0, 0, 0],
108+
[target_velocity, 0, 0, 0, 0, 0, 0, 0, 0],
109+
motors_dof_idx,
110+
)
111+
scene.step()
112+
113+
# Plot results
114+
pos_simulation_result = tuple(zip(*pos_simulation_result))
115+
pos_vel_simulation_result = tuple(zip(*pos_vel_simulation_result))
116+
117+
plt.plot(pos_simulation_result[0], pos_simulation_result[1], label="control_dofs_position")
118+
plt.plot(pos_vel_simulation_result[0], pos_vel_simulation_result[1], label="control_dofs_position_velocity")
119+
plt.plot(pos_vel_simulation_result[0], pos_vel_simulation_result[2], color="black", label="Target position")
120+
plt.xlabel("Time (s)")
121+
plt.ylabel("Joint position (rad)")
122+
plt.title("Comparison of joint position tracking with two different controllers")
123+
plt.grid()
124+
plt.legend()
125+
plt.show()
126+
127+
128+
if __name__ == "__main__":
129+
main()

‎genesis/engine/entities/hybrid_entity.py‎

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -309,6 +309,22 @@ def control_dofs_position(self, *args, **kwargs):
309309
"""
310310
return self._part_rigid.control_dofs_position(*args, **kwargs)
311311

312+
def control_dofs_position_velocity(self, *args, **kwargs):
313+
"""
314+
Apply position control to the rigid part of the hybrid entity.
315+
316+
Parameters
317+
----------
318+
*args, **kwargs
319+
Passed directly to the rigid entity's control_dofs_position method.
320+
321+
Returns
322+
-------
323+
gs.Tensor
324+
Control output for position adjustment of the rigid body's DOFs.
325+
"""
326+
return self._part_rigid.control_dofs_position_velocity(*args, **kwargs)
327+
312328
def control_dofs_velocity(self, *args, **kwargs):
313329
"""
314330
Apply velocity control to the rigid part of the hybrid entity.

‎genesis/engine/entities/rigid_entity/rigid_entity.py‎

Lines changed: 22 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -2262,19 +2262,39 @@ def control_dofs_velocity(self, velocity, dofs_idx_local=None, envs_idx=None, *,
22622262
@gs.assert_built
22632263
def control_dofs_position(self, position, dofs_idx_local=None, envs_idx=None, *, unsafe=False):
22642264
"""
2265-
Set the PD controller's target position for the entity's dofs. This is used for position control.
2265+
Set the position controller's target position for the entity's dofs. The controller is a proportional term
2266+
plus a velocity damping term (virtual friction).
22662267
22672268
Parameters
22682269
----------
22692270
position : array_like
22702271
The target position to set.
2272+
dofs_idx_local : array_like, optional
2273+
The indices of the dofs to control. If None, all dofs will be controlled. Note that here this uses the local `q_idx`, not the scene-level one. Defaults to None.
2274+
envs_idx : array_like, optional
2275+
The indices of the environments. If None, all environments will be considered. Defaults to None.
2276+
"""
2277+
dofs_idx = self._get_idx(dofs_idx_local, self.n_dofs, self._dof_start, unsafe=True)
2278+
self._solver.control_dofs_position(position, dofs_idx, envs_idx, unsafe=unsafe)
2279+
2280+
@gs.assert_built
2281+
def control_dofs_position_velocity(self, position, velocity, dofs_idx_local=None, envs_idx=None, *, unsafe=False):
2282+
"""
2283+
Set a PD controller's target position and velocity for the entity's dofs. This is used for position control.
2284+
2285+
Parameters
2286+
----------
2287+
position : array_like
2288+
The target position to set.
2289+
velocity : array_like
2290+
The target velocity
22712291
dofs_idx_local : None | array_like, optional
22722292
The indices of the dofs to control. If None, all dofs will be controlled. Note that here this uses the local `q_idx`, not the scene-level one. Defaults to None.
22732293
envs_idx : None | array_like, optional
22742294
The indices of the environments. If None, all environments will be considered. Defaults to None.
22752295
"""
22762296
dofs_idx = self._get_idx(dofs_idx_local, self.n_dofs, self._dof_start, unsafe=True)
2277-
self._solver.control_dofs_position(position, dofs_idx, envs_idx, unsafe=unsafe)
2297+
self._solver.control_dofs_position_velocity(position, velocity, dofs_idx, envs_idx, unsafe=unsafe)
22782298

22792299
@gs.assert_built
22802300
def get_qpos(self, qs_idx_local=None, envs_idx=None, *, unsafe=False):

‎genesis/engine/solvers/rigid/rigid_solver_decomp.py‎

Lines changed: 65 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -1876,9 +1876,7 @@ def control_dofs_velocity(self, velocity, dofs_idx=None, envs_idx=None, *, unsaf
18761876
velocity, dofs_idx, envs_idx, self.dofs_state, self.dofs_info, self._static_rigid_sim_config
18771877
)
18781878
if not unsafe and not has_gains:
1879-
raise gs.raise_exception(
1880-
"Please set control gains kp,kv using `set_dofs_kp`,`set_dofs_kv` prior to calling this method."
1881-
)
1879+
raise gs.raise_exception("Please set control gains kv using `set_dofs_kv` prior to calling this method.")
18821880

18831881
def control_dofs_position(self, position, dofs_idx=None, envs_idx=None, *, unsafe=False):
18841882
position, dofs_idx, envs_idx = self._sanitize_1D_io_variables(
@@ -1889,9 +1887,25 @@ def control_dofs_position(self, position, dofs_idx=None, envs_idx=None, *, unsaf
18891887
has_gains = kernel_control_dofs_position(
18901888
position, dofs_idx, envs_idx, self.dofs_state, self.dofs_info, self._static_rigid_sim_config
18911889
)
1890+
if not unsafe and not has_gains:
1891+
raise gs.raise_exception("Please set control gains kp using `set_dofs_kp` prior to calling this method.")
1892+
1893+
def control_dofs_position_velocity(self, position, velocity, dofs_idx=None, envs_idx=None, *, unsafe=False):
1894+
position, dofs_idx, _ = self._sanitize_1D_io_variables(
1895+
position, dofs_idx, self.n_dofs, envs_idx, skip_allocation=True, unsafe=unsafe
1896+
)
1897+
velocity, dofs_idx, envs_idx = self._sanitize_1D_io_variables(
1898+
velocity, dofs_idx, self.n_dofs, envs_idx, skip_allocation=True, unsafe=unsafe
1899+
)
1900+
if self.n_envs == 0:
1901+
position = position.unsqueeze(0)
1902+
velocity = velocity.unsqueeze(0)
1903+
has_gains = kernel_control_dofs_position_velocity(
1904+
position, velocity, dofs_idx, envs_idx, self.dofs_state, self.dofs_info, self._static_rigid_sim_config
1905+
)
18921906
if not unsafe and not has_gains:
18931907
raise gs.raise_exception(
1894-
"Please set control gains kp,kv using `set_dofs_kp`,`set_dofs_kv` prior to calling this method."
1908+
"Please set control gains kp and/or kv using `set_dofs_kp`,`set_dofs_kv` prior to calling this method."
18951909
)
18961910

18971911
def get_sol_params(self, geoms_idx=None, envs_idx=None, *, joints_idx=None, eqs_idx=None, unsafe=False):
@@ -3232,8 +3246,9 @@ def func_compute_mass_matrix(
32323246
rigid_global_info.mass_mat[i_d, i_d, i_b] += (
32333247
dofs_info.damping[I_d] * rigid_global_info.substep_dt[None]
32343248
)
3235-
if (dofs_state.ctrl_mode[i_d, i_b] == gs.CTRL_MODE.POSITION) or (
3236-
dofs_state.ctrl_mode[i_d, i_b] == gs.CTRL_MODE.VELOCITY
3249+
if (
3250+
dofs_state.ctrl_mode[i_d, i_b] == gs.CTRL_MODE.POSITION
3251+
or dofs_state.ctrl_mode[i_d, i_b] == gs.CTRL_MODE.VELOCITY
32373252
):
32383253
# qM += d qfrc_actuator / d qvel
32393254
rigid_global_info.mass_mat[i_d, i_d, i_b] += (
@@ -3308,8 +3323,9 @@ def func_compute_mass_matrix(
33083323
for i_d, i_b in ti.ndrange(n_dofs, _B):
33093324
I_d = [i_d, i_b] if ti.static(static_rigid_sim_config.batch_dofs_info) else i_d
33103325
rigid_global_info.mass_mat[i_d, i_d, i_b] += dofs_info.damping[I_d] * rigid_global_info.substep_dt[None]
3311-
if (dofs_state.ctrl_mode[i_d, i_b] == gs.CTRL_MODE.POSITION) or (
3312-
dofs_state.ctrl_mode[i_d, i_b] == gs.CTRL_MODE.VELOCITY
3326+
if (
3327+
dofs_state.ctrl_mode[i_d, i_b] == gs.CTRL_MODE.POSITION
3328+
or dofs_state.ctrl_mode[i_d, i_b] == gs.CTRL_MODE.VELOCITY
33133329
):
33143330
# qM += d qfrc_actuator / d qvel
33153331
rigid_global_info.mass_mat[i_d, i_d, i_b] += dofs_info.kv[I_d] * rigid_global_info.substep_dt[None]
@@ -3351,8 +3367,9 @@ def func_factor_mass(
33513367
dofs_info.damping[I_d] * rigid_global_info.substep_dt[None]
33523368
)
33533369
if ti.static(static_rigid_sim_config.integrator == gs.integrator.implicitfast):
3354-
if (dofs_state.ctrl_mode[i_d, i_b] == gs.CTRL_MODE.POSITION) or (
3355-
dofs_state.ctrl_mode[i_d, i_b] == gs.CTRL_MODE.VELOCITY
3370+
if (
3371+
dofs_state.ctrl_mode[i_d, i_b] == gs.CTRL_MODE.POSITION
3372+
or dofs_state.ctrl_mode[i_d, i_b] == gs.CTRL_MODE.VELOCITY
33563373
):
33573374
rigid_global_info.mass_mat_L[i_d, i_d, i_b] += (
33583375
dofs_info.kv[I_d] * rigid_global_info.substep_dt[None]
@@ -3391,8 +3408,9 @@ def func_factor_mass(
33913408
dofs_info.damping[I_d] * rigid_global_info.substep_dt[None]
33923409
)
33933410
if ti.static(static_rigid_sim_config.integrator == gs.integrator.implicitfast):
3394-
if (dofs_state.ctrl_mode[i_d, i_b] == gs.CTRL_MODE.POSITION) or (
3395-
dofs_state.ctrl_mode[i_d, i_b] == gs.CTRL_MODE.VELOCITY
3411+
if (
3412+
dofs_state.ctrl_mode[i_d, i_b] == gs.CTRL_MODE.POSITION
3413+
or dofs_state.ctrl_mode[i_d, i_b] == gs.CTRL_MODE.VELOCITY
33963414
):
33973415
rigid_global_info.mass_mat_L[i_d, i_d, i_b] += (
33983416
dofs_info.kv[I_d] * rigid_global_info.substep_dt[None]
@@ -4044,8 +4062,8 @@ def func_implicit_damping(
40444062
rigid_global_info.mass_mat_mask[i_e, i_b] = True
40454063
if ti.static(static_rigid_sim_config.integrator != gs.integrator.Euler):
40464064
if (
4047-
(dofs_state.ctrl_mode[i_d, i_b] == gs.CTRL_MODE.POSITION)
4048-
or (dofs_state.ctrl_mode[i_d, i_b] == gs.CTRL_MODE.VELOCITY)
4065+
dofs_state.ctrl_mode[i_d, i_b] == gs.CTRL_MODE.POSITION
4066+
or dofs_state.ctrl_mode[i_d, i_b] == gs.CTRL_MODE.VELOCITY
40494067
) and dofs_info.kv[I_d] > gs.EPS:
40504068
rigid_global_info.mass_mat_mask[i_e, i_b] = True
40514069

@@ -5320,10 +5338,9 @@ def func_torque_and_passive_force(
53205338
elif dofs_state.ctrl_mode[i_d, i_b] == gs.CTRL_MODE.POSITION and not (
53215339
joint_type == gs.JOINT_TYPE.FREE and i_d >= links_info.dof_start[I_l] + 3
53225340
):
5323-
force = (
5324-
dofs_info.kp[I_d] * (dofs_state.ctrl_pos[i_d, i_b] - dofs_state.pos[i_d, i_b])
5325-
- dofs_info.kv[I_d] * dofs_state.vel[i_d, i_b]
5326-
)
5341+
force = dofs_info.kp[I_d] * (
5342+
dofs_state.ctrl_pos[i_d, i_b] - dofs_state.pos[i_d, i_b]
5343+
) + dofs_info.kv[I_d] * (dofs_state.ctrl_vel[i_d, i_b] - dofs_state.vel[i_d, i_b])
53275344

53285345
dofs_state.qf_applied[i_d, i_b] = ti.math.clamp(
53295346
force,
@@ -6643,7 +6660,7 @@ def kernel_control_dofs_velocity(
66436660
I_d = [i_d, i_b] if ti.static(static_rigid_sim_config.batch_dofs_info) else i_d
66446661
dofs_state.ctrl_mode[i_d, i_b] = gs.CTRL_MODE.VELOCITY
66456662
dofs_state.ctrl_vel[i_d, i_b] = velocity[i_b_, i_d_]
6646-
if (dofs_info.kp[I_d] > gs.EPS) | (dofs_info.kv[I_d] > gs.EPS):
6663+
if dofs_info.kv[I_d] > gs.EPS:
66476664
has_gains = True
66486665
return has_gains
66496666

@@ -6666,6 +6683,32 @@ def kernel_control_dofs_position(
66666683
I_d = [i_d, i_b] if ti.static(static_rigid_sim_config.batch_dofs_info) else i_d
66676684
dofs_state.ctrl_mode[i_d, i_b] = gs.CTRL_MODE.POSITION
66686685
dofs_state.ctrl_pos[i_d, i_b] = position[i_b_, i_d_]
6686+
dofs_state.ctrl_vel[i_d, i_b] = 0.0
6687+
if dofs_info.kp[I_d] > gs.EPS:
6688+
has_gains = True
6689+
return has_gains
6690+
6691+
6692+
@ti.kernel(fastcache=gs.use_fastcache)
6693+
def kernel_control_dofs_position_velocity(
6694+
position: ti.types.ndarray(),
6695+
velocity: ti.types.ndarray(),
6696+
dofs_idx: ti.types.ndarray(),
6697+
envs_idx: ti.types.ndarray(),
6698+
dofs_state: array_class.DofsState,
6699+
dofs_info: array_class.DofsInfo,
6700+
static_rigid_sim_config: ti.template(),
6701+
) -> ti.i32:
6702+
has_gains = gs.ti_bool(False)
6703+
ti.loop_config(serialize=ti.static(static_rigid_sim_config.para_level < gs.PARA_LEVEL.PARTIAL))
6704+
for i_d_, i_b_ in ti.ndrange(dofs_idx.shape[0], envs_idx.shape[0]):
6705+
i_d = dofs_idx[i_d_]
6706+
i_b = envs_idx[i_b_]
6707+
6708+
I_d = [i_d, i_b] if ti.static(static_rigid_sim_config.batch_dofs_info) else i_d
6709+
dofs_state.ctrl_mode[i_d, i_b] = gs.CTRL_MODE.POSITION
6710+
dofs_state.ctrl_pos[i_d, i_b] = position[i_b_, i_d_]
6711+
dofs_state.ctrl_vel[i_d, i_b] = velocity[i_b_, i_d_]
66696712
if (dofs_info.kp[I_d] > gs.EPS) | (dofs_info.kv[I_d] > gs.EPS):
66706713
has_gains = True
66716714
return has_gains
@@ -6747,10 +6790,9 @@ def kernel_get_dofs_control_force(
67476790
elif dofs_state.ctrl_mode[i_d, i_b] == gs.CTRL_MODE.VELOCITY:
67486791
force = dofs_info.kv[I_d] * (dofs_state.ctrl_vel[i_d, i_b] - dofs_state.vel[i_d, i_b])
67496792
elif dofs_state.ctrl_mode[i_d, i_b] == gs.CTRL_MODE.POSITION:
6750-
force = (
6751-
dofs_info.kp[I_d] * (dofs_state.ctrl_pos[i_d, i_b] - dofs_state.pos[i_d, i_b])
6752-
- dofs_info.kv[I_d] * dofs_state.vel[i_d, i_b]
6753-
)
6793+
force = dofs_info.kp[I_d] * (dofs_state.ctrl_pos[i_d, i_b] - dofs_state.pos[i_d, i_b]) + dofs_info.kv[
6794+
I_d
6795+
] * (dofs_state.ctrl_vel[i_d, i_b] - dofs_state.vel[i_d, i_b])
67546796
tensor[i_b_, i_d_] = ti.math.clamp(
67556797
force,
67566798
dofs_info.force_range[I_d][0],

0 commit comments

Comments
 (0)