16 lines
624 B
Python
16 lines
624 B
Python
|
class PDController:
|
||
|
|
def __init__(self, p_gains, d_gains):
|
||
|
|
self.p_gains = p_gains
|
||
|
|
self.d_gains = d_gains
|
||
|
|
|
||
|
|
def get_action(self, env, des_pos, des_vel):
|
||
|
|
# TODO: make standardized ALRenv such that all of them have current_pos/vel attributes
|
||
|
|
cur_pos = env.current_pos
|
||
|
|
cur_vel = env.current_vel
|
||
|
|
if len(des_pos) != len(cur_pos):
|
||
|
|
des_pos = env.extend_des_pos(des_pos)
|
||
|
|
if len(des_vel) != len(cur_vel):
|
||
|
|
des_vel = env.extend_des_vel(des_vel)
|
||
|
|
trq = self.p_gains * (des_pos - cur_pos) + self.d_gains * (des_vel - cur_vel)
|
||
|
|
return trq
|