def test_admittance_ctrl(device, ctrl_manager, traj_gen, estimator_ft, adm_ctrl, dt=0.001):
if(USE_ROBOT_VIEWER):
viewer=robotviewer.client('XML-RPC');
N = 10*1000;
axis = 1;
fDes = np.zeros((N,6));
f = np.zeros((N,6));
ctrl_manager.setCtrlMode('all', 'adm');
traj_gen.startForceSinusoid('lf',axis, 100, 1.5); #name, axis, final force, time
adm_ctrl.fLeftFoot.value = 6*(0,);
adm_ctrl.fRightFoot.value = 6*(0,);
for i in range(N):
if(i==1500):
traj_gen.stopForce('lf');
traj_gen.startForceSinusoid('lf',axis, -100, 1.5); #name, axis, final force, time
device.increment (dt);
sleep(dt);
fDes[i,:] = traj_gen.fLeftFoot.value;
f[i,:] = estimator_ft.contactWrenchLeftFoot.value;
if(USE_ROBOT_VIEWER and i%30==0):
viewer.updateElementConfig('hrp_device', list(device.state.value)+[0.0,]*10);
if(i%100==0):
# print "f(%.3f) = %.3f, \tfDes = %.3f" % (device.robotState.time*dt,f[i,axis],fDes[i,axis]);
print ("fLeftFootError", adm_ctrl.fLeftFootError.value);
print ("fRightFootError", adm_ctrl.fRightFootError.value);
print ("dqDes", adm_ctrl.dqDes.value);
(fig,ax) = create_empty_figure(3,1);
ax[0].plot(fDes[:,0],'r'); ax[0].plot(f[:,0],'b');
ax[1].plot(fDes[:,1],'r'); ax[1].plot(f[:,1],'b');
ax[2].plot(fDes[:,2],'r'); ax[2].plot(f[:,2],'b');
plt.show();
def test_min_jerk(device, traj_gen):
print "\nGonna move joint to -1.5...";
tt = 1.5;
jid = 2;
traj_gen.moveJoint('rhp', -1.5, tt);
N = int(tt/dt);
qMinJerk = np.zeros(N+2);
dqMinJerk = np.zeros(N+2);
ddqMinJerk = np.zeros(N+2);
q = np.zeros(N+2);
dq = np.zeros(N+2);
if(USE_ROBOT_VIEWER):
viewer=robotviewer.client('XML-RPC');
for i in range(N+2):
device.increment (dt);
qMinJerk[i] = traj_gen.q.value[jid];
dqMinJerk[i] = traj_gen.dq.value[jid];
ddqMinJerk[i] = traj_gen.ddq.value[jid];
q[i] = device.robotState.value[6+jid];
dq[i] = device.jointsVelocities.value[jid];
if(USE_ROBOT_VIEWER and i%30==0):
viewer.updateElementConfig('hrp_device', list(device.state.value)+[0.0,]*10);
if(i%100==0):
print "q(%.3f) = %.3f, \tqDes = %.3f" % (device.robotState.time*dt,device.robotState.value[6+jid],traj_gen.q.value[jid]);
dq_fd = np.diff(qMinJerk)/dt;
ddq_fd = np.diff(dqMinJerk)/dt;
(fig,ax) = create_empty_figure(3,1);
ax[0].plot(qMinJerk,'r'); ax[0].plot(q,'b');
ax[1].plot(dqMinJerk,'r'); ax[1].plot(dq,'b'); ax[1].plot(dq_fd,'g--');
ax[2].plot(ddqMinJerk,'r'); ax[2].plot(ddq_fd,'g--');
plt.show();
def compute_estimates_from_sensors(sensors, delay, ftSensorOffsets=None, USE_FT_SENSORS=True):
NJ = 30; # number of joints
m = sensors['time'].shape[0]; # number of time steps
dt = float(np.mean(sensors['time'][1:]-sensors['time'][:-1])); # sampling period
# Create and initialize estimator
estimator = ForceTorqueEstimator("estimator"+str(np.random.rand()));
estimator.dqRef.value = NJ*(0.0,);
estimator.ddqRef.value = NJ*(0.0,);
#Only use inertia model (not current) to estimate torques.
estimator.wCurrentTrust.value = NJ*(0.0,);
estimator.currentMeasure.value = NJ*(0.0,);
estimator.currentMeasure.value = NJ*(0.0,);
estimator.saturationCurrent.value = NJ*(0.0,);
estimator.motorParameterKt_p.value = tuple(30*[1.,])
estimator.motorParameterKt_n.value = tuple(30*[1.,])
estimator.motorParameterKf_p.value = tuple(30*[0.,])
estimator.motorParameterKf_n.value = tuple(30*[0.,])
estimator.motorParameterKv_p.value = tuple(30*[0.,])
estimator.motorParameterKv_n.value = tuple(30*[0.,])
estimator.motorParameterKa_p.value = tuple(30*[0.,])
estimator.motorParameterKa_n.value = tuple(30*[0.,])
set_sensor_data_in_estimator(estimator, sensors[0]);
print "Time step: %f" % dt;
print "Estimation delay: %f" % delay;
if(ftSensorOffsets==None):
estimator.init(dt,delay,delay,delay,delay,delay,True);
else:
estimator.init(dt,delay,delay,delay,delay,delay,False);
estimator.setFTsensorOffsets(tuple(ftSensorOffsets));
estimator.setUseRawEncoders(False);
estimator.setUseRefJointVel(False);
estimator.setUseRefJointAcc(False);
estimator.setUseFTsensors(USE_FT_SENSORS);
torques = np.zeros((m,NJ));
dq = np.zeros((m,NJ));
ddq = np.zeros((m,NJ));
if(USE_ROBOT_VIEWER):
viewer=robotviewer.client('XML-RPC');
for i in range(m):
if(USE_ROBOT_VIEWER and i%10==0):
viewer.updateElementConfig('hrp_device', [0,0,0.7,0,0,0] + sensors['enc'][i,:].tolist());
set_sensor_data_in_estimator(estimator, sensors[i]);
estimator.jointsTorques.recompute(i);
torques[i,:] = np.array(estimator.jointsTorques.value);
dq[i,:] = np.array(estimator.jointsVelocities.value);
ddq[i,:] = np.array(estimator.jointsAccelerations.value);
if(i==2):
print ("F/T sensor offsets are: ", estimator.getFTsensorOffsets());
if(i%1000==0):
print 'Estimation time: \t %.3f' % (i*dt);
return (torques, dq, ddq);
def initRobotViewer():
"""Initialize robotviewer is possible."""
clt = None
if hasRobotViewer:
try:
clt = robotviewer.client()
except:
print "failed to connect to robotviewer"
return clt
def initRobotViewer():
"""Initialize robotviewer is possible."""
clt = None
if hasRobotViewer:
try:
clt = robotviewer.client()
except:
print "failed to connect to robotviewer"
return clt
def simulate(device, duration, dt=0.001):
N = int(duration/dt);
if(USE_ROBOT_VIEWER):
viewer=robotviewer.client('XML-RPC');
for i in range(1,N):
device.increment (dt);
sleep(dt);
if(USE_ROBOT_VIEWER):
viewer.updateElementConfig('hrp_device', list(device.state.value)+[0.0,]*10);
def test_force_estimator(device,
estimator_ft,
estimator_kin,
dt=0.001,
estimationDelay=5):
N = 1000
# test duration (in number of timesteps)
freq = 0.5
# frequency
A = 100
# amplitude
f = np.zeros((4, 6))
f_est = np.zeros((4, 6))
err_f = np.zeros((4, 6))
count = 0
t = 0
if (USE_ROBOT_VIEWER):
viewer = robotviewer.client('XML-RPC')
print "Start simulation..."
for i in range(1, N):
t += dt
axis = int(random.random() * 6)
for j in range(4):
f[j,
axis] += A * (np.sin(2 * 3.14 * t) - np.sin(2 * 3.14 * (t - dt)))
device.inputForceLARM.value = tuple(f[0, :])
device.inputForceRARM.value = tuple(f[1, :])
device.inputForceLLEG.value = tuple(f[2, :])
device.inputForceRLEG.value = tuple(f[3, :])
device.increment(dt)
if (USE_ROBOT_VIEWER):
viewer.updateElementConfig('hrp_device',
list(device.state.value) + [
0.0,
] * 10)
if (i % estimationDelay == 0):
f_est[0, :] = np.array(estimator_ft.contactWrenchLeftHand.value)
f_est[1, :] = np.array(estimator_ft.contactWrenchRightHand.value)
f_est[2, :] = np.array(estimator_ft.contactWrenchLeftFoot.value)
f_est[3, :] = np.array(estimator_ft.contactWrenchRightFoot.value)
if (i > estimationDelay):
print("Time", i, "f est", f_est[0, :], "f real", f[0, :])
err_f += abs(f_est - f)
count += 1
print "*************************************************************************************************\n"
print "Amplitude of force variation: %.2f" % (A)
print "Frequency of force variation: %.2f" % (freq)
print "Test duration: %.3f s" % (N * dt)
print "Time step: %.3f s" % dt
print "Estimation delay: %.3f" % (estimationDelay * dt)
if (count > 0):
print "Average percentual errors:"
print(f_est / count)
def test_chirp(device, traj_gen, estimator_ft):
device.after.addDownsampledSignal('estimator_ft.ftSensRightFootPrediction',
1)
print "Start linear chirp from %f to -1.0" % device.robotState.value[6 + 2]
tt = 4
jid = 1
traj_gen.startLinChirp('rhr', -0.6, 0.2, 2.0, tt)
N = int(tt / dt)
qChirp = np.zeros(N + 2)
dqChirp = np.zeros(N + 2)
ddqChirp = np.zeros(N + 2)
q = np.zeros(N + 2)
dq = np.zeros(N + 2)
if (USE_ROBOT_VIEWER):
viewer = robotviewer.client('XML-RPC')
viewer.updateElementConfig('hrp_device',
list(device.state.value) + [
0.0,
] * 10)
for i in range(N + 2):
device.increment(dt)
qChirp[i] = traj_gen.q.value[jid]
dqChirp[i] = traj_gen.dq.value[jid]
ddqChirp[i] = traj_gen.ddq.value[jid]
q[i] = device.robotState.value[6 + jid]
dq[i] = device.jointsVelocities.value[jid]
# print "q(%.3f) = %.3f, \tqDes = %.3f" % (device.robotState.time*dt,device.robotState.value[6+jid],traj_gen.q.value[jid]);
# print "dq(%.3f) = %.3f, \tdqDes = %.3f" % (device.robotState.time*dt,device.jointsVelocities.value[6+jid],traj_gen.dq.value[jid]);
# print "tauDes(%.3f)= %.3f" % (device.robotState.time*dt,inv_dyn.tauDes.value[jid]);
# sleep(0.1);
if (USE_ROBOT_VIEWER and i % 30 == 0):
viewer.updateElementConfig('hrp_device',
list(device.state.value) + [
0.0,
] * 10)
if (i % 100 == 0):
print("FT sensor right foot prediction: ",
estimator_ft.ftSensRightFootPrediction.value)
# print "q(%.3f) = %.3f, \tqDes = %.3f" % (device.robotState.time*dt,device.robotState.value[6+jid],traj_gen.q.value[jid]);
dq_fd = np.diff(qChirp) / dt
ddq_fd = np.diff(dqChirp) / dt
(fig, ax) = create_empty_figure(3, 1)
ax[0].plot(qChirp, 'r')
ax[0].plot(q, 'b')
ax[1].plot(dqChirp, 'r')
ax[1].plot(dq, 'b')
ax[1].plot(dq_fd, 'g--')
ax[2].plot(ddqChirp, 'r')
ax[2].plot(ddq_fd, 'g--')
plt.show()
def test_vel_acc_estimator(device, estimator_ft, estimator_kin, dt=0.001, estimationDelay = 5):
j = 0; # index of joint under analysis
N = 1000; # test duration (in number of timesteps)
q = device.state.value;
rad2deg = 180/3.14;
q_real = rad2deg*q[j+6];
dq_real = 0;
ddq_real = 0;
err_q = 0;
err_dq = 0;
err_ddq = 0;
count = 0;
t = estimator_ft.jointsTorques.time;
if(USE_ROBOT_VIEWER):
viewer=robotviewer.client('XML-RPC');
print "Start simulation..."
for i in range(1,N):
device.increment (dt);
if(USE_ROBOT_VIEWER):
viewer.updateElementConfig('hrp_device', list(device.state.value)+[0.0,]*10);
estimator_ft.jointsTorques.recompute (t+i);
if(i%estimationDelay == 0):
q_real = rad2deg*device.robotState.value[j+6];
dq_real = rad2deg*device.jointsVelocities.value[j];
ddq_real = rad2deg*device.jointsAccelerations.value[j];
if(i>estimationDelay):
print "\nTime %.3f) q est VS real = (%.2f, %.2f), dq est VS real (%.2f, %.2f) ddq est VS real (%.2f, %.2f)" % (i*dt, \
rad2deg*estimator_kin.x_filtered.value[j], q_real, \
rad2deg*estimator_kin.dx.value[j], dq_real, \
rad2deg*estimator_kin.ddx.value[j], ddq_real);
print "Torque %f" % (estimator_ft.jointsTorques.value[j]);
if(abs(ddq_real)<1e-2):
ddq_real = 1e-2;
if(q_real!=0.0):
err_q += abs((rad2deg*estimator_kin.x_filtered.value[j] - q_real)/q_real);
if(dq_real!=0.0):
err_dq += abs((rad2deg*estimator_kin.dx.value[j] - dq_real)/dq_real);
if(ddq_real!=0.0):
err_ddq += abs((rad2deg*estimator_kin.ddx.value[j] - ddq_real)/ddq_real);
count += 1;
print "*************************************************************************************************\n";
print "Testing joint %d" % j;
print "Initial position of joint %d: %.2f" % (j, rad2deg*q[6+j]);
print "Desired position of joint %d: %.2f" % (j, rad2deg*device.control.value[j]);
print "Test duration: %.3f s" % (N*dt);
print "Time step: %.3f s" % dt;
print "Estimation delay: %.3f" % (estimationDelay*dt);
if(count>0):
print "Average percentual errors:\n* position: %.2f %%\n* velocity: %.2f %%\n* acceleration: %.2f %%" % \
(100*err_q/count, 100*err_dq/count, 100*err_ddq/count);
def test_force_jacobians(device, estimator_ft, torque_ctrl, traj_gen,
ctrl_manager, inv_dyn, dt):
if (USE_ROBOT_VIEWER):
viewer = robotviewer.client('XML-RPC')
ctrl_manager.setCtrlMode("rhr", "torque")
ctrl_manager.setCtrlMode("rhp", "torque")
ctrl_manager.setCtrlMode("rhy", "torque")
ctrl_manager.setCtrlMode("rk", "torque")
ctrl_manager.setCtrlMode("rap", "torque")
ctrl_manager.setCtrlMode("rar", "torque")
inv_dyn.Kp.value = NJ * (0.0, )
inv_dyn.Kd.value = NJ * (0.0, )
inv_dyn.Kf.value = (6 * 4) * (1.0, )
N = 1
#10*1000;
axis = 2
FORCE_MAX = 0.1
tauFB1 = np.zeros((N, 30))
tauFB2 = np.zeros((N, 30))
traj_gen.startForceSinusoid('rf', axis, FORCE_MAX, 1.5)
#name, axis, final force, time
for i in range(N):
if (i == 1500):
traj_gen.stopForce('rf')
traj_gen.startForceSinusoid('rf', axis, -FORCE_MAX, 1.5)
#name, axis, final force, time
device.increment(dt)
sleep(dt)
if (USE_ROBOT_VIEWER and i % 100 == 0):
viewer.updateElementConfig('hrp_device',
list(device.state.value) + [
0.0,
] * 10)
inv_dyn.tauFB2.recompute(i)
tauFB1[i, :] = inv_dyn.tauFB.value
tauFB2[i, :] = inv_dyn.tauFB2.value
(fig, ax) = create_empty_figure(3, 1)
ax[0].plot(tauFB1[:, 0], 'r')
ax[0].plot(tauFB2[:, 0], 'b--')
ax[1].plot(tauFB1[:, 1], 'r')
ax[1].plot(tauFB2[:, 1], 'b--')
ax[2].plot(tauFB1[:, 2], 'r')
ax[2].plot(tauFB2[:, 2], 'b--')
(fig, ax) = create_empty_figure(3, 1)
ax[0].plot(tauFB1[:, 3], 'r')
ax[0].plot(tauFB2[:, 3], 'b--')
ax[1].plot(tauFB1[:, 4], 'r')
ax[1].plot(tauFB2[:, 4], 'b--')
ax[2].plot(tauFB1[:, 5], 'r')
ax[2].plot(tauFB2[:, 5], 'b--')
plt.show()
def addRobotViewer(robot, **args):
'''
Arguments are:
* small=False
* server='XML-RPC' == { 'XML-RPC' | 'CORBA' }
* verbose=True
'''
verbose = args.get('verbose', True)
small = args.get('small', False)
small_extra = args.get('small_extra', 10)
server = args.get('server', 'XML-RPC')
try:
import robotviewer
RobotClass = robot.__class__
if small:
if verbose:
print('using a small robot')
RobotClass.stateFullSize = lambda x: \
stateFullSize(x, small_extra * (0.0, ))
else:
RobotClass.stateFullSize = stateFullSize
robot.viewer = robotviewer.client(server)
RobotClass.refresh = refreshView
RobotClass.incrementNoView = RobotClass.increment
RobotClass.increment = incrementView
RobotClass.setNoView = RobotClass.set
RobotClass.set = setView
robot.displayList = []
# Check the connection
if args.get('dorefresh', True):
robot.refresh()
except Exception:
if verbose:
print("No robot viewer, sorry.")
robot.viewer = RobotViewerFaked()
def test_min_jerk(device, traj_gen, dt=0.001):
print("\nGonna move joint to -1.5...")
tt = 1.5
jid = 2
traj_gen.moveJoint('rhp', -1.5, tt)
N = int(tt / dt)
qMinJerk = np.zeros(N + 2)
dqMinJerk = np.zeros(N + 2)
ddqMinJerk = np.zeros(N + 2)
q = np.zeros(N + 2)
dq = np.zeros(N + 2)
if (USE_ROBOT_VIEWER):
viewer = robotviewer.client('XML-RPC')
for i in range(N + 2):
device.increment(dt)
qMinJerk[i] = traj_gen.q.value[jid]
dqMinJerk[i] = traj_gen.dq.value[jid]
ddqMinJerk[i] = traj_gen.ddq.value[jid]
q[i] = device.robotState.value[6 + jid]
dq[i] = device.jointsVelocities.value[jid]
if (USE_ROBOT_VIEWER and i % 30 == 0):
viewer.updateElementConfig('hrp_device',
list(device.state.value) + [0.0] * 10)
if i % 100 == 0:
print("q(%.3f) = %.3f, \tqDes = %.3f" %
(device.robotState.time * dt,
device.robotState.value[6 + jid], traj_gen.q.value[jid]))
dq_fd = np.diff(qMinJerk) / dt
ddq_fd = np.diff(dqMinJerk) / dt
(fig, ax) = create_empty_figure(3, 1)
ax[0].plot(qMinJerk, 'r')
ax[0].plot(q, 'b')
ax[1].plot(dqMinJerk, 'r')
ax[1].plot(dq, 'b')
ax[1].plot(dq_fd, 'g--')
ax[2].plot(ddqMinJerk, 'r')
ax[2].plot(ddq_fd, 'g--')
plt.show()
def addRobotViewer(robot,**args):
'''
Arguments are:
* small=False
* server='XML-RPC' == { 'XML-RPC' | 'CORBA' }
* verbose=True
'''
verbose = args.get('verbose',True)
small = args.get('small',False)
small_extra = args.get('small_extra',10)
server = args.get('server','XML-RPC')
try:
import robotviewer
RobotClass = robot.__class__
if small:
if verbose: print 'using a small robot'
RobotClass.stateFullSize = lambda x: stateFullSize(x,small_extra*(0.0,))
else: RobotClass.stateFullSize = stateFullSize
robot.viewer = robotviewer.client(server)
RobotClass.refresh = refreshView
RobotClass.incrementNoView = RobotClass.increment
RobotClass.increment = incrementView
RobotClass.setNoView = RobotClass.set
RobotClass.set = setView
robot.displayList= []
# Check the connection
if args.get('dorefresh',True):
robot.refresh()
except:
if verbose: print "No robot viewer, sorry."
robot.viewer = RobotViewerFaked()
#------------------------------------------------------
# Try to read the input file, if possible
try:
fileName = prefix+'position.dat'
file_dyn_position = open(fileName,'r') # Open the file
except:
print "Non valid input file ",fileName
sys.exit(0)
#------------------------------------------------------
# Initialize robotviewer, if possible
try:
import robotviewer
clt = robotviewer.client(options.server)
except:
print "No robot viewer, sorry."
sys.exit(0)
#-----------------------------------------------------
nbImage=0
lastImage=0
robotTime = 2
hq=[]
def inc():
global robotTime, q, hq, robotSize, nbImage, lastImage, decimate
from dynamic_graph.sot.hrp2_14.robot import Robot
from dynamic_graph.sot.dynamics.test import Stepper
from dynamic_graph import enableTrace, plug
from dynamic_graph.sot.dynamics.solver import Solver
rvName = "hrp"
def toViewerConfig(config):
return config + 10 * (0.0,)
try:
import robotviewer
clt = robotviewer.client()
hasRobotViewer = True
except:
hasRobotViewer = False
print "no robotviewer"
timeStep = 0.005
gravity = 9.81
class Motion(object):
signalList = []
def __init__(self, robot, solver):
self.solver = solver
self.robot = robot
default="CORBA",
)
(options, args) = parser.parse_args()
parser.print_help()
print "\n -------------------------------------------------------------------------"
# --- DISPLAY INIT --------------------------------------------------------------------
# --- DISPLAY INIT --------------------------------------------------------------------
# --- DISPLAY INIT --------------------------------------------------------------------
# Initialize robotviewer, if possible
try:
import robotviewer
clt = robotviewer.client(options.server) # default is CORBA
except:
print " -- WARNING!: No robot viewer (or not correct server)."
class RobotViewerFaked:
def update(*args):
void
def updateElementConfig(*args):
void
clt = RobotViewerFaked()
# --- PARSER INIT ------------------------------------------------------------------
# --- PARSER INIT ------------------------------------------------------------------
'''
Execute several trajectory interpolation for various se(3) velocities. The
purpose of this script is to exhibit the use of the exp and log map for
interpolating SE(3) movements.
'''
import robotviewer
viewer = robotviewer.client('XML-RPC')
try:
viewer.updateElementConfig('RomeoTrunkYaw', [1, 0, 0, 0, 0, 0])
except:
viewer.updateElementConfig = lambda a, b: a
def se3ToRpy(m):
return M.translation.T.tolist()[0] + matrixToRpy(M.rotation).T.tolist()[0]
import pinocchio as se3
from pinocchio.utils import *
norm = npl.norm
import time
N = 1000
M = se3.SE3.Identity()
# Integrate a constant body velocity.
v = zero(3)
v[2] = 1.0 / N
w = zero(3)
w[1] = 1.0 / N
# half sitting
# qhs=matrix((0,0,0,0,0,0, 0,0,-26,50,-24,0,0,0,-26,50,-24,0,0,0,0,0,15,10,0,-30,0,0,10,15,-10,0,-30,0,0,10))
# robot.set((0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.4537856055185257, 0.87266462599716477, -0.41887902047863906, 0.0, 0.0, 0.0, -0.4537856055185257, 0.87266462599716477, -0.41887902047863906, 0.0, 0.0, 0.0, 0.0, 0.0, 0.26179938779914941, 0.17453292519943295, 0.0, -0.52359877559829882, 0.0, 0.0, 0.17453292519943295, 0.26179938779914941, -0.17453292519943295, 0.0, -0.52359877559829882, 0.0, 0.0, 0.17453292519943295))
# ------------------------------------------------------------------------------
# --- VIEWER -------------------------------------------------------------------
# ------------------------------------------------------------------------------
try:
import robotviewer
def stateFullSize(robot):
return [float(val) for val in robot.state.value] + 10 * [0.0]
RobotClass.stateFullSize = stateFullSize
robot.viewer = robotviewer.client("XML-RPC")
# robot.viewer = robotviewer.client('CORBA')
# Check the connection
robot.viewer.updateElementConfig("hrp", robot.stateFullSize())
def refreshView(robot):
robot.viewer.updateElementConfig("hrp", robot.stateFullSize())
RobotClass.refresh = refreshView
def incrementView(robot, dt):
robot.incrementNoView(dt)
robot.refresh()
# if zmp.zmp.time > 0:
# robot.viewer.updateElementConfig('zmp',[zmp.zmp.value[0],zmp.zmp.value[1],0,0,0,0])
def compute_estimates_from_sensors(sensors, delay, ftSensorOffsets=None, USE_FT_SENSORS=True):
NJ = 30; # number of joints
m = sensors['time'].shape[0]; # number of time steps
dt = float(np.mean(sensors['time'][1:]-sensors['time'][:-1])); # sampling period
# Create and initialize estimator
estimator_ft = ForceTorqueEstimator("estimator_ft"+str(np.random.rand()));
estimator_kin = VelAccEstimator("estimator_kin"+str(np.random.rand()));
plug(estimator_kin.x, estimator_ft.q_filtered);
plug(estimator_kin.dx, estimator_ft.dq_filtered);
plug(estimator_kin.ddx, estimator_ft.ddq_filtered);
estimator_ft.dqRef.value = NJ*(0.0,);
estimator_ft.ddqRef.value = NJ*(0.0,);
#Only use inertia model (not current) to estimate torques.
estimator_ft.wCurrentTrust.value = NJ*(0.0,);
estimator_ft.currentMeasure.value = NJ*(0.0,);
estimator_ft.saturationCurrent.value = NJ*(0.0,);
estimator_ft.motorParameterKt_p.value = tuple(30*[1.,])
estimator_ft.motorParameterKt_n.value = tuple(30*[1.,])
estimator_ft.motorParameterKf_p.value = tuple(30*[0.,])
estimator_ft.motorParameterKf_n.value = tuple(30*[0.,])
estimator_ft.motorParameterKv_p.value = tuple(30*[0.,])
estimator_ft.motorParameterKv_n.value = tuple(30*[0.,])
estimator_ft.motorParameterKa_p.value = tuple(30*[0.,])
estimator_ft.motorParameterKa_n.value = tuple(30*[0.,])
set_sensor_data_in_estimator(estimator_ft, estimator_kin, sensors[0]);
print "Time step: %f" % dt;
print "Estimation delay: %f" % delay;
if(ftSensorOffsets==None):
estimator_ft.init(dt,delay,delay,delay,delay,True);
else:
estimator_ft.init(dt,delay,delay,delay,delay,False);
estimator_ft.setFTsensorOffsets(tuple(ftSensorOffsets));
estimator_kin.init(dt,NJ, delay);
estimator_ft.setUseRawEncoders(False);
estimator_ft.setUseRefJointVel(False);
estimator_ft.setUseRefJointAcc(False);
estimator_ft.setUseFTsensors(USE_FT_SENSORS);
torques = np.zeros((m,NJ));
dq = np.zeros((m,NJ));
ddq = np.zeros((m,NJ));
if(USE_ROBOT_VIEWER):
viewer=robotviewer.client('XML-RPC');
for i in range(m):
if(USE_ROBOT_VIEWER and i%10==0):
viewer.updateElementConfig('hrp_device', [0,0,0.7,0,0,0] + sensors['enc'][i,:].tolist());
set_sensor_data_in_estimator(estimator_ft, estimator_kin, sensors[i]);
estimator_ft.jointsTorques.recompute(i);
torques[i,:] = np.array(estimator_ft.jointsTorques.value);
dq[i,:] = np.array(estimator_kin.dx.value);
ddq[i,:] = np.array(estimator_kin.ddx.value);
if(i==2):
print ("F/T sensor offsets are: ", estimator_ft.getFTsensorOffsets());
if(i%1000==0):
print 'Estimation time: \t %.3f' % (i*dt);
return (torques, dq, ddq);
def compute_estimates_from_sensors(sensors,
delay,
ftSensorOffsets=None,
USE_FT_SENSORS=True):
NJ = 30
# number of joints
m = sensors['time'].shape[0]
# number of time steps
dt = float(np.mean(sensors['time'][1:] - sensors['time'][:-1]))
# sampling period
# Create and initialize estimator
estimator_ft = ForceTorqueEstimator("estimator_ft" + str(np.random.rand()))
estimator_kin = NumericalDifference("estimator_kin" +
str(np.random.rand()))
ft_RF_filter = NumericalDifference("ft_RF_filter" + str(np.random.rand()))
ft_LF_filter = NumericalDifference("ft_LF_filter" + str(np.random.rand()))
ft_RH_filter = NumericalDifference("ft_RH_filter" + str(np.random.rand()))
ft_LH_filter = NumericalDifference("ft_LH_filter" + str(np.random.rand()))
acc_filter = NumericalDifference("dv_filter" + str(np.random.rand()))
gyro_filter = NumericalDifference("w_filter" + str(np.random.rand()))
estimator_kin = NumericalDifference("estimator_kin" +
str(np.random.rand()))
plug(estimator_kin.x, estimator_ft.q_filtered)
plug(estimator_kin.dx, estimator_ft.dq_filtered)
plug(estimator_kin.ddx, estimator_ft.ddq_filtered)
plug(acc_filter.x_filtered, estimator_ft.accelerometer)
plug(gyro_filter.x_filtered, estimator_ft.gyro)
plug(gyro_filter.dx, estimator_ft.dgyro)
plug(ft_RF_filter.x_filtered, estimator_ft.ftSensRightFoot)
plug(ft_LF_filter.x_filtered, estimator_ft.ftSensLeftFoot)
plug(ft_RH_filter.x_filtered, estimator_ft.ftSensRightHand)
plug(ft_LH_filter.x_filtered, estimator_ft.ftSensLeftHand)
estimator_ft.rotor_inertias.value = NJ * (0., )
estimator_ft.gear_ratios.value = NJ * (0., )
estimator_ft.dqRef.value = NJ * (0.0, )
estimator_ft.ddqRef.value = NJ * (0.0, )
# Only use inertia model (not current) to estimate torques.
estimator_ft.wCurrentTrust.value = NJ * (0.0, )
estimator_ft.current.value = NJ * (0.0, )
estimator_ft.saturationCurrent.value = NJ * (0.0, )
estimator_ft.motorParameterKt_p.value = tuple(30 * [1.])
estimator_ft.motorParameterKt_n.value = tuple(30 * [1.])
estimator_ft.motorParameterKf_p.value = tuple(30 * [0.])
estimator_ft.motorParameterKf_n.value = tuple(30 * [0.])
estimator_ft.motorParameterKv_p.value = tuple(30 * [0.])
estimator_ft.motorParameterKv_n.value = tuple(30 * [0.])
estimator_ft.motorParameterKa_p.value = tuple(30 * [0.])
estimator_ft.motorParameterKa_n.value = tuple(30 * [0.])
set_sensor_data_in_estimator(estimator_ft, estimator_kin, acc_filter,
gyro_filter, ft_RH_filter, ft_LF_filter,
ft_LH_filter, ft_RF_filter, sensors[0])
print("Time step: %f" % dt)
print("Estimation delay: %f" % delay)
if ftSensorOffsets is None:
estimator_ft.init(True)
else:
estimator_ft.init(False)
estimator_ft.setFTsensorOffsets(tuple(ftSensorOffsets))
estimator_kin.init(dt, NJ, delay, 2)
ft_RF_filter.init(dt, 6, delay, 1)
ft_LF_filter.init(dt, 6, delay, 1)
ft_RH_filter.init(dt, 6, delay, 1)
ft_LH_filter.init(dt, 6, delay, 1)
gyro_filter.init(dt, 3, delay, 1)
acc_filter.init(dt, 3, delay, 1)
estimator_ft.setUseRawEncoders(False)
estimator_ft.setUseRefJointVel(False)
estimator_ft.setUseRefJointAcc(False)
estimator_ft.setUseFTsensors(USE_FT_SENSORS)
torques = np.zeros((m, NJ))
dq = np.zeros((m, NJ))
ddq = np.zeros((m, NJ))
if (USE_ROBOT_VIEWER):
viewer = robotviewer.client('XML-RPC')
for i in range(m):
if USE_ROBOT_VIEWER and i % 10 == 0:
viewer.updateElementConfig('hrp_device', [0, 0, 0.7, 0, 0, 0] +
sensors['enc'][i, :].tolist())
set_sensor_data_in_estimator(estimator_ft, estimator_kin, acc_filter,
gyro_filter, ft_RH_filter, ft_LF_filter,
ft_LH_filter, ft_RF_filter, sensors[i])
estimator_ft.jointsTorques.recompute(i)
torques[i, :] = np.array(estimator_ft.jointsTorques.value)
dq[i, :] = np.array(estimator_kin.dx.value)
ddq[i, :] = np.array(estimator_kin.ddx.value)
if i == 2:
print("F/T sensor offsets are: ",
estimator_ft.getFTsensorOffsets())
if i % 1000 == 0:
print('Estimation time: \t %.3f' % (i * dt))
return (torques, dq, ddq)
parser.add_option("-d", dest="delayTime", help="Delay time between iterations (in sec). Default is %default", \
type=float, default="0")
parser.add_option("-s", dest="server", help="Name of server (CORBA or XML-RPC). Default is %default", \
metavar="NAME_SERVER", default="CORBA")
(options, args) = parser.parse_args()
parser.print_help()
print '\n -------------------------------------------------------------------------'
# --- DISPLAY INIT --------------------------------------------------------------------
# --- DISPLAY INIT --------------------------------------------------------------------
# --- DISPLAY INIT --------------------------------------------------------------------
# Initialize robotviewer, if possible
try:
import robotviewer
clt = robotviewer.client(options.server) # default is CORBA
except:
print " -- WARNING!: No robot viewer (or not correct server)."
class RobotViewerFaked:
def update(*args):
void
def updateElementConfig(*args):
void
clt = RobotViewerFaked()
# --- PARSER INIT ------------------------------------------------------------------
# --- PARSER INIT ------------------------------------------------------------------
# --- PARSER INIT ------------------------------------------------------------------
robot = RobotClass("robot")
robot.resize(robotDim)
robot.set( initialConfig[robotName] )
dt=5e-3
# --- VIEWER -------------------------------------------------------------------
# --- VIEWER -------------------------------------------------------------------
# --- VIEWER -------------------------------------------------------------------
try:
import robotviewer
def stateFullSize(robot):
return [float(val) for val in robot.state.value]+10*[0.0]
RobotClass.stateFullSize = stateFullSize
robot.viewer = robotviewer.client('XML-RPC')
# Check the connection
robot.viewer.updateElementConfig('hrp',robot.stateFullSize())
def refreshView( robot ):
robot.viewer.updateElementConfig('hrp',robot.stateFullSize())
RobotClass.refresh = refreshView
def incrementView( robot,dt ):
robot.incrementNoView(dt)
robot.refresh()
RobotClass.incrementNoView = RobotClass.increment
RobotClass.increment = incrementView
def setView( robot,*args ):
robot.setNoView(*args)
robot.refresh()
RobotClass.setNoView = RobotClass.set
else: delayTime = float(options.delayTime)
#------------------------------------------------------
# Try to read the input file, if possible
try:
fileName = prefix + 'position.dat'
file_dyn_position = open(fileName, 'r') # Open the file
except:
print "Non valid input file ", fileName
sys.exit(0)
#------------------------------------------------------
# Initialize robotviewer, if possible
try:
import robotviewer
clt = robotviewer.client(options.server)
except:
print "No robot viewer, sorry."
sys.exit(0)
#-----------------------------------------------------
nbImage = 0
lastImage = 0
robotTime = 2
hq = []
def inc():
global robotTime, q, hq, robotSize, nbImage, lastImage, decimate
while True:
#fileName = sys.argv[1] # Input File ([2] if using 'py', [1] if 'python')
prefix='mocap/'
fileName = prefix+'yoganmsd.pos'
file_dyn_position = open(fileName,'r') # Open the file
# fileffName = prefix+'ffposition.dat'
# file_dyn_ffposition = open(fileffName,'r') # Open the file
except:
print "Please, specify the input file"
sys.exit(0)
#------------------------------------------------------
# Initialize robotviewer, if possible
try:
import robotviewer
clt = robotviewer.client('XML-RPC')
except:
print "No robot viewer, sorry."
sys.exit(0)
#-----------------------------------------------------
# Main Loop
robotTime = 1
hq=[]
def inc():
global robotTime, q, hq
while True:
pos_angles = file_dyn_position.readline()
if len(pos_angles)==0: break
