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 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 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)
