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)