Skip to content

GitLab

Commit 2418ee35 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

[Python 3]

parent 4a97e0ba
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)
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment