def go_home(self):
vrep.simxSetObjectOrientation(self.clientID, self.dummyTargetHandle, -1,self.home_euler,vrep.simx_opmode_oneshot_wait)
time.sleep(0.5)
vrep.simxSetObjectPosition(self.clientID, self.dummyTargetHandle, -1, self.home_position,
vrep.simx_opmode_oneshot_wait)
time.sleep(0.5)
return
def move(self,pose):
'''
将末端移动到相应的位置
:param pose:4*4
:param mu_q:四元数高斯噪声的均值
:param simu_q:四元数噪声的方差
:param mu_t:position高斯噪声的均值
:param simu_q:position四元数噪声的方差
'''
euler = transforms3d.euler.mat2euler(pose[:3,:3],'rxyz')
vrep.simxSetObjectPosition(self.clientID, self.dummyTargetHandle, -1, pose[:3, 3],
vrep.simx_opmode_oneshot_wait)
time.sleep(2)
vrep.simxSetObjectOrientation(self.clientID, self.dummyTargetHandle, -1,euler,vrep.simx_opmode_oneshot_wait)
time.sleep(2)
q0 = transforms3d.quaternions.mat2quat(pose[:3,:3])
q = np.array([q0[1],q0[2],q0[3],q0[0]])
errorcode, q1 = vrep.simxGetObjectQuaternion(self.clientID, self.dummyTipHandle, -1,
vrep.simx_opmode_oneshot_wait)
# vrep.simxSetObjectQuaternion(self.clientID,self.dummyTargetHandle,-1,q0,vrep.simx_opmode_oneshot_wait)
#time.sleep(1)
errorcode,q1 = vrep.simxGetObjectQuaternion(self.clientID,self.dummyTipHandle,-1,vrep.simx_opmode_oneshot_wait)
errorcode, euler1 = vrep.simxGetObjectOrientation(self.clientID, self.dummyTipHandle, -1,
vrep.simx_opmode_oneshot_wait)
errorcode,t1 = vrep.simxGetObjectPosition(self.clientID,self.dummyTipHandle,-1,vrep.simx_opmode_oneshot_wait)
norm_q = min(np.linalg.norm(q1-q),np.linalg.norm(q1+q))
norm_t = np.linalg.norm(pose[:3,3]-t1)
if not (norm_q<0.01 and norm_t<0.001):
vrep.simxSetObjectOrientation(self.clientID, self.dummyTargetHandle, -1, euler,
vrep.simx_opmode_oneshot_wait)
time.sleep(2)
errorcode, q1 = vrep.simxGetObjectQuaternion(self.clientID, self.dummyTipHandle, -1,
vrep.simx_opmode_oneshot_wait)
norm_q = min(np.linalg.norm(q1 - q), np.linalg.norm(q1 + q))
if not norm_q < 0.01:
print("error move,q_dis:{0},t_dis:{1}".format(norm_q, norm_t))
return False
q = np.empty([4],np.float32)
q[0] = q1[0]+random.gauss(self.mu_q,self.simu_q)
q[1] = q1[1]+random.gauss(self.mu_q,self.simu_q)
q[2] = q1[2]+random.gauss(self.mu_q,self.simu_q)
q[3] = q1[3]+random.gauss(self.mu_q,self.simu_q)
q0 = q/np.linalg.norm(q)
t = np.empty([3],np.float32)
t[0] = t1[0]+random.gauss(self.mu_t,self.simu_t)
t[1] = t1[1]+random.gauss(self.mu_t,self.simu_t)
t[2] = t1[2]+random.gauss(self.mu_t,self.simu_t)
vrep.simxSetObjectQuaternion(self.clientID, self.dummyTargetHandle, -1, q0, vrep.simx_opmode_oneshot_wait)
vrep.simxSetObjectPosition(self.clientID, self.dummyTargetHandle, -1, t, vrep.simx_opmode_oneshot_wait)
time.sleep(1.5)
return True
def go_guodudian(self):
guodudian_position = np.array([-0.1, -0.1, 0.9])
vrep.simxSetObjectPosition(self.clientID, self.dummyTargetHandle, -1, guodudian_position,
vrep.simx_opmode_oneshot_wait)
time.sleep(2)
guodudian_euler = np.array([0, -math.pi / 2, math.pi / 2])
vrep.simxSetObjectOrientation(self.clientID, self.dummyTargetHandle, -1, guodudian_euler,
vrep.simx_opmode_oneshot_wait)
time.sleep(2)
guodudian_position = np.array([0.2, 0.0, 0.5])
vrep.simxSetObjectPosition(self.clientID, self.dummyTargetHandle, -1, guodudian_position,
vrep.simx_opmode_oneshot_wait)
time.sleep(2)
def move(self, pose):
euler = transforms3d.euler.mat2euler(pose[:3, :3], 'rxyz')
vrep.simxSetObjectPosition(self.clientID, self.KinectRgbHandle, -1,
pose[:3, 3], vrep.simx_opmode_oneshot_wait)
time.sleep(0.5)
vrep.simxSetObjectOrientation(self.clientID, self.KinectRgbHandle, -1,
euler, vrep.simx_opmode_oneshot_wait)
# q0 = transforms3d.quaternions.mat2quat(pose[:3,:3])
# q = np.array([q0[1],q0[2],q0[3],q0[0]])
# errorcode, q1 = vrep.simxGetObjectQuaternion(self.clientID, self.dummyTipHandle, -1,
# vrep.simx_opmode_oneshot_wait)
# vrep.simxSetObjectQuaternion(self.clientID,self.dummyTargetHandle,-1,q0,vrep.simx_opmode_oneshot_wait)
# time.sleep(1)
time.sleep(0.5)