sot-torque-control  1.5.3
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
test_sinusoid_full.py
Go to the documentation of this file.
1 # This uses run_command assuming a prolog
2 
3 from dynamic_graph import plug
4 from dynamic_graph.sot.torque_control import HRP2DevicePosCtrl, TestSinusoidControl
5 
6 
7 def initialize(robot):
8  halfSitting = (0, 0, -0.4538, 0.8727, -0.4189, 0,
9  0, 0, -0.4538, 0.8727, -0.4189, 0,
10  0, 0, 0, 0,
11  0.2618, -0.1745, 0, -0.5236, 0, 0, 0,
12  0.2618, 0.1745, 0, -0.5236, 0, 0, 0)
13 
14  device_position = HRP2DevicePosCtrl("hrp2")
15  nj = 30
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
21 
22  control = TestSinusoidControl('control')
23  control.setAmplitude(0.5)
24  control.setFrequency(0.1)
25  control.setJointID(3)
26  control.dt.value = robot.timeStep
27  robot.sinControl = control
28 
29  plug(robot.device.state, control.positionMeas)
30  plug(control.positionDes, robot.device.control)
torque_control.test_sinusoid_full.initialize
def initialize(robot)
Definition: test_sinusoid_full.py:7