def set_quaternion(self, quaternion):
vrep.simxSetObjectQuaternion(self.client_id,
self.handle,
relativeToObjectHandle=-1,
quaternion=quaternion,
operationMode=vrep.simx_opmode_oneshot)
self.wait_until_stop(self.handle)
def setObjectQuaternion(self, object_name, quaternion, relative2='parent'):
#Changes what it is relative to
if relative2 == 'parent':
relative_handle = vrep.sim_handle_parent
else:
relative_handle = self.handles[self.handle_names.index(relative2)]
if self.clientID == None:
print("Robot not attached to VREP environment")
else:
vrep.simxSetObjectQuaternion(
self.clientID,
object_name,
relative_handle,
quaternion, #(x, y, z, w)
operationMode=self.opmode)
def setVrepPoses(n_poses, clientID, optitrak_joint_base_positions,
optitrak_joint_base_quats, fk_positions, fk_orientations,
robot_pose_handles, pose_handles):
for i in range(n_poses):
vrep.simxSetObjectPosition(
clientID,
pose_handles[i],
-1, # Setting the absolute position
position=fk_positions[i],
operationMode=vrep.simx_opmode_oneshot #vrep.simx_opmode_blocking
)
vrep.simxSetObjectPosition(
clientID,
robot_pose_handles[i],
-1, # Setting the absolute position
position=optitrak_joint_base_positions[i],
operationMode=vrep.simx_opmode_oneshot #vrep.simx_opmode_blocking
)
vrep.simxSetObjectQuaternion(
clientID,
pose_handles[i],
-1,
fk_orientations[i], #(x, y, z, w)
operationMode=vrep.simx_opmode_oneshot #vrep.simx_opmode_blocking
)
vrep.simxSetObjectQuaternion(
clientID,
robot_pose_handles[i],
-1,
optitrak_joint_base_quats[i], #(x, y, z, w)
operationMode=vrep.simx_opmode_oneshot #vrep.simx_opmode_blocking
)
vrep.simxSetObjectPosition(
clientID,
robot_pose_handles[-1],
-1, # Setting the absolute position
position=optitrak_joint_base_positions[-1],
operationMode=vrep.simx_opmode_oneshot #vrep.simx_opmode_blocking
)
vrep.simxSetObjectQuaternion(
clientID,
robot_pose_handles[-1],
-1,
optitrak_joint_base_quats[-1], #(x, y, z, w)
operationMode=vrep.simx_opmode_oneshot #vrep.simx_opmode_blocking
)
def create_pose(transform_matrix=numpy.eye(4)):
'''
Creates a shape of type 'dummy' in the scene
:param transform_matrix:
:return: None
'''
size = 0.01
res, dummy_handle = vrep.simxCreateDummy(clientID, size, None,
vrep.simx_opmode_blocking)
__check_resp(res, vrep.simx_return_ok)
position = util.get_position_from_matrix(transform_matrix)
quat = util.get_quat_from_matrix(transform_matrix)
res = vrep.simxSetObjectQuaternion(clientID, dummy_handle, -1, quat,
vrep.simx_opmode_blocking)
__check_resp(res, vrep.simx_return_ok)
res = vrep.simxSetObjectPosition(clientID, dummy_handle, -1, position,
vrep.simx_opmode_blocking)
__check_resp(res, vrep.simx_return_ok)
quaternion = forward_kinematics.quaternion_from_matrix(pose)
temp = quaternion[0]
quaternion[0] = quaternion[1]
quaternion[1] = quaternion[2]
quaternion[2] = quaternion[3]
quaternion[3] = temp
position = (pose[0, 3], pose[1, 3], pose[2, 3] - 0.193424112)
print("Theta:", theta_list)
print("Calculated qua:", quaternion)
# Show predicted position
errorCode, dummy_handle_1 = vrep.simxCreateDummy(
clientID, 0.1, None, vrep.simx_opmode_oneshot_wait)
vrep.simxSetObjectPosition(clientID, dummy_handle_1, joint_handles[0],
position, vrep.simx_opmode_oneshot_wait)
vrep.simxSetObjectQuaternion(clientID, dummy_handle_1, joint_handles[0],
quaternion, vrep.simx_opmode_oneshot_wait)
# print("\nEULER!",euler_angles)
input("Press any key to move to predicted location")
# Set the position of each joint
for i in range(7):
vrep.simxSetJointTargetPosition(clientID, joint_handles[i],
theta_list[i],
vrep.simx_opmode_oneshot_wait)
# print("Setting joint", i+1, "to", theta_list[i])
sleep(2)
errorCode, dummy_handle_3 = vrep.simxCreateDummy(
clientID, 0.1, None, vrep.simx_opmode_oneshot_wait)
simx_opmode_oneshot #vrep.simx_opmode_blocking
)
vrep.simxSetObjectPosition(
clientID,
robot_pose_handles[i],
-1, # Setting the absolute position
position=optitrak_joint_base_positions[i],
operationMode=vrep.
simx_opmode_oneshot #vrep.simx_opmode_blocking
)
vrep.simxSetObjectQuaternion(
clientID,
pose_handles[i],
-1,
fk_orientations[i], #(x, y, z, w)
operationMode=vrep.
simx_opmode_oneshot #vrep.simx_opmode_blocking
)
vrep.simxSetObjectQuaternion(
clientID,
robot_pose_handles[i],
-1,
optitrak_joint_base_quats[i], #(x, y, z, w)
operationMode=vrep.
simx_opmode_oneshot #vrep.simx_opmode_blocking
)
#---------------------------------------------
#update joint angle goal using IK, 2ms block
def callback(quat):
global joint_handles
global clientID
# Decompose message
wrist = quat.wrist_quat
shoulder = quat.shoulder_quat
elbow_angle_in_rad = quat.elbow_angle_in_rad
# Rotate shoulder quaternion +90 degrees around z-axis
q_shoulder = Quaternion(shoulder.w, shoulder.x, shoulder.y, shoulder.z)
rot = Quaternion(w=0.0, x=0.707, y=0.707, z=0.0)
q_shoulder = rot * q_shoulder * rot.inverse
rot_s = Quaternion(w=0.0, x=0.0, y=0.0, z=1.0)
q_shoulder = rot_s * q_shoulder
elements_s = q_shoulder.elements
# Elbow angle
q_lower = Quaternion(axis=(0.0, 0.0, 1.0), radians=elbow_angle_in_rad)
elements_l = q_lower.elements
q_wrist = Quaternion(wrist.w, wrist.z, wrist.x, wrist.y)
rot_w_y = Quaternion(w=0.707, x=0.0, y=0.707, z=0.0)
rot_w_x = Quaternion(w=0.707, x=0.707, y=0.0, z=0.0)
q_wrist = rot_w_x * rot_w_y * q_wrist * rot_w_y.inverse * rot_w_x.inverse
rot_w = Quaternion(w=0.707, x=0.707, y=0.0, z=0.0)
q_wrist = rot_w * q_wrist
rot_w = Quaternion(w=0.985, x=0.0, y=0.0, z=-0.184)
q_wrist = rot_w * q_wrist
rot_w2 = Quaternion(w=0.0, x=0.0, y=0.0, z=1.0)
q_wrist = rot_w2 * q_wrist
elements_w = q_wrist.elements
# Send orientation to VREP
if not joint_handles is None:
# send shoulder orientation
vrep.simxSetObjectQuaternion(
clientID, joint_handles[0], -1,
[elements_s[1], elements_s[2], elements_s[3], elements_s[0]],
vrep.simx_opmode_oneshot)
# send lower arm orientation
vrep.simxSetObjectQuaternion(
clientID, joint_handles[2], joint_handles[1],
[elements_l[1], elements_l[2], elements_l[3], elements_l[0]],
vrep.simx_opmode_oneshot)
# send wrist orientation
vrep.simxSetObjectQuaternion(
clientID, joint_handles[4], -1,
[elements_w[1], elements_w[2], elements_w[3], elements_w[0]],
vrep.simx_opmode_oneshot)
# get end effector orientation
ret, q = vrep.simxGetObjectQuaternion(clientID, joint_handles[4], -1,
vrep.simx_opmode_buffer)
# calculate end effector position
ret, posb = vrep.simxGetObjectPosition(clientID, joint_handles[0], -1,
vrep.simx_opmode_buffer)
ret, pos = vrep.simxGetObjectPosition(clientID, joint_handles[4], -1,
vrep.simx_opmode_streaming)
print(pos)
end_eff = [pos[0], pos[1], pos[2], q[3], q[0], q[1], q[2]]
end_eff_data = Float32MultiArray()
end_eff_data.data = end_eff
pub.publish(end_eff_data)
def set_obj_quaternion(handle, quat, mode):
result = vrep.simxSetObjectQuaternion(clientID, handle, -1, quat, mode)
check_error(result, handle, 0)
T = np.dot(T, expm(screw2twist(s2) * theta[1][i]))
T = np.dot(T, expm(screw2twist(s3) * theta[2][i]))
T = np.dot(T, expm(screw2twist(s4) * theta[3][i]))
T = np.dot(T, expm(screw2twist(s5) * theta[4][i]))
T = np.dot(T, expm(screw2twist(s6) * theta[5][i]))
end = np.dot(T, M)
# result, position = vrep.simxGetObjectPosition(clientID, frame_handle, -1, vrep.simx_opmode_streaming)
# check_error(result, "get frame1 position")
# result = vrep.simxSetObjectPosition(clientID, frame0_handle, -1, position, vrep.simx_opmode_oneshot)
# check_error(result, "frame1 position")
quat = get_quat(end[0:3, 0:3])
print('quat: ', end[0:3, 0:3])
# result, quat = vrep.simxGetObjectQuaternion(clientID, frame0_handle, -1, vrep.simx_opmode_streaming)
result = vrep.simxSetObjectQuaternion(clientID, frame0_handle, -1, quat,
vrep.simx_opmode_oneshot)
result = vrep.simxSetObjectPosition(clientID, frame0_handle, -1,
end[0:3, 3], vrep.simx_opmode_oneshot)
# check_error(result, "frame1 position")
print(quat)
print("ending position: \n", end[0:3, 3])
time.sleep(2)
for j in range(0, 6):
set_joint_value(j, theta[j][i])
# time.sleep(1)
time.sleep(5)
# Stop simulation
#change_dummy=vrep.simxSetObjectPosition(clientID,dummy,jointHandles[0],coordinate,vrep.simx_opmode_oneshot)
result,frame=vrep.simxGetObjectHandle(clientID, 'ReferenceFrame', vrep.simx_opmode_blocking)
change_frame_v=vrep.simxSetObjectPosition(clientID,frame,jointHandles[0],coordinate,vrep.simx_opmode_oneshot)
change_frame_theta=vrep.simxSetObjectQuaternion(clientID,frame,jointHandles[0],new_orientation,vrep.simx_opmode_oneshot)
time.sleep(1)
##################################################################################################################################
# определяем необходимый угол поворота ЗУ до объекта относительно камеры
if not (x - 1 <= resolution_RGB[1] / 2 <=
x + 1) or not (y - 1 <= resolution_RGB[0] / 2 <= y + 1):
# расчет углов поворота
angle_y = rad_in_pixel * ((resolution_RGB[0] / 2) - x)
angle_x = rad_in_pixel * (y - (resolution_RGB[1] / 2))
print(angle_x, angle_y)
y_y = math.sin(angle_y / 2)
y_w = math.cos(angle_y / 2)
x_x = math.sin(angle_x / 2)
x_w = math.cos(angle_x / 2)
quat_res = quatRotation(y_x, x_x, y_y, x_y, y_z, x_z, y_w, x_w)
vrep.simxSetObjectQuaternion(clientID, target_handle, target_handle,
quat_res, vrep.simx_opmode_oneshot_wait)
time.sleep(0.04)
continue
# запрос изображения с камеры глубины, движение к объекту
_, resolution_RGB, image_depth = vrep.simxGetVisionSensorDepthBuffer(
clientID, camera_depth_handle, vrep.simx_opmode_buffer)
Array_depth = np.array(image_depth)
Array_depth = Array_depth.reshape((resolution_RGB[0], resolution_RGB[1]))
Array_depth = np.flipud(Array_depth)
distance_centroid = Array_depth[y, x] * 10
print(distance_centroid)
print(area)
def SetObjectQuaternion(self, objectHandle, quaternion):
# set position relative to global frame
vrep.simxSetObjectQuaternion(self.clientId, objectHandle, -1,
quaternion, vrep.simx_opmode_oneshot)