Simple Tracking: Robot configuration and PD tracking¶
This example demonstrates a PD-controller in zero gravity. Please note that the numeric vector of target positions can be configured semantically with names of degrees of freedom.
Screenshot¶
Code¶
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 | import pydart2 as pydart
import numpy as np
class Controller:
def __init__(self, skel):
self.skel = skel
self.target = None
self.Kp = np.array([0.0] * 6 + [400.0] * (self.skel.ndofs - 6))
self.Kd = np.array([0.0] * 6 + [40.0] * (self.skel.ndofs - 6))
def compute(self):
return -self.Kp * (self.skel.q - self.target) - self.Kd * self.skel.dq
if __name__ == '__main__':
print('Example: bipedStand')
pydart.init()
print('pydart initialization OK')
world = pydart.World(1.0 / 2000.0)
world.set_gravity([0.0, 0.0, 0.0])
print('World OK')
skel = world.add_skeleton('./data/sdf/atlas/atlas_v3_no_head.sdf')
print('Skeleton = ' + str(skel))
# Set joint damping
for dof in skel.dofs:
dof.set_damping_coefficient(80.0)
# Set target pose
target = skel.positions()
target[("l_arm_shy", "r_arm_shy")] = -1.0, 1.0
target[("l_leg_hpy", "r_leg_hpy")] = -1.0, -1.0
target[("l_leg_kny", "r_leg_kny")] = 1.0, 1.0
# Initialize the controller
controller = Controller(skel)
controller.target = target
skel.set_controller(controller)
print('create controller OK')
pydart.gui.viewer.launch(world,
default_camera=1) # Use Z-up camera
|