sot-torque-control  1.6.4
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
 
Loading...
Searching...
No Matches
test_sinusoid_full.py
Go to the documentation of this file.
1# This uses run_command assuming a prolog
2
3from dynamic_graph import plug
4from dynamic_graph.sot.torque_control import HRP2DevicePosCtrl, TestSinusoidControl
5
6
7def initialize(robot):
8 halfSitting = (
9 0,
10 0,
11 -0.4538,
12 0.8727,
13 -0.4189,
14 0,
15 0,
16 0,
17 -0.4538,
18 0.8727,
19 -0.4189,
20 0,
21 0,
22 0,
23 0,
24 0,
25 0.2618,
26 -0.1745,
27 0,
28 -0.5236,
29 0,
30 0,
31 0,
32 0.2618,
33 0.1745,
34 0,
35 -0.5236,
36 0,
37 0,
38 0,
39 )
40
41 device_position = HRP2DevicePosCtrl("hrp2")
42 nj = 30
43 device_position.resize(nj)
44 device_position.set(halfSitting)
45 device_position.kp.value = nj * (1,)
46 device_position.kd.value = nj * (2 * pow(device_position.kp.value[0], 0.5),)
47 robot.device = device_position
48
49 control = TestSinusoidControl("control")
50 control.setAmplitude(0.5)
51 control.setFrequency(0.1)
52 control.setJointID(3)
53 control.dt.value = robot.timeStep
54 robot.sinControl = control
55
56 plug(robot.device.state, control.positionMeas)
57 plug(control.positionDes, robot.device.control)