def exec_traj(p_sphere_pos,v_sphere_pos,params):
global thread_data
global thread_lock
robot=params['robot']
#Reach the current position of p_sphere
HRP4.SetConfig(robot,thread_data['initial_q'])
config=Reach(params['linkindex'],params['linkindex2'],p_sphere_pos,params['n_steps'],params)
HRP4.SetConfig(robot,config)
#If Move_p, update the position of the v_sphere
if thread_data['state']=='Move_p':
Trans=eye(4)
Trans[0:3,3]=p_sphere_pos+thread_data['cur_vit']*params['dist']
thread_data['v_sphere'].SetTransform(Trans)
#Else, i.e. Move_v, update the current v
else:
thread_data['cur_vit']=(v_sphere_pos-p_sphere_pos)/params['dist']
#COmpute the deformed trajectory and move robot 2
traj=params['traj']
tau=params['tau']
T=params['T']
q_T=thread_data['initial_q']
q_Td=config
qd_T=thread_data['initial_qd']
J=compute_Jacobian_speed(params)
qd_Td=qd_T+dot(linalg.pinv(J),thread_data['cur_vit']-dot(J,qd_T))
if params['with_velocity']:
traj_def=Affine.deform_traj(traj,tau,T,q_Td,params['active_dofs'],qd_T,qd_Td)
else:
traj_def=Affine.deform_traj(traj,tau,T,q_Td,params['active_dofs'])
for k in range(traj_def.n_steps):
traj_def.q_vect[[0,1,2],k]+=params['offset']
robot2=params['robot2']
Trajectory.Execute(robot2,traj_def,0.001)
thread_data['traj_def']=traj_def
T_list = [1.4]
pwp_traj = Trajectory.Interpolate(q_list, qd_list, T_list)
T = sum(T_list)
n_discr = 500
t_step = T / n_discr
traj_1 = pwp_traj.GetSampleTraj(T, t_step)
traj_2 = Trajectory.DynamicShift(robot, traj_1, 7)
spline_traj = Trajectory.SplineInterpolateTrajectory(traj_2.t_vect,
traj_2.q_vect,
k=3,
s=0)
traj = spline_traj.GetSampleTraj(T, t_step)
Trajectory.Execute(robot, traj, 0.01)
xminf = -0.085
xmaxf = 0.15
yminf = -0.16
ymaxf = 0.16
zz = 0.001
p1 = array([xminf, yminf, zz])
p2 = array([xmaxf, yminf, zz])
p3 = array([xmaxf, ymaxf, zz])
p4 = array([xminf, ymaxf, zz])
handle_base = env.drawlinestrip(array([p1, p2, p3, p4, p1]), 5)
border = 0.007
xmin = xminf + border
xmax = xmaxf - border
clf()
hold('on')
plot(traj2.t_vect[2:N - 2], transpose(tau2[:, 2:N - 2]), linewidth=2)
#plot(traj.t_vect,transpose(tau),linewidth=1)
plot([0, T2], array([tau_min, tau_min]), '--', linewidth=2)
plot([0, T2], array([tau_max, tau_max]), '--', linewidth=2)
plot([tv[0], tv[0]], [-m * r, m * r], 'k--', linewidth=1.3)
plot([tv[1], tv[1]], [-m * r, m * r], 'k--', linewidth=1.3)
plot([tv[2], tv[2]], [-m * r, m * r], 'k--', linewidth=1.3)
axis([0, T2, -m * 1.2, m * 1.2])
grid('on')
show()
# Visualize the algorithms (see the OpenRave window)
robot.SetDOFValues([0, 0, 0, 0])
time.sleep(0.5)
Trajectory.Execute(robot, traj, 0.03)
time.sleep(0.5)
robot.SetDOFValues([0, 0, 0, 0])
time.sleep(0.5)
Trajectory.Execute(robot, traj2, 0.03)
time.sleep(0.5)
robot.SetDOFValues([0, 0, 0, 0])
time.sleep(0.5)
Trajectory.Execute(robot, traj3, 0.03)
time.sleep(0.5)
def run():
HRP4.SetConfig(robot,traj_mod.q_vect[:,params['T']])
Trajectory.Execute(robot2,traj_mod_shift,0.001)
