Commit e8642f31 authored by student's avatar student
Browse files

Add third exercize with GUI to allow user to play with TSID for balancing a biped

parent 9ff17681
import pinocchio as se3
import tsid
import numpy as np import numpy as np
import numpy.matlib as matlib import numpy.matlib as matlib
from numpy import nan from numpy import nan
from numpy.linalg import norm as norm from numpy.linalg import norm as norm
import os
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
import plot_utils as plut import plot_utils as plut
import gepetto.corbaserver
import time import time
import commands import romeo_conf as conf
from tsid_biped import TsidBiped
np.set_printoptions(precision=3, linewidth=200, suppress=True)
print "".center(conf.LINE_WIDTH,'#')
LINE_WIDTH = 60 print " Test Task Space Inverse Dynamics ".center(conf.LINE_WIDTH, '#')
print "".center(LINE_WIDTH,'#') print "".center(conf.LINE_WIDTH,'#'), '\n'
print " Test Task Space Inverse Dynamics ".center(LINE_WIDTH, '#')
print "".center(LINE_WIDTH,'#'), '\n' tsid = TsidBiped(conf)
lxp = 0.14 # foot length in positive x direction N = conf.N_SIMULATION
lxn = 0.077 # foot length in negative x direction com_pos = matlib.empty((3, N))*nan
lyp = 0.069 # foot length in positive y direction com_vel = matlib.empty((3, N))*nan
lyn = 0.069 # foot length in negative y direction com_acc = matlib.empty((3, N))*nan
lz = 0.105 # foot sole height with respect to ankle joint
mu = 0.3 # friction coefficient com_pos_ref = matlib.empty((3, N))*nan
fMin = 5.0 # minimum normal force com_vel_ref = matlib.empty((3, N))*nan
fMax = 1000.0 # maximum normal force com_acc_ref = matlib.empty((3, N))*nan
rf_frame_name = "RAnkleRoll" # right foot frame name com_acc_des = matlib.empty((3, N))*nan # acc_des = acc_ref - Kp*pos_err - Kd*vel_err
lf_frame_name = "LAnkleRoll" # left foot frame name
contactNormal = np.matrix([0., 0., 1.]).T # direction of the normal to the contact surface t = 0.0
q, v = tsid.q, tsid.v
w_com = 1.0 # weight of center of mass task
w_posture = 1e-3 # weight of joint posture task for i in range(0, N):
w_forceRef = 1e-5 # weight of force regularization task
kp_contact = 10.0 # proportional gain of contact constraint
kp_com = 10.0 # proportional gain of center of mass task
kp_posture = 10.0 # proportional gain of joint posture task
dt = 0.001 # controller time step
PRINT_N = 500 # print every PRINT_N time steps
DISPLAY_N = 25 # update robot configuration in viwewer every DISPLAY_N time steps
N_SIMULATION = 4000 # number of time steps simulated
filename = str(os.path.dirname(os.path.abspath(__file__)))
path = filename + '/../models/romeo'
urdf = path + '/urdf/romeo.urdf'
vector = se3.StdVec_StdString()
vector.extend(item for item in path)
robot = tsid.RobotWrapper(urdf, vector, se3.JointModelFreeFlyer(), False)
srdf = path + '/srdf/romeo_collision.srdf'
# for gepetto viewer
robot_display = se3.RobotWrapper.BuildFromURDF(urdf, [path, ], se3.JointModelFreeFlyer())
l = commands.getstatusoutput("ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l")
if int(l[1]) == 0:
os.system('gepetto-gui &')
time.sleep(1)
cl = gepetto.corbaserver.Client()
gui = cl.gui
robot_display.initDisplay(loadModel=True)
q = se3.getNeutralConfiguration(robot.model(), srdf, False)
q[2] += 0.84
v = np.matrix(np.zeros(robot.nv)).T
robot_display.displayCollisions(False)
robot_display.displayVisuals(True)
robot_display.display(q)
assert robot.model().existFrame(rf_frame_name)
assert robot.model().existFrame(lf_frame_name)
t = 0.0 # time
invdyn = tsid.InverseDynamicsFormulationAccForce("tsid", robot, False)
invdyn.computeProblemData(t, q, v)
data = invdyn.data()
contact_Point = np.matrix(np.ones((3,4)) * lz)
contact_Point[0, :] = [-lxn, -lxn, lxp, lxp]
contact_Point[1, :] = [-lyn, lyp, -lyn, lyp]
contactRF =tsid.Contact6d("contact_rfoot", robot, rf_frame_name, contact_Point, contactNormal, mu, fMin, fMax)
contactRF.setKp(kp_contact * matlib.ones(6).T)
contactRF.setKd(2.0 * np.sqrt(kp_contact) * matlib.ones(6).T)
H_rf_ref = robot.position(data, robot.model().getJointId(rf_frame_name))
contactRF.setReference(H_rf_ref)
invdyn.addRigidContact(contactRF, w_forceRef)
contactLF =tsid.Contact6d("contact_lfoot", robot, lf_frame_name, contact_Point, contactNormal, mu, fMin, fMax)
contactLF.setKp(kp_contact * matlib.ones(6).T)
contactLF.setKd(2.0 * np.sqrt(kp_contact) * matlib.ones(6).T)
H_lf_ref = robot.position(data, robot.model().getJointId(lf_frame_name))
contactLF.setReference(H_lf_ref)
invdyn.addRigidContact(contactLF, w_forceRef)
comTask = tsid.TaskComEquality("task-com", robot)
comTask.setKp(kp_com * matlib.ones(3).T)
comTask.setKd(2.0 * np.sqrt(kp_com) * matlib.ones(3).T)
invdyn.addMotionTask(comTask, w_com, 1, 0.0)
postureTask = tsid.TaskJointPosture("task-posture", robot)
postureTask.setKp(kp_posture * matlib.ones(robot.nv-6).T)
postureTask.setKd(2.0 * np.sqrt(kp_posture) * matlib.ones(robot.nv-6).T)
invdyn.addMotionTask(postureTask, w_posture, 1, 0.0)
com_ref = robot.com(data)
#com_ref[1] += 0.05
trajCom = tsid.TrajectoryEuclidianConstant("traj_com", com_ref)
q_ref = q[7:]
trajPosture = tsid.TrajectoryEuclidianConstant("traj_joint", q_ref)
solver = tsid.SolverHQuadProgFast("qp solver")
solver.resize(invdyn.nVar, invdyn.nEq, invdyn.nIn)
com_pos = matlib.empty((3, N_SIMULATION))*nan
com_vel = matlib.empty((3, N_SIMULATION))*nan
com_acc = matlib.empty((3, N_SIMULATION))*nan
com_pos_ref = matlib.empty((3, N_SIMULATION))*nan
com_vel_ref = matlib.empty((3, N_SIMULATION))*nan
com_acc_ref = matlib.empty((3, N_SIMULATION))*nan
com_acc_des = matlib.empty((3, N_SIMULATION))*nan # acc_des = acc_ref - Kp*pos_err - Kd*vel_err
for i in range(0, N_SIMULATION):
time_start = time.time() time_start = time.time()
sampleCom = trajCom.computeNext() sampleCom = tsid.trajCom.computeNext()
comTask.setReference(sampleCom) tsid.comTask.setReference(sampleCom)
samplePosture = trajPosture.computeNext() samplePosture = tsid.trajPosture.computeNext()
postureTask.setReference(samplePosture) tsid.postureTask.setReference(samplePosture)
HQPData = invdyn.computeProblemData(t, q, v) HQPData = tsid.formulation.computeProblemData(t, q, v)
# if i == 0: HQPData.print_all() # if i == 0: HQPData.print_all()
sol = solver.solve(HQPData) sol = tsid.solver.solve(HQPData)
if(sol.status!=0): if(sol.status!=0):
print "QP problem could not be solved! Error code:", sol.status print "QP problem could not be solved! Error code:", sol.status
break break
tau = invdyn.getActuatorForces(sol) tau = tsid.formulation.getActuatorForces(sol)
dv = invdyn.getAccelerations(sol) dv = tsid.formulation.getAccelerations(sol)
com_pos[:,i] = robot.com(invdyn.data()) com_pos[:,i] = tsid.robot.com(tsid.formulation.data())
com_vel[:,i] = robot.com_vel(invdyn.data()) com_vel[:,i] = tsid.robot.com_vel(tsid.formulation.data())
com_acc[:,i] = comTask.getAcceleration(dv) com_acc[:,i] = tsid.comTask.getAcceleration(dv)
com_pos_ref[:,i] = sampleCom.pos() com_pos_ref[:,i] = sampleCom.pos()
com_vel_ref[:,i] = sampleCom.vel() com_vel_ref[:,i] = sampleCom.vel()
com_acc_ref[:,i] = sampleCom.acc() com_acc_ref[:,i] = sampleCom.acc()
com_acc_des[:,i] = comTask.getDesiredAcceleration com_acc_des[:,i] = tsid.comTask.getDesiredAcceleration
if i%PRINT_N == 0: if i%conf.PRINT_N == 0:
print "Time %.3f"%(t) print "Time %.3f"%(t)
if invdyn.checkContact(contactRF.name, sol): if tsid.formulation.checkContact(tsid.contactRF.name, sol):
f = invdyn.getContactForce(contactRF.name, sol) f = tsid.formulation.getContactForce(tsid.contactRF.name, sol)
print "\tnormal force %s: %.1f"%(contactRF.name.ljust(20,'.'),contactRF.getNormalForce(f)) print "\tnormal force %s: %.1f"%(tsid.contactRF.name.ljust(20,'.'), tsid.contactRF.getNormalForce(f))
if invdyn.checkContact(contactLF.name, sol): if tsid.formulation.checkContact(tsid.contactLF.name, sol):
f = invdyn.getContactForce(contactLF.name, sol) f = tsid.formulation.getContactForce(tsid.contactLF.name, sol)
print "\tnormal force %s: %.1f"%(contactLF.name.ljust(20,'.'),contactLF.getNormalForce(f)) print "\tnormal force %s: %.1f"%(tsid.contactLF.name.ljust(20,'.'), tsid.contactLF.getNormalForce(f))
print "\ttracking err %s: %.3f"%(comTask.name.ljust(20,'.'), norm(comTask.position_error, 2)) print "\ttracking err %s: %.3f"%(tsid.comTask.name.ljust(20,'.'), norm(tsid.comTask.position_error, 2))
print "\t||v||: %.3f\t ||dv||: %.3f"%(norm(v, 2), norm(dv)) print "\t||v||: %.3f\t ||dv||: %.3f"%(norm(v, 2), norm(dv))
v_mean = v + 0.5*dt*dv q, v = tsid.integrate_dv(q, v, dv, conf.dt)
v += dt*dv t += conf.dt
q = se3.integrate(robot.model(), q, dt*v_mean)
t += dt
if i%DISPLAY_N == 0: robot_display.display(q) if i%conf.DISPLAY_N == 0: tsid.robot_display.display(q)
time_spent = time.time() - time_start time_spent = time.time() - time_start
if(time_spent < dt): time.sleep(dt-time_spent) if(time_spent < conf.dt): time.sleep(conf.dt-time_spent)
# PLOT STUFF # PLOT STUFF
time = np.arange(0.0, N_SIMULATION*dt, dt) time = np.arange(0.0, N*conf.dt, conf.dt)
(f, ax) = plut.create_empty_figure(3,1) (f, ax) = plut.create_empty_figure(3,1)
for i in range(3): for i in range(3):
......
import pinocchio as se3
import tsid
import numpy as np import numpy as np
import numpy.matlib as matlib import numpy.matlib as matlib
from numpy import nan from numpy import nan
from numpy.linalg import norm as norm from numpy.linalg import norm as norm
import os
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
import plot_utils as plut import plot_utils as plut
import gepetto.corbaserver
import time import time
import commands import romeo_conf as conf
from tsid_biped import TsidBiped
np.set_printoptions(precision=3, linewidth=200, suppress=True)
print "".center(conf.LINE_WIDTH,'#')
LINE_WIDTH = 60 print " Test Task Space Inverse Dynamics ".center(conf.LINE_WIDTH, '#')
print "".center(LINE_WIDTH,'#') print "".center(conf.LINE_WIDTH,'#'), '\n'
print " Test Task Space Inverse Dynamics ".center(LINE_WIDTH, '#')
print "".center(LINE_WIDTH,'#'), '\n' tsid = TsidBiped(conf)
lxp = 0.14 # foot length in positive x direction N = conf.N_SIMULATION
lxn = 0.077 # foot length in negative x direction com_pos = matlib.empty((3, N))*nan
lyp = 0.069 # foot length in positive y direction com_vel = matlib.empty((3, N))*nan
lyn = 0.069 # foot length in negative y direction com_acc = matlib.empty((3, N))*nan
lz = 0.105 # foot sole height with respect to ankle joint
mu = 0.3 # friction coefficient com_pos_ref = matlib.empty((3, N))*nan
fMin = 5.0 # minimum normal force com_vel_ref = matlib.empty((3, N))*nan
fMax = 1000.0 # maximum normal force com_acc_ref = matlib.empty((3, N))*nan
rf_frame_name = "RAnkleRoll" # right foot frame name com_acc_des = matlib.empty((3, N))*nan # acc_des = acc_ref - Kp*pos_err - Kd*vel_err
lf_frame_name = "LAnkleRoll" # left foot frame name
contactNormal = np.matrix([0., 0., 1.]).T # direction of the normal to the contact surface offset = tsid.robot.com(tsid.formulation.data())
w_com = 1.0 # weight of center of mass task
w_posture = 1e-3 # weight of joint posture task
w_forceRef = 1e-5 # weight of force regularization task
kp_contact = 10.0 # proportional gain of contact constraint
kp_com = 10.0 # proportional gain of center of mass task
kp_posture = 10.0 # proportional gain of joint posture task
dt = 0.001 # controller time step
PRINT_N = 500 # print every PRINT_N time steps
DISPLAY_N = 25 # update robot configuration in viwewer every DISPLAY_N time steps
N_SIMULATION = 4000 # number of time steps simulated
filename = str(os.path.dirname(os.path.abspath(__file__)))
path = filename + '/../models/romeo'
urdf = path + '/urdf/romeo.urdf'
vector = se3.StdVec_StdString()
vector.extend(item for item in path)
robot = tsid.RobotWrapper(urdf, vector, se3.JointModelFreeFlyer(), False)
srdf = path + '/srdf/romeo_collision.srdf'
# for gepetto viewer
robot_display = se3.RobotWrapper.BuildFromURDF(urdf, [path, ], se3.JointModelFreeFlyer())
l = commands.getstatusoutput("ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l")
if int(l[1]) == 0:
os.system('gepetto-gui &')
time.sleep(1)
cl = gepetto.corbaserver.Client()
gui = cl.gui
robot_display.initDisplay(loadModel=True)
q = se3.getNeutralConfiguration(robot.model(), srdf, False)
q[2] += 0.84
v = np.matrix(np.zeros(robot.nv)).T
robot_display.displayCollisions(False)
robot_display.displayVisuals(True)
robot_display.display(q)
assert robot.model().existFrame(rf_frame_name)
assert robot.model().existFrame(lf_frame_name)
t = 0.0 # time
invdyn = tsid.InverseDynamicsFormulationAccForce("tsid", robot, False)
invdyn.computeProblemData(t, q, v)
data = invdyn.data()
contact_Point = np.matrix(np.ones((3,4)) * lz)
contact_Point[0, :] = [-lxn, -lxn, lxp, lxp]
contact_Point[1, :] = [-lyn, lyp, -lyn, lyp]
contactRF =tsid.Contact6d("contact_rfoot", robot, rf_frame_name, contact_Point, contactNormal, mu, fMin, fMax)
contactRF.setKp(kp_contact * matlib.ones(6).T)
contactRF.setKd(2.0 * np.sqrt(kp_contact) * matlib.ones(6).T)
H_rf_ref = robot.position(data, robot.model().getJointId(rf_frame_name))
contactRF.setReference(H_rf_ref)
invdyn.addRigidContact(contactRF, w_forceRef)
contactLF =tsid.Contact6d("contact_lfoot", robot, lf_frame_name, contact_Point, contactNormal, mu, fMin, fMax)
contactLF.setKp(kp_contact * matlib.ones(6).T)
contactLF.setKd(2.0 * np.sqrt(kp_contact) * matlib.ones(6).T)
H_lf_ref = robot.position(data, robot.model().getJointId(lf_frame_name))
contactLF.setReference(H_lf_ref)
invdyn.addRigidContact(contactLF, w_forceRef)
comTask = tsid.TaskComEquality("task-com", robot)
comTask.setKp(kp_com * matlib.ones(3).T)
comTask.setKd(2.0 * np.sqrt(kp_com) * matlib.ones(3).T)
invdyn.addMotionTask(comTask, w_com, 1, 0.0)
postureTask = tsid.TaskJointPosture("task-posture", robot)
postureTask.setKp(kp_posture * matlib.ones(robot.nv-6).T)
postureTask.setKd(2.0 * np.sqrt(kp_posture) * matlib.ones(robot.nv-6).T)
invdyn.addMotionTask(postureTask, w_posture, 1, 0.0)
com_ref = robot.com(data)
trajCom = tsid.TrajectoryEuclidianConstant("traj_com", com_ref)
sampleCom = trajCom.computeNext()
q_ref = q[7:]
trajPosture = tsid.TrajectoryEuclidianConstant("traj_joint", q_ref)
solver = tsid.SolverHQuadProgFast("qp solver")
solver.resize(invdyn.nVar, invdyn.nEq, invdyn.nIn)
com_pos = matlib.empty((3, N_SIMULATION))*nan
com_vel = matlib.empty((3, N_SIMULATION))*nan
com_acc = matlib.empty((3, N_SIMULATION))*nan
com_pos_ref = matlib.empty((3, N_SIMULATION))*nan
com_vel_ref = matlib.empty((3, N_SIMULATION))*nan
com_acc_ref = matlib.empty((3, N_SIMULATION))*nan
com_acc_des = matlib.empty((3, N_SIMULATION))*nan # acc_des = acc_ref - Kp*pos_err - Kd*vel_err
offset = robot.com(data)
amp = np.matrix([0.0, 0.05, 0.0]).T amp = np.matrix([0.0, 0.05, 0.0]).T
two_pi_f = 2*np.pi*np.matrix([0.0, 0.5, 0.0]).T two_pi_f = 2*np.pi*np.matrix([0.0, 0.5, 0.0]).T
two_pi_f_amp = np.multiply(two_pi_f,amp) two_pi_f_amp = np.multiply(two_pi_f,amp)
two_pi_f_squared_amp = np.multiply(two_pi_f, two_pi_f_amp) two_pi_f_squared_amp = np.multiply(two_pi_f, two_pi_f_amp)
for i in range(0, N_SIMULATION): sampleCom = tsid.trajCom.computeNext()
samplePosture = tsid.trajPosture.computeNext()
t = 0.0
q, v = tsid.q, tsid.v
for i in range(0, N):
time_start = time.time() time_start = time.time()
sampleCom.pos(offset + np.multiply(amp, matlib.sin(two_pi_f*t))) sampleCom.pos(offset + np.multiply(amp, matlib.sin(two_pi_f*t)))
sampleCom.vel(np.multiply(two_pi_f_amp, matlib.cos(two_pi_f*t))) sampleCom.vel(np.multiply(two_pi_f_amp, matlib.cos(two_pi_f*t)))
sampleCom.acc(np.multiply(two_pi_f_squared_amp, -matlib.sin(two_pi_f*t))) sampleCom.acc(np.multiply(two_pi_f_squared_amp, -matlib.sin(two_pi_f*t)))
comTask.setReference(sampleCom) tsid.comTask.setReference(sampleCom)
samplePosture = trajPosture.computeNext() tsid.postureTask.setReference(samplePosture)
postureTask.setReference(samplePosture)
HQPData = invdyn.computeProblemData(t, q, v) HQPData = tsid.formulation.computeProblemData(t, q, v)
# if i == 0: HQPData.print_all() # if i == 0: HQPData.print_all()
sol = solver.solve(HQPData) sol = tsid.solver.solve(HQPData)
if(sol.status!=0): if(sol.status!=0):
print "QP problem could not be solved! Error code:", sol.status print "QP problem could not be solved! Error code:", sol.status
break break
tau = invdyn.getActuatorForces(sol) tau = tsid.formulation.getActuatorForces(sol)
dv = invdyn.getAccelerations(sol) dv = tsid.formulation.getAccelerations(sol)
com_pos[:,i] = robot.com(invdyn.data()) com_pos[:,i] = tsid.robot.com(tsid.formulation.data())
com_vel[:,i] = robot.com_vel(invdyn.data()) com_vel[:,i] = tsid.robot.com_vel(tsid.formulation.data())
com_acc[:,i] = comTask.getAcceleration(dv) com_acc[:,i] = tsid.comTask.getAcceleration(dv)
com_pos_ref[:,i] = sampleCom.pos() com_pos_ref[:,i] = sampleCom.pos()
com_vel_ref[:,i] = sampleCom.vel() com_vel_ref[:,i] = sampleCom.vel()
com_acc_ref[:,i] = sampleCom.acc() com_acc_ref[:,i] = sampleCom.acc()
com_acc_des[:,i] = comTask.getDesiredAcceleration com_acc_des[:,i] = tsid.comTask.getDesiredAcceleration
if i%PRINT_N == 0: if i%conf.PRINT_N == 0:
print "Time %.3f"%(t) print "Time %.3f"%(t)
if invdyn.checkContact(contactRF.name, sol): if tsid.formulation.checkContact(tsid.contactRF.name, sol):
f = invdyn.getContactForce(contactRF.name, sol) f = tsid.formulation.getContactForce(tsid.contactRF.name, sol)
print "\tnormal force %s: %.1f"%(contactRF.name.ljust(20,'.'),contactRF.getNormalForce(f)) print "\tnormal force %s: %.1f"%(tsid.contactRF.name.ljust(20,'.'), tsid.contactRF.getNormalForce(f))
if invdyn.checkContact(contactLF.name, sol): if tsid.formulation.checkContact(tsid.contactLF.name, sol):
f = invdyn.getContactForce(contactLF.name, sol) f = tsid.formulation.getContactForce(tsid.contactLF.name, sol)
print "\tnormal force %s: %.1f"%(contactLF.name.ljust(20,'.'),contactLF.getNormalForce(f)) print "\tnormal force %s: %.1f"%(tsid.contactLF.name.ljust(20,'.'), tsid.contactLF.getNormalForce(f))
print "\ttracking err %s: %.3f"%(comTask.name.ljust(20,'.'), norm(comTask.position_error, 2)) print "\ttracking err %s: %.3f"%(tsid.comTask.name.ljust(20,'.'), norm(tsid.comTask.position_error, 2))
print "\t||v||: %.3f\t ||dv||: %.3f"%(norm(v, 2), norm(dv)) print "\t||v||: %.3f\t ||dv||: %.3f"%(norm(v, 2), norm(dv))
v_mean = v + 0.5*dt*dv q, v = tsid.integrate_dv(q, v, dv, conf.dt)
v += dt*dv t += conf.dt
q = se3.integrate(robot.model(), q, dt*v_mean)
t += dt
if i%DISPLAY_N == 0: robot_display.display(q) if i%conf.DISPLAY_N == 0: tsid.robot_display.display(q)
time_spent = time.time() - time_start time_spent = time.time() - time_start
if(time_spent < dt): time.sleep(dt-time_spent) if(time_spent < conf.dt): time.sleep(conf.dt-time_spent)
# PLOT STUFF # PLOT STUFF
time = np.arange(0.0, N_SIMULATION*dt, dt) time = np.arange(0.0, N*conf.dt, conf.dt)
(f, ax) = plut.create_empty_figure(3,1) (f, ax) = plut.create_empty_figure(3,1)
for i in range(3): for i in range(3):
......
# -*- coding: utf-8 -*-
"""
Created on Wed Apr 17 22:31:22 2019
@author: student
"""
import numpy as np
import numpy.matlib as matlib
import pinocchio as pin
import Tkinter as tk
from Tkinter import Scale, Button, Frame, Entry, Label, Tk, mainloop, HORIZONTAL
import threading
class Scale3d:
def __init__(self, master, name, from_, to, tickinterval, length, orient, command):
self.s = 3*[None]
axes = ['X','Y','Z']
for i in range(3):
self.s[i] = Scale(master, label=name+' '+axes[i], from_=from_[i], to=to[i],
tickinterval=tickinterval[i], orient=orient[i], length=length[i], command=command)
self.s[i].pack()
separator = Frame(height=2, bd=1, relief=tk.SUNKEN)
separator.pack(fill=tk.X, padx=5, pady=5)
def get(self):
return self.s[0].get(), self.s[1].get(), self.s[2].get()
class Entry3d:
def __init__(self, master, name):
self.s = 3*[None]
axes = ['X','Y','Z']
for i in range(3):
Label(master, text=name+" "+axes[i]).pack() #side=tk.TOP)
self.s[i] = Entry(master, width=5)
self.s[i].pack() #side=tk.BOTTOM)
separator = Frame(height=1, bd=1, relief=tk.SUNKEN)
separator.pack(fill=tk.X, padx=2, pady=2) #, side=tk.BOTTOM)
def get(self):
try:
return [float(self.s[i].get()) for i in range(3)]
except:
print "could not convert string to float", [self.s[i].get() for i in range(3)]
return 3*[0.0]
scale_com, scale_RF, scale_LF = None, None, None
button_contact_RF, button_contact_LF = None, None
push_robot_active, push_robot_com_vel, com_vel_entry = False, 3*[0.0], None
def update_com_ref_scale(value):
x, y, z = scale_com.get()
tsid.trajCom.setReference(com_0 + np.matrix([1e-2*x, 1e-2*y, 1e-2*z]).T)
def update_RF_ref_scale(value):
x, y, z = scale_RF.get()
H_rf_ref = H_rf_0.copy()
H_rf_ref.translation += + np.matrix([1e-2*x, 1e-2*y, 1e-2*z]).T
tsid.trajRF.setReference(H_rf_ref)
def update_LF_ref_scale(value):
x, y, z = scale_LF.get()
H_lf_ref = H_lf_0.copy()
H_lf_ref.translation += + np.matrix([1e-2*x, 1e-2*y, 1e-2*z]).T
tsid.trajLF.setReference(H_lf_ref)
def switch_contact_RF():
if(tsid.contact_RF_active):
tsid.remove_contact_RF()