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
2418ee35
Commit
2418ee35
authored
Mar 20, 2020
by
Guilhem Saurel
Browse files
[Python 3]
parent
4a97e0ba
Changes
5
Hide whitespace changes
Inline
Side-by-side
python/cmd/cmd_base_vel_est.py
View file @
2418ee35
# plug encoder velocities (with different base vel) to balance controller
from
dynamic_graph.sot.core
import
Stack_of_vector
,
Selec_of_vector
from
dynamic_graph.sot.core.operator
import
Selec_of_vector
,
Stack_of_vector
#robot.q_fd = create_chebi2_lp_filter_Wn_03_N_4('q_filter', robot.timeStep, 36)
from
dynamic_graph.sot.torque_control.filter_differentiator
import
FilterDifferentiator
# Replace force sensor filters
# Compute finite differences of base position
from
dynamic_graph.sot.torque_control.utils.filter_utils
import
(
create_butter_lp_filter_Wn_05_N_3
,
create_chebi2_lp_filter_Wn_03_N_4
)
robot
.
v
=
Stack_of_vector
(
'v'
)
plug
(
robot
.
base_estimator
.
v_flex
,
robot
.
v
.
sin1
)
plug
(
robot
.
filters
.
estimator_kin
.
dx
,
robot
.
v
.
sin2
)
...
...
@@ -7,17 +14,11 @@ robot.v.selec1(0, 6)
robot
.
v
.
selec2
(
0
,
30
)
plug
(
robot
.
v
.
sout
,
robot
.
inv_dyn
.
v
)
# Compute finite differences of base position
from
dynamic_graph.sot.torque_control.utils.filter_utils
import
create_chebi2_lp_filter_Wn_03_N_4
#robot.q_fd = create_chebi2_lp_filter_Wn_03_N_4('q_filter', robot.timeStep, 36)
from
dynamic_graph.sot.torque_control.filter_differentiator
import
FilterDifferentiator
robot
.
q_fd
=
FilterDifferentiator
(
'q_filter'
)
robot
.
q_fd
.
init
(
robot
.
timeStep
,
36
,
(
1.
,
0.
),
(
1.
,
0.
))
plug
(
robot
.
base_estimator
.
q
,
robot
.
q_fd
.
x
)
create_topic
(
robot
.
ros
,
robot
.
q_fd
.
dx
,
'q_fd'
)
# Replace force sensor filters
from
dynamic_graph.sot.torque_control.utils.filter_utils
import
create_butter_lp_filter_Wn_05_N_3
,
create_chebi2_lp_filter_Wn_03_N_4
robot
.
force_LF_filter
=
create_chebi2_lp_filter_Wn_03_N_4
(
'force_LF_filter'
,
robot
.
timeStep
,
6
)
robot
.
force_RF_filter
=
create_chebi2_lp_filter_Wn_03_N_4
(
'force_RF_filter'
,
robot
.
timeStep
,
6
)
plug
(
robot
.
device
.
forceLLEG
,
robot
.
force_LF_filter
.
x
)
...
...
python/dynamic_graph/sot/torque_control/create_entities_utils.py
View file @
2418ee35
...
...
@@ -6,6 +6,7 @@
from
dynamic_graph
import
plug
from
dynamic_graph.sot.core.Latch
import
Latch
from
dynamic_graph.sot.core.operator
import
Add_of_vector
,
Selec_of_vector
from
dynamic_graph.sot.torque_control.admittance_controller
import
AdmittanceController
from
dynamic_graph.sot.torque_control.control_manager
import
ControlManager
from
dynamic_graph.sot.torque_control.current_controller
import
CurrentController
...
...
@@ -24,7 +25,6 @@ NJ = 30
def
create_encoders
(
robot
):
from
dynamic_graph.sot.core
import
Selec_of_vector
encoders
=
Selec_of_vector
(
'qn'
)
plug
(
robot
.
device
.
robotState
,
encoders
.
sin
)
encoders
.
selec
(
6
,
NJ
+
6
)
...
...
@@ -148,7 +148,6 @@ def create_floatingBase(robot):
floatingBase
=
FromLocalToGLobalFrame
(
robot
.
flex_est
,
"FloatingBase"
)
plug
(
robot
.
ff_locator
.
freeflyer_aa
,
floatingBase
.
sinPos
)
from
dynamic_graph.sot.core
import
Selec_of_vector
base_vel_no_flex
=
Selec_of_vector
(
'base_vel_no_flex'
)
plug
(
robot
.
ff_locator
.
v
,
base_vel_no_flex
.
sin
)
base_vel_no_flex
.
selec
(
0
,
6
)
...
...
@@ -281,7 +280,6 @@ def create_balance_controller(robot, conf, motor_params, dt, robot_name='robot')
plug
(
robot
.
ff_locator
.
v
,
ctrl
.
v
)
try
:
from
dynamic_graph.sot.core
import
Selec_of_vector
robot
.
ddq_des
=
Selec_of_vector
(
'ddq_des'
)
plug
(
ctrl
.
dv_des
,
robot
.
ddq_des
.
sin
)
robot
.
ddq_des
.
selec
(
6
,
NJ
+
6
)
...
...
@@ -455,7 +453,6 @@ def create_admittance_ctrl(robot, conf, dt=0.001, robot_name='robot'):
admit_ctrl
.
force_integral_deadzone
.
value
=
conf
.
force_integral_deadzone
# connect it to torque control
from
dynamic_graph.sot.core
import
Add_of_vector
robot
.
sum_torque_adm
=
Add_of_vector
(
'sum_torque_adm'
)
plug
(
robot
.
inv_dyn
.
tau_des
,
robot
.
sum_torque_adm
.
sin1
)
plug
(
admit_ctrl
.
u
,
robot
.
sum_torque_adm
.
sin2
)
...
...
python/dynamic_graph/sot/torque_control/tests/test_balance_ctrl_openhrp.py
View file @
2418ee35
...
...
@@ -8,7 +8,7 @@ from time import sleep
from
dynamic_graph
import
plug
from
dynamic_graph.ros
import
RosPublish
from
dynamic_graph.sot.core
import
Selec_of_vector
from
dynamic_graph.sot.core
.operator
import
Selec_of_vector
from
dynamic_graph.sot.torque_control.create_entities_utils
import
NJ
from
dynamic_graph.sot.torque_control.main
import
main_v3
from
dynamic_graph.sot.torque_control.utils.sot_utils
import
Bunch
,
start_sot
...
...
python/dynamic_graph/sot/torque_control/tests/test_torque_offset_estimator.py.in
View file @
2418ee35
...
...
@@ -4,8 +4,6 @@
@author: Rohan Budhiraja
"""
# ______________________________________________________________________________
# ******************************************************************************
#
...
...
@@ -20,71 +18,107 @@
#-----------------------------------------------------------------------------
#SET THE PATH TO THE URDF AND MESHES
#Define robotName, urdfpath and initialConfig
dt = 5e-3
urdfPath = "@TALOS_DATA_DATAROOTDIR@/talos-data/robots/talos_reduced.urdf"
urdfDir = ["@TALOS_DATA_DATAROOTDIR@"]
robotName = 'TALOS'
OperationalPointsMap = {'left-wrist' : 'arm_left_7_joint',
'right-wrist' : 'arm_right_7_joint',
'left-ankle' : 'leg_left_5_joint',
'right-ankle' : 'leg_right_5_joint',
'gaze' : 'head_2_joint',
'waist' : 'root_joint',
'chest' : 'torso_2_joint'}
initialConfig = (0., 0., 0.648702, 0., 0. , 0., # Free flyer
0., 0., -0.453786, 0.872665, -0.418879, 0., # Left Leg
0., 0., -0.453786, 0.872665, -0.418879, 0., # Right Leg
0., 0., # Chest
0.261799, 0.17453, 0., -0.523599, 0., 0., 0.1,0.0, # Left Arm
#0., 0.,0.,0., 0.,0.,0., # Left gripper
-0.261799, -0.17453, 0., -0.523599, 0., 0., 0.1,0.0, # Right Arm
#0., 0.,0.,0., 0.,0.,0., # Right gripper
0., 0. # Head
)
OperationalPointsMap = {
'left-wrist': 'arm_left_7_joint',
'right-wrist': 'arm_right_7_joint',
'left-ankle': 'leg_left_5_joint',
'right-ankle': 'leg_right_5_joint',
'gaze': 'head_2_joint',
'waist': 'root_joint',
'chest': 'torso_2_joint'
}
initialConfig = (
0.,
0.,
0.648702,
0.,
0.,
0., # Free flyer
0.,
0.,
-0.453786,
0.872665,
-0.418879,
0., # Left Leg
0.,
0.,
-0.453786,
0.872665,
-0.418879,
0., # Right Leg
0.,
0., # Chest
0.261799,
0.17453,
0.,
-0.523599,
0.,
0.,
0.1,
0.0, # Left Arm
#0., 0.,0.,0., 0.,0.,0., # Left gripper
-0.261799,
-0.17453,
0.,
-0.523599,
0.,
0.,
0.1,
0.0, # Right Arm
#0., 0.,0.,0., 0.,0.,0., # Right gripper
0.,
0. # Head
)
pinocchioRobot = RobotWrapper(urdfPath, urdfDir, se3.JointModelFreeFlyer())
import numpy as np
#-----------------------------------------------------------------------------
import pinocchio as se3
from numpy import eye
from pinocchio import Motion, rnea
#---- DYN --------------------------------------------------------------------
#-----------------------------------------------------------------------------
from pinocchio.robot_wrapper import RobotWrapper
import pinocchio as se3
from dynamic_graph.sot.dynamic_pinocchio import fromSotToPinocchio
pinocchioRobot = RobotWrapper(urdfPath, urdfDir, se3.JointModelFreeFlyer())
pinocchioRobot.initDisplay(loadModel=True)
pinocchioRobot.display(fromSotToPinocchio(initialConfig))
from dynamic_graph.sot.dynamic_pinocchio.humanoid_robot import HumanoidRobot
robot = HumanoidRobot(robotName, pinocchioRobot.model,
pinocchioRobot.data, initialConfig, OperationalPointsMap)
njoints = pinocchioRobot.model.nv-6
# ------------------------------------------------------------------------------
# ---- Kinematic Stack of Tasks (SoT) -----------------------------------------
# ------------------------------------------------------------------------------
from dynamic_graph import plug
from dynamic_graph.sot.core import SOT
from dynamic_graph.sot.core.matrix_util import matrixToTuple
# ---- TASK GRIP
from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd
from dynamic_graph.sot.core.sot import SOT
from dynamic_graph.sot.dynamic_pinocchio import fromSotToPinocchio
from dynamic_graph.sot.dynamic_pinocchio.humanoid_robot import HumanoidRobot
# Sensor Calibration Entity.
from dynamic_graph.sot.torque_control.torque_offset_estimator import TorqueOffsetEstimator as TOE
pinocchioRobot.initDisplay(loadModel=True)
pinocchioRobot.display(fromSotToPinocchio(initialConfig))
robot = HumanoidRobot(robotName, pinocchioRobot.model, pinocchioRobot.data, initialConfig, OperationalPointsMap)
njoints = pinocchioRobot.model.nv - 6
sot = SOT('sot')
sot.setSize(robot.dynamic.getDimension())
plug(sot.control,robot.device.control)
plug(sot.control,
robot.device.control)
# ------------------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
# ---- TASK GRIP
from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from numpy import eye
taskRH = MetaTaskKine6d('rh',robot.dynamic,'rh',robot.OperationalPointsMap['right-wrist'])
handMgrip = eye(4); handMgrip[0:3,3] = (0.1,0,0)
taskRH = MetaTaskKine6d('rh', robot.dynamic, 'rh', robot.OperationalPointsMap['right-wrist'])
handMgrip = eye(4)
handMgrip[0:3, 3] = (0.1, 0, 0)
taskRH.opmodif = matrixToTuple(handMgrip)
taskRH.feature.frame('desired')
taskLH
= MetaTaskKine6d('lh',robot.dynamic,'lh',robot.OperationalPointsMap['left-wrist'])
taskLH = MetaTaskKine6d('lh',
robot.dynamic,
'lh',
robot.OperationalPointsMap['left-wrist'])
taskLH.opmodif = matrixToTuple(handMgrip)
taskLH.feature.frame('desired')
...
...
@@ -94,83 +128,71 @@ robot.dynamic.com.recompute(0)
taskCom.featureDes.errorIN.value = robot.dynamic.com.value
taskCom.task.controlGain.value = 10
# --- CONTACTS
#define contactLF and contactRF
for name,joint in [ ['LF',robot.OperationalPointsMap['left-ankle']], ['RF',robot.OperationalPointsMap['right-ankle'] ] ]:
contact = MetaTaskKine6d('contact'+name,robot.dynamic,name,joint)
for name, joint in [['LF', robot.OperationalPointsMap['left-ankle']],
['RF', robot.OperationalPointsMap['right-ankle']]]:
contact = MetaTaskKine6d('contact' + name, robot.dynamic, name, joint)
contact.feature.frame('desired')
contact.gain.setConstant(10)
contact.keep()
locals()['contact'+name] = contact
locals()['contact' + name] = contact
targetRH = (0.5,-0.2,1.0)
targetLH = (0.5,0.2,1.0)
targetRH = (0.5,
-0.2,
1.0)
targetLH = (0.5,
0.2,
1.0)
#addRobotViewer(robot, small=False)
#robot.viewer.updateElementConfig('zmp',target+(0,0,0))
gotoNd(taskRH,targetRH,'111',(4.9,0.9,0.01,0.9))
gotoNd(taskLH,targetLH,'111',(4.9,0.9,0.01,0.9))
gotoNd(taskRH,
targetRH,
'111',
(4.9,
0.9,
0.01,
0.9))
gotoNd(taskLH,
targetLH,
'111',
(4.9,
0.9,
0.01,
0.9))
sot.push(contactRF.task.name)
sot.push(contactLF.task.name)
sot.push(taskCom.task.name)
sot.push(taskLH.task.name)
sot.push(taskRH.task.name)
# Sensor Calibration Entity.
from dynamic_graph.sot.torque_control.torque_offset_estimator import TorqueOffsetEstimator as TOE
import numpy as np
toe = TOE("test_entity_toe")
id4 = ((1.0,0.0,0.0,0.0),(0.0,1.0,0.0,0.0),(0.0,0.0,1.0,0.0),(0.0,0.0,0.0,1.0))
id4 = ((1.0,
0.0,
0.0,
0.0),
(0.0,
1.0,
0.0,
0.0),
(0.0,
0.0,
1.0,
0.0),
(0.0,
0.0,
0.0,
1.0))
epsilon = 0.1
ref_offset = 0.1
tau_offset = np.asarray((ref_offset,)*njoints).squeeze()
toe.init(urdfPath, id4, epsilon,
OperationalPointsMap['waist'],
OperationalPointsMap['chest'])
tau_offset = np.asarray((ref_offset, ) * njoints).squeeze()
toe.init(urdfPath, id4, epsilon, OperationalPointsMap['waist'], OperationalPointsMap['chest'])
plug(robot.device.state, toe.base6d_encoders)
toe.gyroscope.value = (0.,0.,0.)
toe.gyroscope.value = (0.,
0.,
0.)
from pinocchio import Motion
gravity = Motion.Zero()
gravity.linear = np.asarray((0.0,0.0,-9.81))
gravity.linear = np.asarray((0.0,
0.0,
-9.81))
#-------------------------------------------------------------------------------
#----- MAIN LOOP ---------------------------------------------------------------
#-------------------------------------------------------------------------------
robot.device.increment(dt)
from pinocchio import rnea
np.set_printoptions(suppress=True)
def runner(n):
for i in xrange(n):
q_pin = fromSotToPinocchio(robot.device.state.value)
q_pin[0,:6]
= 0.0
q_pin[0,6] = 1.0
nvZero = np.asarray((0.0,
)*
pinocchioRobot.model.nv)
q_pin[0,
:6] = 0.0
q_pin[0,
6] = 1.0
nvZero = np.asarray((0.0,
) *
pinocchioRobot.model.nv)
tau = np.asarray(rnea(pinocchioRobot.model, pinocchioRobot.data, q_pin, nvZero, nvZero)).squeeze()
toe.jointTorques.value = tau_offset + tau[6:]
pinocchioRobot.forwardKinematics(q_pin)
toe.accelerometer.value = np.asarray((pinocchioRobot.data.oMi[15].inverse()
*
gravity).linear).squeeze()
toe.accelerometer.value = np.asarray((pinocchioRobot.data.oMi[15].inverse()
*
gravity).linear).squeeze()
toe.jointTorquesEstimated.recompute(robot.device.state.time)
robot.device.increment(dt)
pinocchioRobot.display(q_pin)
runner(10)
toe.computeOffset(100,1.0)
toe.computeOffset(100,
1.0)
runner(100)
runner(1000)
print "The desired offset is", tau_offset
print "The obtained offset is", toe.getSensorOffsets()
print "The test is passed: ", (np.absolute(np.absolute(np.asarray(toe.getSensorOffsets())).max()-ref_offset)<epsilon)
print "The test is passed: ", (np.absolute(np.absolute(np.asarray(toe.getSensorOffsets())).max() - ref_offset) <
epsilon)
python/dynamic_graph/sot/torque_control/tests/test_velocity_filters.py
View file @
2418ee35
...
...
@@ -3,7 +3,9 @@ import sys
from
time
import
sleep
import
numpy
as
np
from
dynamic_graph
import
plug
from
dynamic_graph.sot.core.operator
import
Selec_of_vector
# from dynamic_graph.sot.torque_control.create_entities_utils import create_estimators
# from dynamic_graph.sot.torque_control.create_entities_utils import create_position_controller
# from dynamic_graph.sot.torque_control.create_entities_utils import create_trajectory_generator
...
...
@@ -57,7 +59,6 @@ def get_default_conf():
def
create_base_encoders
(
robot
):
from
dynamic_graph.sot.core
import
Selec_of_vector
base_encoders
=
Selec_of_vector
(
'base_encoders'
)
plug
(
robot
.
device
.
robotState
,
base_encoders
.
sin
)
base_encoders
.
selec
(
0
,
NJ
+
6
)
...
...
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