3 from dynamic_graph
import plug
4 from dynamic_graph.sot.torque_control
import HRP2DevicePosCtrl, TestSinusoidControl
8 halfSitting = (0, 0, -0.4538, 0.8727, -0.4189, 0,
9 0, 0, -0.4538, 0.8727, -0.4189, 0,
11 0.2618, -0.1745, 0, -0.5236, 0, 0, 0,
12 0.2618, 0.1745, 0, -0.5236, 0, 0, 0)
14 device_position = HRP2DevicePosCtrl(
"hrp2")
16 device_position.resize(nj)
17 device_position.set(halfSitting)
18 device_position.kp.value = nj * (1, )
19 device_position.kd.value = nj * (2 * pow(device_position.kp.value[0], 0.5), )
20 robot.device = device_position
22 control = TestSinusoidControl(
'control')
23 control.setAmplitude(0.5)
24 control.setFrequency(0.1)
26 control.dt.value = robot.timeStep
27 robot.sinControl = control
29 plug(robot.device.state, control.positionMeas)
30 plug(control.positionDes, robot.device.control)