Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Stack Of Tasks
sot-torque-control
Commits
bef2bc20
Commit
bef2bc20
authored
Dec 12, 2017
by
Andrea Del Prete
Browse files
Clean-up command files
parent
db24c7c5
Changes
3
Hide whitespace changes
Inline
Side-by-side
python/cmd/cmd_current_ctrl_tuning.py
View file @
bef2bc20
create_topic
(
robot
.
ros
,
robot
.
device
.
currents
,
'i'
);
create_topic
(
robot
.
ros
,
robot
.
device
.
control
,
'ctrl'
);
create_topic
(
robot
.
ros
,
robot
.
ctrl_manager
.
currents_real
,
'i_real'
);
create_topic
(
robot
.
ros
,
robot
.
ctrl_manager
.
pwmDes
,
'i_des'
)
create_topic
(
robot
.
ros
,
robot
.
device
.
robotState
,
'robotState'
)
...
...
python/cmd/cmd_hrp2_balance_ctrl.py
View file @
bef2bc20
...
...
@@ -10,12 +10,13 @@ robot.timeStep=0.0015
robot
=
main_v3
(
robot
,
startSoT
=
True
,
go_half_sitting
=
0
)
go_to_position
(
robot
.
traj_gen
,
30
*
(
0.0
,),
5.0
)
robot
.
inv_dyn
.
w_forces
.
value
=
1e-4
#robot.base_estimator.set_imu_weight(0.0)
plug
(
robot
.
filters
.
estimator_kin
.
dx
,
robot
.
base_estimator
.
joint_velocities
);
#plug(robot.filters.estimator_kin.dx, robot.current_ctrl.dq);
#plug(robot.filters.estimator_kin.dx, robot.torque_ctrl.jointsVelocities);
robot
.
inv_dyn
.
kp_com
.
value
=
(
3
0.0
,
3
0.0
,
5
0.0
)
robot
.
inv_dyn
.
kd_com
.
value
=
(
8
.0
,
8
.0
,
0.0
)
robot
.
inv_dyn
.
kp_com
.
value
=
(
2
0.0
,
2
0.0
,
2
0.0
)
robot
.
inv_dyn
.
kd_com
.
value
=
(
0
.0
,
0
.0
,
0.0
)
robot
.
torque_ctrl
.
KpTorque
.
value
=
30
*
(
0.0
,)
#plug(robot.filters_sg.ft_LF_filter.x_filtered, robot.base_estimator.forceLLEG)
#plug(robot.filters_sg.ft_RF_filter.x_filtered, robot.base_estimator.forceRLEG)
...
...
@@ -58,7 +59,7 @@ go_to_position(robot.traj_gen, robot.halfSitting[6:], 5.0);
robot
.
base_estimator
.
reset_foot_positions
();
robot
.
inv_dyn
.
updateComOffset
()
# start torque control on leg joints
robot
.
ctrl_manager
.
setCtrlMode
(
'rhp-rhy-rhr-rk-rar-rap-lhp-lhr-lhy-
lk-
lar-lap'
,
'torque'
)
robot
.
ctrl_manager
.
setCtrlMode
(
'rhp-rhy-rhr-rk-rar-rap-lhp-lhr-lhy-lar-lap'
,
'torque'
)
# enable integral feedback in torque control
import
dynamic_graph.sot.torque_control.hrp2.motors_parameters
as
motor_params
...
...
@@ -108,7 +109,8 @@ create_topic(robot.ros, robot.device.accelerometer, 'accelerometer');
robot
.
com_traj_gen
.
move
(
2
,
0.85
,
2.0
)
smoothly_set_signal
(
robot
.
torque_ctrl
.
KpTorque
,
30
*
(
1.
,))
smoothly_set_signal
(
robot
.
inv_dyn
.
kp_posture
,
30
*
(
5.
,))
smoothly_set_signal
(
robot
.
inv_dyn
.
kp_posture
,
30
*
(
20.
,))
smoothly_set_signal
(
robot
.
inv_dyn
.
kp_pos
,
30
*
(
50.
,))
robot
.
com_traj_gen
.
stop
(
-
1
)
robot
.
com_traj_gen
.
move
(
1
,
0.03
,
1.5
)
...
...
python/cmd/cmd_test_admittance_ctrl.py
View file @
bef2bc20
from
dynamic_graph.sot.torque_control.main
import
*
from
dynamic_graph.sot.torque_control.utils.sot_utils
import
smoothly_set_signal
,
stop_sot
from
dynamic_graph
import
plug
import
numpy
as
np
robot
.
timeStep
=
0.0015
robot
=
main_v3
(
robot
,
startSoT
=
True
,
go_half_sitting
=
1
)
robot
=
main_v3
(
robot
,
startSoT
=
1
,
go_half_sitting
=
1
)
plug
(
robot
.
filters
.
estimator_kin
.
dx
,
robot
.
base_estimator
.
joint_velocities
);
robot
.
inv_dyn
.
kp_com
.
value
=
(
20.0
,
20.0
,
20.0
)
robot
.
inv_dyn
.
kd_com
.
value
=
(
0.0
,
0.0
,
0.0
)
robot
.
torque_ctrl
.
KpTorque
.
value
=
30
*
(
0.0
,)
robot
.
inv_dyn
.
kp_com
.
value
=
(
20.0
,
20.0
,
20.0
)
robot
.
inv_dyn
.
kd_com
.
value
=
(
5.0
,
5.0
,
0.0
)
robot
.
inv_dyn
.
kp_posture
.
value
=
30
*
(
30.
,)
robot
.
base_estimator
.
reset_foot_positions
();
robot
.
inv_dyn
.
updateComOffset
()
robot
.
ctrl_manager
.
setCtrlMode
(
'rhp-rhy-rhr-rk-rar-rap-lhp-lhr-lhy-lar-lap'
,
'torque'
)
robot
.
traj_gen
.
moveJoint
(
'lk'
,
0.8
,
3.0
)
robot
.
com_traj_gen
.
move
(
2
,
0.85
,
3.0
)
create_topic
(
robot
.
ros
,
robot
.
base_estimator
.
q
,
'q'
);
create_topic
(
robot
.
ros
,
robot
.
adm_ctrl
.
fRightFootRef
,
'f_des_R'
)
create_topic
(
robot
.
ros
,
robot
.
adm_ctrl
.
fLeftFootRef
,
'f_des_L'
)
create_topic
(
robot
.
ros
,
robot
.
adm_ctrl
.
u
,
'u_adm'
)
create_topic
(
robot
.
ros
,
robot
.
adm_ctrl
.
dqDes
,
'dq_des'
)
create_topic
(
robot
.
ros
,
robot
.
estimator_ft
.
contactWrenchLeftSole
,
'f_L'
);
create_topic
(
robot
.
ros
,
robot
.
estimator_ft
.
contactWrenchRightSole
,
'f_R'
);
create_topic
(
robot
.
ros
,
robot
.
inv_dyn
.
com
,
'com'
)
create_topic
(
robot
.
ros
,
robot
.
inv_dyn
.
com_vel
,
'com_vel'
)
create_topic
(
robot
.
ros
,
robot
.
inv_dyn
.
com_ref_pos
,
'com_ref_pos'
)
create_topic
(
robot
.
ros
,
robot
.
inv_dyn
.
com_ref_vel
,
'com_ref_vel'
)
create_topic
(
robot
.
ros
,
robot
.
inv_dyn
.
com_acc_des
,
'com_acc_des'
)
create_topic
(
robot
.
ros
,
robot
.
base_estimator
.
q
,
'q'
);
create_topic
(
robot
.
ros
,
robot
.
base_estimator
.
v
,
'v'
);
create_topic
(
robot
.
ros
,
robot
.
base_estimator
.
v_flex
,
'v_flex'
);
create_topic
(
robot
.
ros
,
robot
.
base_estimator
.
v_kin
,
'v_kin'
);
create_topic
(
robot
.
ros
,
robot
.
base_estimator
.
v_gyr
,
'v_gyr'
);
create_topic
(
robot
.
ros
,
robot
.
inv_dyn
.
zmp_des
,
'cop_des'
)
create_topic
(
robot
.
ros
,
robot
.
inv_dyn
.
zmp
,
'cop'
)
create_topic
(
robot
.
ros
,
robot
.
inv_dyn
.
zmp_des_right_foot
,
'cop_des_R'
)
create_topic
(
robot
.
ros
,
robot
.
inv_dyn
.
zmp_des_left_foot
,
'cop_des_L'
)
create_topic
(
robot
.
ros
,
robot
.
inv_dyn
.
zmp_right_foot
,
'cop_R'
)
create_topic
(
robot
.
ros
,
robot
.
inv_dyn
.
zmp_left_foot
,
'cop_L'
)
create_topic
(
robot
.
ros
,
robot
.
inv_dyn
.
dq_admittance
,
'dq_adm'
)
create_topic
(
robot
.
ros
,
robot
.
ctrl_manager
.
u
,
'i_des'
);
create_topic
(
robot
.
ros
,
robot
.
filters
.
ft_LF_filter
.
x_filtered
,
'f_LF_filt'
)
create_topic
(
robot
.
ros
,
robot
.
filters_sg
.
ft_LF_filter
.
x_filtered
,
'f_LF_filt_sg'
,
robot
,
robot
.
filters_sg
.
ft_LF_filter
)
create_topic
(
robot
.
ros
,
robot
.
inv_dyn
.
zmp_des_right_foot
,
'cop_des_R'
)
create_topic
(
robot
.
ros
,
robot
.
inv_dyn
.
zmp_des_left_foot
,
'cop_des_L'
)
create_topic
(
robot
.
ros
,
robot
.
ctrl_manager
.
u
,
'i_des'
);
create_topic
(
robot
.
ros
,
robot
.
current_ctrl
.
i_real
,
'i_real'
);
robot
.
adm_ctrl
.
force_integral_saturation
.
value
=
(
0
,
0
,
160.0
,
20.0
,
20.0
,
0
)
kp_vel
=
12
*
[
0.5
,]
+
18
*
[
0.0
,]
smoothly_set_signal
(
robot
.
adm_ctrl
.
kp_vel
,
tuple
(
kp_vel
))
#smoothly_set_signal(robot.adm_ctrl.kp_vel, 30*(0.0,))
smoothly_set_signal
(
robot
.
adm_ctrl
.
ki_force
,
(
0
,
0
,
20.0
,
2
,
0
,
0
))
b
=
(
0.00554272
,
0.01108543
,
0.00554272
)
a
=
(
1.
,
-
1.77863178
,
0.80080265
)
b
=
(
1.0
,
0.0
)
a
=
(
1.0
,
0.0
)
robot
.
filters
.
ft_RF_filter
.
switch_filter
(
b
,
a
)
robot
.
filters
.
ft_LF_filter
.
switch_filter
(
b
,
a
)
robot
.
filters
.
ft_RH_filter
.
switch_filter
(
b
,
a
)
robot
.
filters
.
ft_LH_filter
.
switch_filter
(
b
,
a
)
robot
.
filters
.
acc_filter
.
switch_filter
(
b
,
a
)
b
=
(
2.16439898e-05
,
4.43473520e-05
,
-
1.74065002e-05
,
-
8.02197247e-05
,
-
1.74065002e-05
,
4.43473520e-05
,
2.16439898e-05
)
a
=
(
1.
,
-
5.32595322
,
11.89749109
,
-
14.26803139
,
9.68705647
,
-
3.52968633
,
0.53914042
)
robot
.
filters
.
gyro_filter
.
switch_filter
(
b
,
a
)
robot
.
filters
.
estimator_kin
.
switch_filter
(
b
,
a
)
K
=
(
1e10
,
1e10
,
1e10
,
707
,
502
,
1e10
);
robot
.
base_estimator
.
set_stiffness_left_foot
(
K
)
robot
.
base_estimator
.
set_stiffness_right_foot
(
K
)
robot
.
base_estimator
.
set_imu_weight
(
0.0
)
robot
.
inv_dyn
.
w_forces
.
value
=
1e-4
robot
.
base_estimator
.
reset_foot_positions
();
sleep
(
1
)
robot
.
inv_dyn
.
updateComOffset
()
robot
.
ctrl_manager
.
setCtrlMode
(
'rhp-rhy-rhr-rk-rar-rap-lhp-lhr-lhy-lk-lar-lap'
,
'torque'
)
robot
.
com_traj_gen
.
move
(
1
,
0.03
,
1.5
)
robot
.
com_traj_gen
.
startSinusoid
(
1
,
-
0.03
,
2.0
)
robot
.
torque_ctrl
.
KdVel
.
value
=
30
*
(
0.1
,)
ki_vel
=
30
*
[
0.0
,]
ki_vel
[
5
]
=
0.1
ki_vel
[
11
]
=
0.1
robot
.
torque_ctrl
.
KiVel
.
value
=
tuple
(
ki_vel
)
def
print_cop_stats
(
robot
):
cop_L
=
robot
.
inv_dyn
.
zmp_left_foot
.
value
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment