def euler_jacobian(euler,u):
gamma=euler[0] #yaw
beta=euler[1] #pitch
alpha=euler[2] #roll
J=zeros((3,3))
J[:,0]=dot(HRP4.euler2mat_dgamma(euler),u)
J[:,1]=dot(HRP4.euler2mat_dbeta(euler),u)
J[:,2]=dot(HRP4.euler2mat_dalpha(euler),u)
return J
def DynamicShift(robot,traj,fixed_link_i):
fixed_link=robot.GetLinks()[fixed_link_i]
base_link=robot.GetLinks()[0]
traj2=copy.deepcopy(traj)
with robot:
HRP4.SetConfig(robot,traj.q_vect[:,0])
fixed_link_init=fixed_link.GetGlobalCOM()
for i in range(traj.n_steps):
with robot:
HRP4.SetConfig(robot,traj.q_vect[:,i])
fixed_link_cur=fixed_link.GetGlobalCOM()
shift=fixed_link_init-fixed_link_cur
traj2.q_vect[0:3,i]=traj2.q_vect[0:3,i]+shift
return traj2
def Execute(robot,traj,t_sleep,stepsize=1,drawcom=0):
global com_handle
n=robot.GetDOF()
for i in range(0,traj.n_steps,stepsize):
if traj.dim==n+6:
robot.GetLinks()[0].SetTransform(HRP4.v2t(traj.q_vect[0:6,i]))
robot.SetDOFValues(traj.q_vect[6:traj.dim,i])
else:
robot.SetDOFValues(traj.q_vect[:,i])
if drawcom==1:
CoM=ZMP.ComputeCOM([robot.GetLinks()[0].GetTransform()[0:3,3],robot.GetDOFValues()],{'robot':robot,'exclude_list':[]})
CoM_proj=zeros(3)
CoM_proj[0]=CoM[0]
CoM_proj[1]=CoM[1]
com_handle=robot.GetEnv().drawlinestrip(array([CoM,CoM_proj]),5)
elif drawcom==2:
q=traj.q_vect[:,i]
qd=traj.qd_vect[:,i]
qdd=traj.qdd_vect[:,i]
zmp=ZMP.ComputeZMP([q[0:3],qd[0:3],qdd[0:3],q[6:len(q)],qd[6:len(q)],qdd[6:len(q)]],{'robot':robot,'exclude_list':[],'gravity':9.8,'moment_coef':1})
zmp_proj=array([zmp[0],zmp[1],0])
zmp_high=array([zmp[0],zmp[1],0.5])
zmp_handle=robot.GetEnv().drawlinestrip(array([zmp_proj,zmp_high]),5)
time.sleep(t_sleep)
def compute_Jacobian_speed(params):
robot=params['robot']
linkindex=params['linkindex']
linkindex2=params['linkindex2']
euler=HRP4.mat2euler(robot.GetLinks()[0].GetTransform()[0:3,0:3])
link2=robot.GetLinks()[linkindex2]
p_link2=link2.GetGlobalCOM()
return FollowTrajectory.Jacobian(euler,linkindex,p_link2,params)
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
def CheckCollisionTraj(robot,traj):
n=robot.GetDOF()
for i in range(traj.n_steps):
with robot:
if traj.dim==n+6:
robot.GetLinks()[0].SetTransform(HRP4.v2t(traj.q_vect[0:6,i]))
robot.SetDOFValues(traj.q_vect[6:traj.dim,i])
else:
robot.SetDOFValues(traj.q_vect[:,i])
if robot.GetEnv().CheckCollision(robot):
return [True,'env',i]
if robot.CheckSelfCollision():
return [True,'self',i]
return [False,None,None]
def CheckCollisionTraj(robot, traj):
n = robot.GetDOF()
for i in range(traj.n_steps):
with robot:
if traj.dim == n + 6:
robot.GetLinks()[0].SetTransform(HRP4.v2t(traj.q_vect[0:6, i]))
robot.SetDOFValues(traj.q_vect[6:traj.dim, i])
else:
robot.SetDOFValues(traj.q_vect[:, i])
if robot.GetEnv().CheckCollision(robot):
return [True, 'env', i]
if robot.CheckSelfCollision():
return [True, 'self', i]
return [False, None, None]
def Execute(robot,sample_traj,t_sleep,stepsize=1):
global com_handle
n=robot.GetDOF()
for i in range(0,sample_traj.n_steps,stepsize):
if sample_traj.dim==n+6:
robot.GetLinks()[0].SetTransform(HRP4.v2t(sample_traj.q_vect[0:6,i]))
robot.SetDOFValues(sample_traj.q_vect[6:sample_traj.dim,i])
else:
robot.SetDOFValues(sample_traj.q_vect[:,i])
CoM=ZMP.ComputeCOM([robot.GetLinks()[0].GetTransform()[0:3,3],robot.GetDOFValues()],{'robot':robot,'exclude_list':[]})
CoM_proj=zeros(3)
CoM_proj[0]=CoM[0]
CoM_proj[1]=CoM[1]
com_handle=robot.GetEnv().drawlinestrip(array([CoM,CoM_proj]),5)
time.sleep(t_sleep)
def Execute(robot, sample_traj, t_sleep, stepsize=1):
global com_handle
n = robot.GetDOF()
for i in range(0, sample_traj.n_steps, stepsize):
if sample_traj.dim == n + 6:
robot.GetLinks()[0].SetTransform(HRP4.v2t(sample_traj.q_vect[0:6, i]))
robot.SetDOFValues(sample_traj.q_vect[6 : sample_traj.dim, i])
else:
robot.SetDOFValues(sample_traj.q_vect[:, i])
CoM = ZMP.ComputeCOM(
[robot.GetLinks()[0].GetTransform()[0:3, 3], robot.GetDOFValues()], {"robot": robot, "exclude_list": []}
)
CoM_proj = zeros(3)
CoM_proj[0] = CoM[0]
CoM_proj[1] = CoM[1]
com_handle = robot.GetEnv().drawlinestrip(array([CoM, CoM_proj]), 5)
time.sleep(t_sleep)
def start(params):
global thread_data
global thread_lock
robot=params['robot']
robot2=params['robot2']
traj=params['traj']
linkindex=params['linkindex']
linkindex2=params['linkindex2']
#Robot 2
final_config=array(traj.q_vect[:,-1])
final_config[[0,1,2]]+=params['offset']
HRP4.SetConfig(robot2,final_config)
#Find the initial speed of the sphere
T=params['T']
initial_q=traj.q_vect[:,T]
initial_qd=traj.q_vect[:,T]-traj.q_vect[:,T-1]
J_link2=compute_Jacobian_speed(params)
initial_sphere_speed=dot(J_link2,initial_qd)
#Initial positions of the spheres
p_sphere_pos=robot.GetLinks()[linkindex2].GetGlobalCOM()
v_sphere_pos=p_sphere_pos+initial_sphere_speed*params['dist']
p_sphere=CreateSphere(p_sphere_pos,[0,1,0],params)
v_sphere=CreateSphere(v_sphere_pos,[1,0,0],params)
thread_data={'continue':True}
thread_data['initial_q']=initial_q
thread_data['initial_qd']=initial_qd
thread_data['p_sphere']=p_sphere
thread_data['v_sphere']=v_sphere
thread_data['p_sphere_pos']=p_sphere_pos
thread_data['v_sphere_pos']=v_sphere_pos
thread_data['cur_vit']=initial_sphere_speed
thread_data['state']='Rest'
thread_lock=thread.allocate_lock()
thread.start_new_thread(update,(params,))
def Execute(robot, traj, t_sleep, stepsize=1, drawcom=0):
global com_handle
n = robot.GetDOF()
for i in range(0, traj.n_steps, stepsize):
if traj.dim == n + 6:
robot.GetLinks()[0].SetTransform(HRP4.v2t(traj.q_vect[0:6, i]))
robot.SetDOFValues(traj.q_vect[6:traj.dim, i])
else:
robot.SetDOFValues(traj.q_vect[:, i])
if drawcom == 1:
CoM = ZMP.ComputeCOM([
robot.GetLinks()[0].GetTransform()[0:3, 3],
robot.GetDOFValues()
], {
'robot': robot,
'exclude_list': []
})
CoM_proj = zeros(3)
CoM_proj[0] = CoM[0]
CoM_proj[1] = CoM[1]
com_handle = robot.GetEnv().drawlinestrip(array([CoM, CoM_proj]),
5)
elif drawcom == 2:
q = traj.q_vect[:, i]
qd = traj.qd_vect[:, i]
qdd = traj.qdd_vect[:, i]
zmp = ZMP.ComputeZMP(
[
q[0:3], qd[0:3], qdd[0:3], q[6:len(q)], qd[6:len(q)],
qdd[6:len(q)]
], {
'robot': robot,
'exclude_list': [],
'gravity': 9.8,
'moment_coef': 1
})
zmp_proj = array([zmp[0], zmp[1], 0])
zmp_high = array([zmp[0], zmp[1], 0.5])
zmp_handle = robot.GetEnv().drawlinestrip(
array([zmp_proj, zmp_high]), 5)
time.sleep(t_sleep)
import HRP4
set_printoptions(precision=5)
################# Loading the environment ########################
env = Environment() # create openrave environment
env.SetViewer('qtcoin') # attach viewer (optional)
env.Load('/home/cuong/Dropbox/Code/mintime/robots/hrp4r.dae')
env.Load('/home/cuong/Dropbox/Code/mintime/robots/floorwalls.xml')
collisionChecker = RaveCreateCollisionChecker(env, 'pqp')
env.SetCollisionChecker(collisionChecker)
robot = env.GetRobots()[0]
hrp = HRP4.HRP4robot(robot)
n = robot.GetDOF()
g = 9.8
env.GetPhysicsEngine().SetGravity([0, 0, -g])
# DOF and velocity limits
dof_lim = robot.GetDOFLimits()
vel_lim = robot.GetDOFVelocityLimits()
robot.SetDOFLimits(-3 * ones(n), 3 * ones(n))
robot.SetDOFVelocityLimits(100 * vel_lim)
exclude_list = []
for k in range(n):
if robot.GetLinks()[k].GetMass() < 0.01:
exclude_list.append(k)
def run2():
params['with_velocity']=True
HRP4.SetConfig(robot,traj_mod.q_vect[:,params['T']])
AffineDemo.start(params)
def Reach(markers,milestones,n_steps,params):
robot=params['robot']
Q0=params['Q0']
Q0inv=params['Q0inv']
lower_lim=params['lower_lim']
upper_lim=params['upper_lim']
K_v=params['K_v']
K_p=params['K_p']
K_li=params['K_li']
K_sr=params['K_sr']
n_dof=robot.GetDOF()
n_var=n_dof+6
baselink=robot.GetLinks()[0]
res=[]
res.append(HRP4.GetConfig(robot))
cur_config=res[0]
with robot:
for kk in range(len(milestones)):
for step in range(n_steps):
T=baselink.GetTransform()
euler=HRP4.mat2euler(T[0:3,0:3])
# Compute the Jacobians and desired velocities of the markers to follow
J_markers=None
v_desired=None
for j in range(len(markers)):
# Jacobian
linkj=robot.GetLinks()[markers[j]['link_index']]
p_cur=dot(linkj.GetTransform(),add1(markers[j]['local_pos']))[0:3]
J_markers=concat([J_markers,Jacobian(euler,markers[j]['link_index'],p_cur,params)])
# velocity
p_end=markers[j]['p_vect'][:,milestones[kk]]
v_desired=concat([v_desired,(p_end-p_cur)/(n_steps-step)])
# Find the out-of-range DOFs
lower_list=[]
upper_list=[]
DOFvalues=robot.GetDOFValues()
for j in range(n_dof):
if DOFvalues[j]<lower_lim[j]:
lower_list.append(j)
elif DOFvalues[j]>upper_lim[j]:
upper_list.append(j)
# Compute the Jacobians and the desired velocities for the out-of-range DOFs
J_lower=zeros((n_var,n_var))
v_lower=zeros(n_var)
J_upper=zeros((n_var,n_var))
v_upper=zeros(n_var)
for i in lower_list:
J_lower[6+i,6+i]=1
v_lower[6+i]=K_li*(lower_lim[i]-DOFvalues[i])
for i in upper_list:
J_upper[6+i,6+i]=1
v_upper[6+i]=K_li*(upper_lim[i]-DOFvalues[i])
J_limits=concat([J_lower,J_upper])
v_limits=concat([v_lower,v_upper])
# Inverse kinematics computations
# J_main=J_markers
# v_main=v_desired
# J_aux=J_limits
# v_aux=v_limits
# J_main_star=dot(J_main,Q0inv)
# J_main_star_dash=linalg.pinv(J_main_star)
# J_weighted_pinv=dot(Q0inv,J_main_star_dash)
# thetad_0=dot(J_weighted_pinv,v_main)
# W=eye(n_var)-dot(J_weighted_pinv,J_main)
# v_aux_0=dot(J_aux,thetad_0)
# S=dot(J_aux,W)
# [ms,ns]=shape(S)
# delta_v_aux=v_aux-v_aux_0
# Sstar=dot(transpose(S),linalg.inv(dot(S,transpose(S))+K_sr*eye(ms)))
# y=dot(Sstar,delta_v_aux)
# thetad=thetad_0+dot(W,y)
J_main=concat([J_markers,J_limits])
v_main=concat([v_desired,v_limits])
J_main_star=dot(J_main,Q0inv)
J_main_star_dash=linalg.pinv(J_main_star)
J_weighted_pinv=dot(Q0inv,J_main_star_dash)
thetad_0=dot(J_weighted_pinv,v_main)
thetad=thetad_0
cur_config=cur_config+thetad
res.append(cur_config)
HRP4.SetConfig(robot,cur_config)
return Trajectory.SampleTrajectory(transpose(array(res)))
def IKreach(drag,pinned_links,p_end):
global thread_params
robot=thread_params['robot']
p_step=thread_params['p_step']
Q0=thread_params['Q0']
Q0inv=thread_params['Q0inv']
lower_lim=thread_params['lower_lim']
upper_lim=thread_params['upper_lim']
K_li=thread_params['K_li']
K_sr=thread_params['K_sr']
ndof=robot.GetDOF()
drag_link=drag[0]
drag_type=drag[1]
n_pins=len(pinned_links)
baselink=robot.GetLinks()[0]
link=robot.GetLinks()[drag_link]
p_init=link.GetGlobalCOM()
n_steps=norm(p_end-p_init)/p_step
for steps in range(int(n_steps)+1):
p_cur=link.GetGlobalCOM()
T=baselink.GetTransform()
euler=HRP4.mat2euler(T[0:3,0:3])
# Compute the dragged link Jacobian
if drag_type=='translation':
J_drag_a=Jacobian(euler,drag_link,p_cur)
J_drag_b=Jacobian(euler,drag_link,p_cur+array([0,0,1]))
J_drag_c=Jacobian(euler,drag_link,p_cur+array([0,1,0]))
J_drag=concat([J_drag_a,J_drag_b,J_drag_c])
(k,nvar)=shape(J_drag_a)
else:
J_drag=Jacobian(euler,drag_link,p_cur)
(k,nvar)=shape(J_drag)
# Compute the desired_velocity
dist=norm(p_end-p_cur)
if dist<p_step:
v_drag_0=p_end-p_cur
else:
v_drag_0=(p_end-p_cur)/dist*p_step
if drag_type=='translation':
v_drag=concat([v_drag_0,v_drag_0,v_drag_0])
else:
v_drag=v_drag_0
# Compute the Jacobians and the desired velocities of the pins
J_pins=None
v_pins=None
for i in range(n_pins):
pinned_i=pinned_links[i]
pinned_link=robot.GetLinks()[pinned_i]
CoMi=pinned_link.GetGlobalCOM()
J_pinned_ia=Jacobian(euler,pinned_i,CoMi)
J_pinned_ib=Jacobian(euler,pinned_i,CoMi+array([0,0,1]))
J_pinned_ic=Jacobian(euler,pinned_i,CoMi+array([0,1,0]))
J_pins=concat([J_pins,J_pinned_ia,J_pinned_ib,J_pinned_ic])
v_pins=concat([v_pins,zeros(3*k)])
# Find the out-of-range DOFs
lower_list=[]
upper_list=[]
DOFvalues=robot.GetDOFValues()
for j in range(ndof):
if DOFvalues[j]<lower_lim[j]:
lower_list.append(j)
elif DOFvalues[j]>upper_lim[j]:
upper_list.append(j)
# Compute the Jacobians and the desired velocities for the out-of-range DOFs
J_lower=zeros((nvar,nvar))
v_lower=zeros(nvar)
J_upper=zeros((nvar,nvar))
v_upper=zeros(nvar)
for i in lower_list:
J_lower[6+i,6+i]=1
v_lower[6+i]=K_li*(lower_lim[i]-DOFvalues[i])
for i in upper_list:
J_upper[6+i,6+i]=1
v_upper[6+i]=K_li*(upper_lim[i]-DOFvalues[i])
J_limits=concat([J_lower,J_upper])
v_limits=concat([v_lower,v_upper])
# Computations
if thread_params['priority']=='drag':
J_main=J_drag
v_main=v_drag
J_aux=J_pins
v_aux=v_pins
else:
J_main=J_pins
v_main=v_pins
J_aux=J_drag
v_aux=v_drag
J_aux=concat([J_aux,J_limits])
v_aux=concat([v_aux,v_limits])
if J_main!=None:
J_main_star=dot(J_main,Q0inv)
J_main_star_dash=linalg.pinv(J_main_star)
J_weighted_pinv=dot(Q0inv,J_main_star_dash)
thetad_0=dot(J_weighted_pinv,v_main)
W=eye(nvar)-dot(J_weighted_pinv,J_main)
else:
thetad_0=zeros(nvar)
W=eye(nvar)
v_aux_0=dot(J_aux,thetad_0)
S=dot(J_aux,W)
[ms,ns]=shape(S)
delta_v_aux=v_aux-v_aux_0
Sstar=dot(transpose(S),linalg.inv(dot(S,transpose(S))+K_sr*eye(ms)))
y=dot(Sstar,delta_v_aux)
thetad=thetad_0+dot(W,y)
HRP4.SetConfig(robot,HRP4.GetConfig(robot)+thetad)
#Update the positions of the spheres
for i in range(robot.GetDOF()):
if not (i in thread_params['exclude_list']):
UpdateSphere(i)
#Draw the COM
if thread_params['draw_com']:
#try:
# params['com_handle']=None
#except AttributeError:
# pass
CoM=ZMP.ComputeCOM([robot.GetLinks()[0].GetTransform()[0:3,3],robot.GetDOFValues()],{'robot':robot,'exclude_list':[]})
CoM_proj=zeros(3)
CoM_proj[0]=CoM[0]
CoM_proj[1]=CoM[1]
thread_params['com_handle']=robot.GetEnv().drawlinestrip(array([CoM,CoM_proj]),5)
time.sleep(thread_params['timestep'])
set_printoptions(precision=5)
################# Loading the environment ########################
env = Environment() # create openrave environment
env.SetViewer('qtcoin') # attach viewer (optional)
env.Load('/home/cuong/Dropbox/Code/mintime/robots/hrp4r.dae')
env.Load('/home/cuong/Dropbox/Code/mintime/robots/floorwalls.xml')
collisionChecker = RaveCreateCollisionChecker(env,'pqp')
env.SetCollisionChecker(collisionChecker)
robot=env.GetRobots()[0]
hrp=HRP4.HRP4robot(robot)
n=robot.GetDOF()
g=9.8
env.GetPhysicsEngine().SetGravity([0,0,-g])
# DOF and velocity limits
dof_lim=robot.GetDOFLimits()
vel_lim=robot.GetDOFVelocityLimits()
robot.SetDOFLimits(-3*ones(n),3*ones(n))
robot.SetDOFVelocityLimits(100*vel_lim)
exclude_list=[]
for k in range(n):
def IKreach(drag, pinned_links, p_end):
global thread_params
robot = thread_params['robot']
p_step = thread_params['p_step']
Q0 = thread_params['Q0']
Q0inv = thread_params['Q0inv']
lower_lim = thread_params['lower_lim']
upper_lim = thread_params['upper_lim']
K_li = thread_params['K_li']
K_sr = thread_params['K_sr']
ndof = robot.GetDOF()
drag_link = drag[0]
drag_type = drag[1]
n_pins = len(pinned_links)
baselink = robot.GetLinks()[0]
link = robot.GetLinks()[drag_link]
p_init = link.GetGlobalCOM()
n_steps = norm(p_end - p_init) / p_step
for steps in range(int(n_steps) + 1):
p_cur = link.GetGlobalCOM()
T = baselink.GetTransform()
euler = HRP4.mat2euler(T[0:3, 0:3])
# Compute the dragged link Jacobian
if drag_type == 'translation':
J_drag_a = Jacobian(euler, drag_link, p_cur)
J_drag_b = Jacobian(euler, drag_link, p_cur + array([0, 0, 1]))
J_drag_c = Jacobian(euler, drag_link, p_cur + array([0, 1, 0]))
J_drag = concat([J_drag_a, J_drag_b, J_drag_c])
(k, nvar) = shape(J_drag_a)
else:
J_drag = Jacobian(euler, drag_link, p_cur)
(k, nvar) = shape(J_drag)
# Compute the desired_velocity
dist = norm(p_end - p_cur)
if dist < p_step:
v_drag_0 = p_end - p_cur
else:
v_drag_0 = (p_end - p_cur) / dist * p_step
if drag_type == 'translation':
v_drag = concat([v_drag_0, v_drag_0, v_drag_0])
else:
v_drag = v_drag_0
# Compute the Jacobians and the desired velocities of the pins
J_pins = None
v_pins = None
for i in range(n_pins):
pinned_i = pinned_links[i]
pinned_link = robot.GetLinks()[pinned_i]
CoMi = pinned_link.GetGlobalCOM()
J_pinned_ia = Jacobian(euler, pinned_i, CoMi)
J_pinned_ib = Jacobian(euler, pinned_i, CoMi + array([0, 0, 1]))
J_pinned_ic = Jacobian(euler, pinned_i, CoMi + array([0, 1, 0]))
J_pins = concat([J_pins, J_pinned_ia, J_pinned_ib, J_pinned_ic])
v_pins = concat([v_pins, zeros(3 * k)])
# Find the out-of-range DOFs
lower_list = []
upper_list = []
DOFvalues = robot.GetDOFValues()
for j in range(ndof):
if DOFvalues[j] < lower_lim[j]:
lower_list.append(j)
elif DOFvalues[j] > upper_lim[j]:
upper_list.append(j)
# Compute the Jacobians and the desired velocities for the out-of-range DOFs
J_lower = zeros((nvar, nvar))
v_lower = zeros(nvar)
J_upper = zeros((nvar, nvar))
v_upper = zeros(nvar)
for i in lower_list:
J_lower[6 + i, 6 + i] = 1
v_lower[6 + i] = K_li * (lower_lim[i] - DOFvalues[i])
for i in upper_list:
J_upper[6 + i, 6 + i] = 1
v_upper[6 + i] = K_li * (upper_lim[i] - DOFvalues[i])
J_limits = concat([J_lower, J_upper])
v_limits = concat([v_lower, v_upper])
# Computations
if thread_params['priority'] == 'drag':
J_main = J_drag
v_main = v_drag
J_aux = J_pins
v_aux = v_pins
else:
J_main = J_pins
v_main = v_pins
J_aux = J_drag
v_aux = v_drag
J_aux = concat([J_aux, J_limits])
v_aux = concat([v_aux, v_limits])
if J_main != None:
J_main_star = dot(J_main, Q0inv)
J_main_star_dash = linalg.pinv(J_main_star)
J_weighted_pinv = dot(Q0inv, J_main_star_dash)
thetad_0 = dot(J_weighted_pinv, v_main)
W = eye(nvar) - dot(J_weighted_pinv, J_main)
else:
thetad_0 = zeros(nvar)
W = eye(nvar)
v_aux_0 = dot(J_aux, thetad_0)
S = dot(J_aux, W)
[ms, ns] = shape(S)
delta_v_aux = v_aux - v_aux_0
Sstar = dot(transpose(S),
linalg.inv(dot(S, transpose(S)) + K_sr * eye(ms)))
y = dot(Sstar, delta_v_aux)
thetad = thetad_0 + dot(W, y)
HRP4.SetConfig(robot, HRP4.GetConfig(robot) + thetad)
#Update the positions of the spheres
for i in range(robot.GetDOF()):
if not (i in thread_params['exclude_list']):
UpdateSphere(i)
#Draw the COM
if thread_params['draw_com']:
#try:
# params['com_handle']=None
#except AttributeError:
# pass
CoM = ZMP.ComputeCOM([
robot.GetLinks()[0].GetTransform()[0:3, 3],
robot.GetDOFValues()
], {
'robot': robot,
'exclude_list': []
})
CoM_proj = zeros(3)
CoM_proj[0] = CoM[0]
CoM_proj[1] = CoM[1]
thread_params['com_handle'] = robot.GetEnv().drawlinestrip(
array([CoM, CoM_proj]), 5)
time.sleep(thread_params['timestep'])
def run():
HRP4.SetConfig(robot,traj_mod.q_vect[:,params['T']])
Trajectory.Execute(robot2,traj_mod_shift,0.001)
xmaxf=0.16
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)
collisionChecker = RaveCreateCollisionChecker(env,'pqp')
env.SetCollisionChecker(collisionChecker)
robot=env.GetRobots()[0]
[lower_lim,upper_lim]=robot.GetDOFLimits()
hrp=HRP4.HRP4robot(robot)
n=robot.GetDOF()
robot.SetDOFLimits(-1e10*ones(n),1e10*ones(n))
Q0=eye(n+6)
Q0[0,0]=3
Q0[1,11]=3
Q0[2,2]=3
Q0[3,3]=1
Q0[4,4]=1
Q0[5,5]=1
params={'x':1}
params['robot']=robot
params['exclude_list']=exclude_list
params['p_step']=0.01
params['timestep']=0.001
def Reach(linkindex,linkindex2,p_end,n_steps,params):
robot=params['robot']
Q0=params['Q0']
Q0inv=params['Q0inv']
lower_lim=params['lower_lim']
upper_lim=params['upper_lim']
K_v=params['K_v']
K_p=params['K_p']
K_li=params['K_li']
K_sr=params['K_sr']
n_dof=robot.GetDOF()
n_var=n_dof+6
baselink=robot.GetLinks()[0]
res=[]
res.append(HRP4.GetConfig(robot))
cur_config=res[0]
link2=robot.GetLinks()[linkindex2]
with robot:
for step in range(n_steps):
T=baselink.GetTransform()
euler=HRP4.mat2euler(T[0:3,0:3])
# Compute the Jacobians and desired velocities of the markers to follow
p_cur=link2.GetGlobalCOM()
J_marker=FollowTrajectory.Jacobian(euler,linkindex,p_cur,params)
v_desired=(p_end-p_cur)/(n_steps-step)
# Find the out-of-range DOFs
lower_list=[]
upper_list=[]
DOFvalues=robot.GetDOFValues()
for j in range(n_dof):
if DOFvalues[j]<lower_lim[j]:
lower_list.append(j)
elif DOFvalues[j]>upper_lim[j]:
upper_list.append(j)
# Compute the Jacobians and the desired velocities for the out-of-range DOFs
J_lower=zeros((n_var,n_var))
v_lower=zeros(n_var)
J_upper=zeros((n_var,n_var))
v_upper=zeros(n_var)
for i in lower_list:
J_lower[6+i,6+i]=1
v_lower[6+i]=K_li*(lower_lim[i]-DOFvalues[i])
for i in upper_list:
J_upper[6+i,6+i]=1
v_upper[6+i]=K_li*(upper_lim[i]-DOFvalues[i])
J_limits=FollowTrajectory.concat([J_lower,J_upper])
v_limits=FollowTrajectory.concat([v_lower,v_upper])
J_main=FollowTrajectory.concat([J_marker,J_limits])
v_main=FollowTrajectory.concat([v_desired,v_limits])
J_main_star=dot(J_main,Q0inv)
J_main_star_dash=linalg.pinv(J_main_star)
J_weighted_pinv=dot(Q0inv,J_main_star_dash)
thetad_0=dot(J_weighted_pinv,v_main)
thetad=thetad_0
cur_config=cur_config+thetad
res.append(cur_config)
HRP4.SetConfig(robot,cur_config)
return cur_config
################# Loading the environment ########################
env = Environment() # create openrave environment
env.SetViewer('qtcoin') # attach viewer (optional)
env.Load('/home/cuong/Dropbox/Code/mintime/robots/hrp4r.dae')
env.Load('/home/cuong/Dropbox/Code/mintime/robots/hrp4r.dae')
env.Load('/home/cuong/Dropbox/Code/mintime/robots/floorwalls.xml')
exclude_list=[1,2,17,5,6,8,9,12,13,15,17,19,20,23,24,26,27,28,29,30,31,32,33,34,35,36,37,40,41,43,44,45,46,47,48,49]
robot=env.GetRobots()[0]
robot2=env.GetRobots()[1]
[lower_lim_o,upper_lim_o]=robot.GetDOFLimits()
lower_lim=lower_lim_o/1.2
upper_lim=upper_lim_o/1.2
hrp=HRP4.HRP4robot(robot)
n=robot.GetDOF()
robot.SetDOFLimits(-1e10*ones(n),1e10*ones(n))
robot2.SetDOFLimits(-1e10*ones(n),1e10*ones(n))
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])
