Skip to content
Snippets Groups Projects
Commit a9c1b26a authored by Gabriele Buondonno's avatar Gabriele Buondonno
Browse files

[zmp] connect robot.dynamic.velocity and robot.dynamic.acceleration in test_dcmZmpControl.py

parent d4df0753
No related branches found
No related tags found
No related merge requests found
...@@ -16,11 +16,8 @@ dt = robot.timeStep; ...@@ -16,11 +16,8 @@ dt = robot.timeStep;
# --- Desired values # --- Desired values
robot.dynamic.com.recompute(0) robot.dynamic.com.recompute(0)
comDes = robot.dynamic.com.value comDes = robot.dynamic.com.value
#dcmDes = comDes dcmDes = comDes
#zmpDes = comDes[:2] + (0.0,) zmpDes = comDes[:2] + (0.0,)
robot.dynamic.zmp.recompute(0)
dcmDes = robot.dynamic.zmp.value
zmpDes = robot.dynamic.zmp.value
ddcomDes = (0.0,0.0,0.0) ddcomDes = (0.0,0.0,0.0)
# --- Pendulum parameters # --- Pendulum parameters
...@@ -70,7 +67,7 @@ robot.zmp_estimator = robot.dynamic ...@@ -70,7 +67,7 @@ robot.zmp_estimator = robot.dynamic
# -------------------------- ADMITTANCE CONTROL -------------------------- # -------------------------- ADMITTANCE CONTROL --------------------------
# --- DCM controller # --- DCM controller
Kp_dcm = [500.0,500.0,500.0] Kp_dcm = [1.0,1.0,1.0]
Ki_dcm = [0.0,0.0,0.0] # zero (to be set later) Ki_dcm = [0.0,0.0,0.0] # zero (to be set later)
gamma_dcm = 0.2 gamma_dcm = 0.2
...@@ -92,7 +89,7 @@ dcm_controller.init(dt) ...@@ -92,7 +89,7 @@ dcm_controller.init(dt)
robot.dcm_control = dcm_controller robot.dcm_control = dcm_controller
Ki_dcm = [1.0,1.0,0.0] # to be set later Ki_dcm = [0.1,0.1,0.0] # this value is employed later
# --- CoM admittance controller # --- CoM admittance controller
Kp_adm = [0.0,0.0,0.0] # zero (to be set later) Kp_adm = [0.0,0.0,0.0] # zero (to be set later)
...@@ -108,7 +105,7 @@ com_admittance_control.setState(comDes,[0.0,0.0,0.0]) ...@@ -108,7 +105,7 @@ com_admittance_control.setState(comDes,[0.0,0.0,0.0])
robot.com_admittance_control = com_admittance_control robot.com_admittance_control = com_admittance_control
Kp_adm = [20.0,10.0,0.0] # to be set later Kp_adm = [1.0,1.0,0.0] # this value is employed later
# -------------------------- SOT CONTROL -------------------------- # -------------------------- SOT CONTROL --------------------------
...@@ -143,6 +140,14 @@ robot.sot.push(robot.contactLF.task.name) ...@@ -143,6 +140,14 @@ robot.sot.push(robot.contactLF.task.name)
robot.sot.push(robot.taskCom.task.name) robot.sot.push(robot.taskCom.task.name)
robot.device.control.recompute(0) robot.device.control.recompute(0)
# --- Fix robot.dynamic inputs
plug(robot.sot.control,robot.dynamic.velocity)
from dynamic_graph.sot.core import Derivator_of_Vector
robot.dvdt = Derivator_of_Vector("dv_dt")
robot.dvdt.dt.value = dt
plug(robot.sot.control,robot.dvdt.sin)
plug(robot.dvdt.sout,robot.dynamic.acceleration)
# -------------------------- PLOTS -------------------------- # -------------------------- PLOTS --------------------------
# --- ROS PUBLISHER # --- ROS PUBLISHER
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment