Skip to content
GitLab
Commit 4894bb8f authored by Andrea Del Prete's avatar Andrea Del Prete
Browse files

Convert all python scripts from python 2 to python 3 using automatic tool... 

Convert all python scripts from python 2 to python 3 using automatic tool 2to3. Stop using deprecated pinocchio function in test_Formulation.py
parent e18cd9b2
Hide whitespace changes
Inline Side-by-side
exercizes/ex_0_ur5_joint_space_control.py
View file @ 4894bb8f
......@@ -8,14 +8,14 @@ import time
import pinocchio as se3
import tsid
import gepetto.corbaserver
import commands
import subprocess
import os
import ur5_conf as conf
print "".center(conf.LINE_WIDTH,'#')
print " Joint Space Inverse Dynamics - Manipulator ".center(conf.LINE_WIDTH, '#')
print "".center(conf.LINE_WIDTH,'#'), '\n'
print("".center(conf.LINE_WIDTH,'#'))
print(" Joint Space Inverse Dynamics - Manipulator ".center(conf.LINE_WIDTH, '#'))
print("".center(conf.LINE_WIDTH,'#'), '\n')
PLOT_JOINT_POS = 1
PLOT_JOINT_VEL = 1
......@@ -52,12 +52,12 @@ solver.resize(formulation.nVar, formulation.nEq, formulation.nIn)
if(USE_VIEWER):
robot_display = se3.RobotWrapper.BuildFromURDF(conf.urdf, [conf.path, ])
l = commands.getstatusoutput("ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l")
l = subprocess.getstatusoutput("ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l")
if int(l[1]) == 0:
os.system('gepetto-gui &')
time.sleep(1)
gepetto.corbaserver.Client()
robot_display.initDisplay(loadModel=True)
robot_display.initViewer(loadModel=True)
robot_display.displayCollisions(False)
robot_display.displayVisuals(True)
robot_display.display(q0)
......@@ -99,7 +99,7 @@ for i in range(0, N):
HQPData = formulation.computeProblemData(t, q[:,i], v[:,i])
sol = solver.solve(HQPData)
if(sol.status!=0):
print "Time %.3f QP problem could not be solved! Error code:"%t, sol.status
print("Time %.3f QP problem could not be solved! Error code:"%t, sol.status)
break
tau[:,i] = formulation.getActuatorForces(sol)
......@@ -107,8 +107,8 @@ for i in range(0, N):
dv_des[:,i] = postureTask.getDesiredAcceleration
if i%conf.PRINT_N == 0:
print "Time %.3f"%(t)
print "\ttracking err %s: %.3f"%(postureTask.name.ljust(20,'.'), norm(postureTask.position_error, 2))
print("Time %.3f"%(t))
print("\ttracking err %s: %.3f"%(postureTask.name.ljust(20,'.'), norm(postureTask.position_error, 2)))
# numerical integration
v_mean = v[:,i] + 0.5*dt*dv[:,i]
......@@ -126,7 +126,7 @@ for i in range(0, N):
time = np.arange(0.0, N*conf.dt, conf.dt)
if(PLOT_JOINT_POS):
(f, ax) = plut.create_empty_figure(robot.nv/2,2)
(f, ax) = plut.create_empty_figure(int(robot.nv/2),2)
ax = ax.reshape(robot.nv)
for i in range(robot.nv):
ax[i].plot(time, q[i,:-1].A1, label='Joint pos '+str(i))
......@@ -137,7 +137,7 @@ if(PLOT_JOINT_POS):
leg.get_frame().set_alpha(0.5)
if(PLOT_JOINT_VEL):
(f, ax) = plut.create_empty_figure(robot.nv/2,2)
(f, ax) = plut.create_empty_figure(int(robot.nv/2),2)
ax = ax.reshape(robot.nv)
for i in range(robot.nv):
ax[i].plot(time, v[i,:-1].A1, label='Joint vel '+str(i))
......@@ -150,7 +150,7 @@ if(PLOT_JOINT_VEL):
leg.get_frame().set_alpha(0.5)
if(PLOT_JOINT_ACC):
(f, ax) = plut.create_empty_figure(robot.nv/2,2)
(f, ax) = plut.create_empty_figure(int(robot.nv/2),2)
ax = ax.reshape(robot.nv)
for i in range(robot.nv):
ax[i].plot(time, dv[i,:-1].A1, label='Joint acc '+str(i))
......@@ -162,7 +162,7 @@ if(PLOT_JOINT_ACC):
leg.get_frame().set_alpha(0.5)
if(PLOT_TORQUES):
(f, ax) = plut.create_empty_figure(robot.nv/2,2)
(f, ax) = plut.create_empty_figure(int(robot.nv/2),2)
ax = ax.reshape(robot.nv)
for i in range(robot.nv):
ax[i].plot(time, tau[i,:].A1, label='Torque '+str(i))
......
exercizes/ex_1.py
View file @ 4894bb8f
......@@ -8,9 +8,9 @@ import time
import romeo_conf as conf
from tsid_biped import TsidBiped
print "".center(conf.LINE_WIDTH,'#')
print " Test Task Space Inverse Dynamics ".center(conf.LINE_WIDTH, '#')
print "".center(conf.LINE_WIDTH,'#'), '\n'
print("".center(conf.LINE_WIDTH,'#'))
print(" Test Task Space Inverse Dynamics ".center(conf.LINE_WIDTH, '#'))
print("".center(conf.LINE_WIDTH,'#'), '\n')
tsid = TsidBiped(conf)
......@@ -40,7 +40,7 @@ for i in range(0, N):
sol = tsid.solver.solve(HQPData)
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
tau = tsid.formulation.getActuatorForces(sol)
......@@ -55,17 +55,17 @@ for i in range(0, N):
com_acc_des[:,i] = tsid.comTask.getDesiredAcceleration
if i%conf.PRINT_N == 0:
print "Time %.3f"%(t)
print("Time %.3f"%(t))
if tsid.formulation.checkContact(tsid.contactRF.name, sol):
f = tsid.formulation.getContactForce(tsid.contactRF.name, sol)
print "\tnormal force %s: %.1f"%(tsid.contactRF.name.ljust(20,'.'), tsid.contactRF.getNormalForce(f))
print("\tnormal force %s: %.1f"%(tsid.contactRF.name.ljust(20,'.'), tsid.contactRF.getNormalForce(f)))
if tsid.formulation.checkContact(tsid.contactLF.name, sol):
f = tsid.formulation.getContactForce(tsid.contactLF.name, sol)
print "\tnormal force %s: %.1f"%(tsid.contactLF.name.ljust(20,'.'), tsid.contactLF.getNormalForce(f))
print("\tnormal force %s: %.1f"%(tsid.contactLF.name.ljust(20,'.'), tsid.contactLF.getNormalForce(f)))
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("\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)))
q, v = tsid.integrate_dv(q, v, dv, conf.dt)
t += conf.dt
......
exercizes/ex_1_ur5.py
View file @ 4894bb8f
......@@ -10,9 +10,9 @@ from tsid_manipulator import TsidManipulator
#import ur5_conf as conf
import ur5_reaching_conf as conf
print "".center(conf.LINE_WIDTH,'#')
print " Task Space Inverse Dynamics - Manipulator ".center(conf.LINE_WIDTH, '#')
print "".center(conf.LINE_WIDTH,'#'), '\n'
print(("".center(conf.LINE_WIDTH,'#')))
print((" Task Space Inverse Dynamics - Manipulator ".center(conf.LINE_WIDTH, '#')))
print(("".center(conf.LINE_WIDTH,'#'), '\n'))
PLOT_EE_POS = 1
PLOT_EE_VEL = 1
......@@ -68,7 +68,7 @@ for i in range(0, N):
sol = tsid.solver.solve(HQPData)
if(sol.status!=0):
print "Time %.3f QP problem could not be solved! Error code:"%t, sol.status
print(("Time %.3f QP problem could not be solved! Error code:"%t, sol.status))
break
tau[:,i] = tsid.formulation.getActuatorForces(sol)
......@@ -83,8 +83,8 @@ for i in range(0, N):
ee_acc_des[:,i] = tsid.eeTask.getDesiredAcceleration[:3,0]
if i%conf.PRINT_N == 0:
print "Time %.3f"%(t)
print "\ttracking err %s: %.3f"%(tsid.eeTask.name.ljust(20,'.'), norm(tsid.eeTask.position_error, 2))
print(("Time %.3f"%(t)))
print(("\ttracking err %s: %.3f"%(tsid.eeTask.name.ljust(20,'.'), norm(tsid.eeTask.position_error, 2))))
q[:,i+1], v[:,i+1] = tsid.integrate_dv(q[:,i], v[:,i], dv, conf.dt)
t += conf.dt
......@@ -132,7 +132,7 @@ if(PLOT_EE_ACC):
leg.get_frame().set_alpha(0.5)
if(PLOT_TORQUES):
(f, ax) = plut.create_empty_figure(tsid.robot.nv/2,2)
(f, ax) = plut.create_empty_figure(int(tsid.robot.nv/2),2)
ax = ax.reshape(tsid.robot.nv)
for i in range(tsid.robot.nv):
ax[i].plot(time, tau[i,:].A1, label='Torque '+str(i))
......@@ -144,7 +144,7 @@ if(PLOT_TORQUES):
leg.get_frame().set_alpha(0.5)
if(PLOT_JOINT_VEL):
(f, ax) = plut.create_empty_figure(tsid.robot.nv/2,2)
(f, ax) = plut.create_empty_figure(int(tsid.robot.nv/2),2)
ax = ax.reshape(tsid.robot.nv)
for i in range(tsid.robot.nv):
ax[i].plot(time, v[i,:-1].A1, label='Joint vel '+str(i))
......
exercizes/ex_2.py
View file @ 4894bb8f
......@@ -8,9 +8,9 @@ import time
import romeo_conf as conf
from tsid_biped import TsidBiped
print "".center(conf.LINE_WIDTH,'#')
print " Test Task Space Inverse Dynamics ".center(conf.LINE_WIDTH, '#')
print "".center(conf.LINE_WIDTH,'#'), '\n'
print("".center(conf.LINE_WIDTH,'#'))
print(" Test Task Space Inverse Dynamics ".center(conf.LINE_WIDTH, '#'))
print("".center(conf.LINE_WIDTH,'#'), '\n')
tsid = TsidBiped(conf)
......@@ -51,7 +51,7 @@ for i in range(0, N):
sol = tsid.solver.solve(HQPData)
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
tau = tsid.formulation.getActuatorForces(sol)
......@@ -66,17 +66,17 @@ for i in range(0, N):
com_acc_des[:,i] = tsid.comTask.getDesiredAcceleration
if i%conf.PRINT_N == 0:
print "Time %.3f"%(t)
print("Time %.3f"%(t))
if tsid.formulation.checkContact(tsid.contactRF.name, sol):
f = tsid.formulation.getContactForce(tsid.contactRF.name, sol)
print "\tnormal force %s: %.1f"%(tsid.contactRF.name.ljust(20,'.'), tsid.contactRF.getNormalForce(f))
print("\tnormal force %s: %.1f"%(tsid.contactRF.name.ljust(20,'.'), tsid.contactRF.getNormalForce(f)))
if tsid.formulation.checkContact(tsid.contactLF.name, sol):
f = tsid.formulation.getContactForce(tsid.contactLF.name, sol)
print "\tnormal force %s: %.1f"%(tsid.contactLF.name.ljust(20,'.'), tsid.contactLF.getNormalForce(f))
print("\tnormal force %s: %.1f"%(tsid.contactLF.name.ljust(20,'.'), tsid.contactLF.getNormalForce(f)))
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("\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)))
q, v = tsid.integrate_dv(q, v, dv, conf.dt)
t += conf.dt
......
exercizes/ex_3_biped_balance_with_gui.py
View file @ 4894bb8f
......@@ -8,8 +8,8 @@ Created on Wed Apr 17 22:31:22 2019
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 tkinter as tk
from tkinter import Scale, Button, Frame, Entry, Label, Tk, mainloop, HORIZONTAL
import threading
class Scale3d:
......@@ -41,7 +41,7 @@ class Entry3d:
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)]
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
......@@ -132,7 +132,7 @@ def run_simu():
sol = tsid.solver.solve(HQPData)
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
# tau = tsid.formulation.getActuatorForces(sol)
......@@ -173,7 +173,7 @@ def run_simu():
tsid.gui.applyConfiguration('world/lf_ref', x_lf_ref.tolist()+[0,0,0,1.])
if i%1000==0:
print "Average loop time: %.1f (expected is %.1f)"%(1e3*time_avg, 1e3*conf.dt)
print("Average loop time: %.1f (expected is %.1f)"%(1e3*time_avg, 1e3*conf.dt))
time_spent = time.time() - time_start
time_avg = (i*time_avg + time_spent) / (i+1)
......@@ -181,9 +181,9 @@ def run_simu():
if(time_avg < 0.9*conf.dt): time.sleep(conf.dt-time_avg)
print "".center(conf.LINE_WIDTH,'#')
print " Test Task Space Inverse Dynamics ".center(conf.LINE_WIDTH, '#')
print "".center(conf.LINE_WIDTH,'#'), '\n'
print("".center(conf.LINE_WIDTH,'#'))
print(" Test Task Space Inverse Dynamics ".center(conf.LINE_WIDTH, '#'))
print("".center(conf.LINE_WIDTH,'#'), '\n')
tsid = TsidBiped(conf)
com_0 = tsid.robot.com(tsid.formulation.data())
......
exercizes/ex_4_walking.py
View file @ 4894bb8f
......@@ -9,9 +9,9 @@ import ex_4_conf as conf
#import ex_4_long_conf as conf
from tsid_biped import TsidBiped
print "".center(conf.LINE_WIDTH,'#')
print " Test Walking ".center(conf.LINE_WIDTH, '#')
print "".center(conf.LINE_WIDTH,'#'), '\n'
print("".center(conf.LINE_WIDTH,'#'))
print(" Test Walking ".center(conf.LINE_WIDTH, '#'))
print("".center(conf.LINE_WIDTH,'#'), '\n')
PLOT_COM = 1
PLOT_COP = 1
......@@ -73,11 +73,11 @@ for i in range(-N_pre, N+N_post):
time_start = time.time()
if i==0:
print "Starting to walk (remove contact left foot)"
print("Starting to walk (remove contact left foot)")
tsid.remove_contact_LF()
elif i>0 and i<N-1:
if contact_phase[i] != contact_phase[i-1]:
print "Time %.3f Changing contact phase from %s to %s"%(t, contact_phase[i-1], contact_phase[i])
print("Time %.3f Changing contact phase from %s to %s"%(t, contact_phase[i-1], contact_phase[i]))
if contact_phase[i] == 'left':
tsid.add_contact_LF()
tsid.remove_contact_RF()
......@@ -96,10 +96,10 @@ for i in range(-N_pre, N+N_post):
sol = tsid.solver.solve(HQPData)
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
if norm(v,2)>40.0:
print "Time %.3f Velocities are too high, stop everything!"%(t), norm(v)
print("Time %.3f Velocities are too high, stop everything!"%(t), norm(v))
break
if i>0:
......@@ -134,15 +134,15 @@ for i in range(-N_pre, N+N_post):
cop_LF[1,i] = -f_LF[3,i] / f_LF[2,i]
if i%conf.PRINT_N == 0:
print "Time %.3f"%(t)
print("Time %.3f"%(t))
if tsid.formulation.checkContact(tsid.contactRF.name, sol) and i>=0:
print "\tnormal force %s: %.1f"%(tsid.contactRF.name.ljust(20,'.'), f_RF[2,i])
print("\tnormal force %s: %.1f"%(tsid.contactRF.name.ljust(20,'.'), f_RF[2,i]))
if tsid.formulation.checkContact(tsid.contactLF.name, sol) and i>=0:
print "\tnormal force %s: %.1f"%(tsid.contactLF.name.ljust(20,'.'), f_LF[2,i])
print("\tnormal force %s: %.1f"%(tsid.contactLF.name.ljust(20,'.'), f_LF[2,i]))
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("\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)))
q, v = tsid.integrate_dv(q, v, dv, conf.dt)
t += conf.dt
......
exercizes/plot_utils.py
View file @ 4894bb8f
......@@ -4,7 +4,7 @@ Created on Fri Jan 16 09:16:56 2015
@author: adelpret
"""
from __future__ import print_function
import matplotlib as mpl
import matplotlib.pyplot as plt
import matplotlib.ticker as ticker
......@@ -111,7 +111,7 @@ def plotNdQuantityPerSolver(nRows, nCols, quantity, title, solver_names, line_st
ax = ax.reshape(nRows, nCols);
k = 0;
if(x==None):
x = range(quantity.shape[0]);
x = list(range(quantity.shape[0]));
for j in range(nCols):
for i in range(nRows):
if(k<quantity.shape[2]):
......@@ -189,7 +189,7 @@ def plotQuantityPerSolver(quantity, title, solver_names, line_styles, yscale='li
f, ax = plt.subplots();
lw = DEFAULT_LINE_WIDTH;
if(x==None):
x = range(quantity.shape[0]);
x = list(range(quantity.shape[0]));
for i in range(len(solver_names)):
ax.plot(x, quantity[:,i], line_styles[i], alpha=LINE_ALPHA, linewidth=lw);
......
exercizes/tsid_biped.py
View file @ 4894bb8f
......@@ -6,7 +6,7 @@ import numpy.matlib as matlib
import os
import gepetto.corbaserver
import time
import commands
import subprocess
class TsidBiped:
......@@ -24,6 +24,7 @@ class TsidBiped:
self.robot = tsid.RobotWrapper(conf.urdf, vector, se3.JointModelFreeFlyer(), False)
robot = self.robot
self.model = robot.model()
print("srdf", conf.srdf)
pin.loadReferenceConfigurations(self.model, conf.srdf, False)
self.q0 = q = self.model.referenceConfigurations["half_sitting"]
v = np.matrix(np.zeros(robot.nv)).T
......@@ -135,12 +136,12 @@ class TsidBiped:
# for gepetto viewer
if(viewer):
self.robot_display = se3.RobotWrapper.BuildFromURDF(conf.urdf, [conf.path, ], se3.JointModelFreeFlyer())
l = commands.getstatusoutput("ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l")
l = subprocess.getstatusoutput("ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l")
if int(l[1]) == 0:
os.system('gepetto-gui &')
time.sleep(1)
gepetto.corbaserver.Client()
self.robot_display.initDisplay(loadModel=True)
self.robot_display.initViewer(loadModel=True)
self.robot_display.displayCollisions(False)
self.robot_display.displayVisuals(True)
self.robot_display.display(q)
......
exercizes/tsid_manipulator.py
View file @ 4894bb8f
......@@ -5,7 +5,7 @@ import numpy.matlib as matlib
import os
import gepetto.corbaserver
import time
import commands
import subprocess
class TsidManipulator:
......@@ -85,7 +85,7 @@ class TsidManipulator:
# for gepetto viewer
if(viewer):
self.robot_display = se3.RobotWrapper.BuildFromURDF(conf.urdf, [conf.path, ])
l = commands.getstatusoutput("ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l")
l = subprocess.getstatusoutput("ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l")
if int(l[1]) == 0:
os.system('gepetto-gui &')
time.sleep(1)
......
script/test_Constraint.py
View file @ 4894bb8f
from __future__ import print_function
import pinocchio as se3
import tsid
......
script/test_Contact.py
View file @ 4894bb8f
......@@ -3,9 +3,9 @@ import tsid
import numpy as np
import copy
print ""
print "Test Contact"
print ""
print("")
print("Test Contact")
print("")
tol = 1e-5
import os
......@@ -68,4 +68,4 @@ forceGenMat = contact.getForceGeneratorMatrix
assert forceGenMat.shape[0] == 6 and forceGenMat.shape[1] == 12
contact.computeForceRegularizationTask(t, q, v, data)
print "All test is done"
print("All test is done")
script/test_ContactPoint.py
View file @ 4894bb8f
......@@ -3,9 +3,9 @@ import tsid
import numpy as np
import copy
print ""
print "Test Contact"
print ""
print("")
print("Test Contact")
print("")
tol = 1e-5
import os
......@@ -58,4 +58,4 @@ forceGenMat = contact.getForceGeneratorMatrix
assert forceGenMat.shape[0] == 3 and forceGenMat.shape[1] == 3
contact.computeForceRegularizationTask(t, q, v, data)
print "All test is done"
print("All test is done")
script/test_Formulation.py
View file @ 4894bb8f
import pinocchio as se3
from pinocchio import libpinocchio_pywrap as pin
import tsid
import numpy as np
import copy
print ""
print "Test InvDyn"
print ""
print("")
print("Test InvDyn")
print("")
import os
filename = str(os.path.dirname(os.path.abspath(__file__)))
......@@ -18,7 +19,11 @@ robot = tsid.RobotWrapper(urdf, vector, se3.JointModelFreeFlyer(), False)
srdf = path + '/srdf/romeo_collision.srdf'
q = se3.getNeutralConfigurationFromSrdf(robot.model(), srdf, False)
model = robot.model()
pin.loadReferenceConfigurations(model, srdf, False)
q = model.referenceConfigurations["half_sitting"]
q[2] += 0.84
v = np.matrix(np.zeros(robot.nv)).transpose()
......@@ -113,7 +118,7 @@ tau_old = np.matrix(np.zeros(robot.nv-6)).transpose()
for i in range(0, max_it):
if i == REMOVE_CONTACT_N:
print "Start breaking contact right foot"
print("Start breaking contact right foot")
invdyn.removeRigidContact(contactRF.name, CONTACT_TRANSITION_TIME)
sampleCom = trajCom.computeNext()
......@@ -135,17 +140,17 @@ for i in range(0, max_it):
#assert norm(tau-tau_old) < 2e1
tau_old = tau
if i%PRINT_N == 0:
print "Time ", i
print("Time ", i)
if invdyn.checkContact(contactRF.name, sol):
f = invdyn.getContactForce(contactRF.name, sol)
print " ", contactRF.name, "force: ", contactRF.getNormalForce(f)
print(" ", contactRF.name, "force: ", contactRF.getNormalForce(f))
if invdyn.checkContact(contactLF.name, sol):
f = invdyn.getContactForce(contactLF.name, sol)
print " ", contactLF.name, "force: ", contactLF.getNormalForce(f)
print(" ", contactLF.name, "force: ", contactLF.getNormalForce(f))
print " ", comTask.name, " err:", norm(comTask.position_error, 2)
print " ", "v: ", norm(v, 2), "dv: ", norm(dv)
print(" ", comTask.name, " err:", norm(comTask.position_error, 2))
print(" ", "v: ", norm(v, 2), "dv: ", norm(dv))
v += dt*dv
q = se3.integrate(robot.model(), q, dt * v)
......@@ -154,5 +159,5 @@ for i in range(0, max_it):
assert norm(dv) < 1e6
assert norm(v) < 1e6
print "Final COM Position", robot.com(invdyn.data()).transpose()
print "Desired COM Position", com_ref.transpose()
print("Final COM Position", robot.com(invdyn.data()).transpose())
print("Desired COM Position", com_ref.transpose())
script/test_RobotWrapper.py
View file @ 4894bb8f
......@@ -2,9 +2,9 @@ import pinocchio as se3
import tsid
import numpy as np
print ""
print "Test RobotWrapper"
print ""
print("")
print("Test RobotWrapper")
print("")
import os
......@@ -25,11 +25,11 @@ ub[0:3] = 10.0*np.matrix(np.ones(3)).transpose()
ub[3:7] = 1.0*np.matrix(np.ones(4)).transpose()
q = se3.randomConfiguration(robot.model(), lb, ub)
print q.transpose()
print(q.transpose())
data = robot.data()
v = np.matrix(np.ones(robot.nv)).transpose()
robot.computeAllTerms(data, q, v)
print robot.com(data)
print(robot.com(data))
print "All test is done"
print("All test is done")
script/test_Solvers.py
View file @ 4894bb8f
......@@ -4,9 +4,9 @@ import tsid
import numpy as np
import copy
print ""
print "Test Solvers"
print ""
print("")
print("Test Solvers")
print("")
EPS = 1e-3
nTest = 100
......@@ -20,7 +20,7 @@ GRADIENT_PERTURBATION_VARIANCE = 1e-2
HESSIAN_PERTURBATION_VARIANCE = 1e-1
MARGIN_PERC = 1e-3
print "Gonna perform", nTest, "tests with", n, "variables, ", neq, "equalities", nin, "inequalities"
print("Gonna perform", nTest, "tests with", n, "variables, ", neq, "equalities", nin, "inequalities")
solver = tsid.SolverHQuadProg("qp solver")
solver.resize(n, neq, nin)
......@@ -54,17 +54,17 @@ eq_const = tsid.ConstraintEquality("eq1", A_eq, b_eq)
const1 = tsid.ConstraintLevel()
const1.append(1.0, eq_const)
const1.append(1.0, in_const)
print "check constraint level #0"
print("check constraint level #0")
const1.print_all()
const2= tsid.ConstraintLevel()
const2.append(1.0, cost)
print "check constraint level #1"