def trivial_transformation(clientID, x, y, z):
relative_pos = [x, y, z]
vrep.simxCreateDummy(clientID, 0, None, vrep.simx_opmode_blocking)
opcode, handle = vrep.simxGetObjectHandle(clientID, "Dummy#",
vrep.simx_opmode_blocking)
opcode, left_zed = vrep.simxGetObjectHandle(clientID, "zed_vision1#",
vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(clientID, handle, left_zed, relative_pos,
vrep.simx_opmode_blocking)
opcode, absolute_pos = vrep.simxGetObjectPosition(
clientID, handle, -1, vrep.simx_opmode_blocking)
return absolute_pos
def trivial_transformation(clientID, x, y, z):
global initialized
relative_pos = [x, y, z]
if initialized is False:
vrep.simxCreateDummy(clientID, 0, None, vrep.simx_opmode_blocking)
initialized = True
opcode, handle = vrep.simxGetObjectHandle(clientID, "Dummy0",
vrep.simx_opmode_blocking)
opcode, left_zed = vrep.simxGetObjectHandle(clientID, "zed_vision1",
vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(clientID, handle, left_zed, relative_pos,
vrep.simx_opmode_blocking)
opcode, absolute_pos = vrep.simxGetObjectPosition(
clientID, handle, -1, vrep.simx_opmode_blocking)
return absolute_pos
def add_dummy(clientID, size, color, theta, base_joint_handle):
S = np.array([[0., 0., 0., 0., 0., 0., 0.], [0., 1., 0., -1., 0., 1., 0.], [1., 0., 1., 0., 1., 0., 1.], [0., -0.34, 0., 0.74, 0., -1.14, 0.], [0., 0., 0., 0., 0., 0., 0.], [0., 0., 0., 0., 0., 0., 0.]])
errorCode, dummy_handle = vrep.simxCreateDummy(clientID, 0.1, color, vrep.simx_opmode_oneshot_wait)
end_position = forward_kinematics.forwardKinematics(theta)[0:3,3:4]
vrep.simxSetObjectPosition(clientID, dummy_handle, base_joint_handle, end_position, vrep.simx_opmode_oneshot_wait)
return dummy_handle
def notifyCollision(clientID, collision, offset):
result, handle = vrep.simxCreateDummy(clientID, 0.3, None,
vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(clientID, handle, -1,
[1.5, -2.5 + (0.35 * offset), 0],
vrep.simx_opmode_oneshot)
return handle
def get_dummy_handle():
dummy = "Dummy"
# dummy_handles.append(get_obj_handle(dummy, vrep.simx_opmode_blocking))
for i in range(0, p_robot.shape[1]):
print(i)
# dummy_handles.append(get_obj_handle(dummy + str(i), vrep.simx_opmode_blocking))
return_code, handle = vrep.simxCreateDummy(clientID,
dummy_diameter[0][i], None,
vrep.simx_opmode_blocking)
check_error(return_code, "dummy", 1)
dummy_handles.append(handle)
def notifyCollision(clientID, collision, offset):
# Create red dummy if in collsion
if collision:
result, handle = vrep.simxCreateDummy(clientID, 0.3, [255, 0, 0],
vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(clientID, handle, -1,
[1.5, -2.5 + (0.35 * offset), 0],
vrep.simx_opmode_oneshot)
return handle
# Create green dummy if in collision
else:
result, handle = vrep.simxCreateDummy(clientID, 0.3, [0, 255, 0],
vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(clientID, handle, -1,
[1.5, -2.5 + (0.35 * offset), 0],
vrep.simx_opmode_oneshot)
return handle
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)
def create_dummy(self, pos):
retCode, dummyHandle = vrep.simxCreateDummy(self.client_id, 0.2,
(128, 128, 128),
vrep.simx_opmode_blocking)
self.set_object_position(dummyHandle, pos)
# Loop through all the robot's joints and get unique ID's for each
joint_handles = []
for i in range(1, 8):
joint_name = 'LBR_iiwa_7_R800_joint' + str(i)
errorCode, handle = vrep.simxGetObjectHandle(clientID, joint_name, vrep.simx_opmode_oneshot_wait)
joint_handles.append(handle)
# Get a handle for the movable inverse kinematics dummy
errorCode, target_dummy_handle = vrep.simxGetObjectHandle(clientID, "Dummy", vrep.simx_opmode_oneshot_wait)
# Adding robot dummies representing bounding volumes onto joint
robot_joint_bounding_handles = []
for joint in range(7):
# Make a Dummy for each joint
if joint == 5:
errorCode, bounding_handle = vrep.simxCreateDummy(clientID, 0.10, [125,125,125], vrep.simx_opmode_oneshot_wait)
elif joint == 6:
errorCode, bounding_handle = vrep.simxCreateDummy(clientID, 0.08, [125,125,125], vrep.simx_opmode_oneshot_wait)
else:
errorCode, bounding_handle = vrep.simxCreateDummy(clientID, 0.15, [125,125,125], vrep.simx_opmode_oneshot_wait)
robot_joint_bounding_handles.append(bounding_handle)
vrep.simxSetObjectParent(clientID, robot_joint_bounding_handles[joint], joint_handles[joint], False, vrep.simx_opmode_oneshot_wait)
# Kevin
# # print("Building wall!")
# wall_handles = []
# # for z_pos in range(1, 2):
# for z_pos in range(1, 12, 2):
# for x_pos in range(-10, 10, 2):
# # for x_pos in range(1, 2):
# errorCode, bounding_handle = vrep.simxCreateDummy(clientID, 0.10, [200,200,200], vrep.simx_opmode_oneshot_wait)
self.parent = parent
# Close all open connections (just in case)
vrep.simxFinish(-1)
# Connect to V-REP (raise exception on failure)
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if clientID == -1:
raise Exception('Failed connecting to remote API server')
################################################################################
inital_S = []
# Dummies for Cuboids
result, dummy_0 = vrep.simxCreateDummy(clientID, 0.1, None,
vrep.simx_opmode_blocking)
# Get handle for the Cuboid
result, Cuboid_handle = vrep.simxGetObjectHandle(clientID, 'Cuboid',
vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('could not get object handle for the Cuboid')
# Get position of the Cuboid
result, Cuboid_position = vrep.simxGetObjectPosition(clientID, Cuboid_handle,
-1,
vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception('Could not get object position for the Cuboid')
Cuboid_position = np.reshape(Cuboid_position, (3, 1)) # Position of the cuboid
# print ("Cuboid position", Cuboid_position)
def perform_IK(T_1, M, S, dof, frame_selection, Larm_joints, Rarm_joints,
body_joints, joint_bodynames, joint_Rarm, joint_Larm,
clientID): #syntax follows homework syntax
theta = np.random.rand(dof, 1).tolist()
theta = [theta[0] for theta in theta]
samples = 10
V_threshold = 0.1
theta_threshold = 0.01
theta_err = 10
S_bracketlist = []
rot, pos = create_rottransmatrix(T_1)
rot_angles = rotMatrix2Euler(rot)
rot_angles = [rot_angles[0] for angle in rot_angles]
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
# Wait for simulation to settle down
time.sleep(2)
res, dummy = vrep.simxCreateDummy(clientID, 0.25, [],
vrep.simx_opmode_blocking)
res, base_handler = vrep.simxGetObjectHandle(clientID,
'Baxter_base_visible',
vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(clientID, dummy, base_handler, pos,
vrep.simx_opmode_blocking)
vrep.simxSetObjectOrientation(clientID, dummy, base_handler, rot_angles,
vrep.simx_opmode_blocking)
time.sleep(2)
for vals in S.T:
s_bracket = create_bracket(vals)
S_bracketlist.append(s_bracket)
for i in range(1, samples):
if frame_selection == 1:
movebody(clientID, body_joints, joint_bodynames, theta)
elif frame_selection == 2:
move_leftarm(clientID, Larm_joints, joint_Larm, theta)
elif frame_selection == 3:
move_rightarm(clientID, Rarm_joints, joint_Rarm, theta)
S_explist, T_2 = perform_FK(S_bracketlist, theta, M)
combined_T = np.dot(T_2, np.linalg.inv(T_1))
V = la.logm(combined_T)
V_screw = mat2screw(V)
V_screw = np.asarray(V_screw)
V_err = la.norm(V_screw)
if V_err < V_threshold or theta_err < theta_threshold:
rot, pos = create_rottransmatrix(T_2)
rot_angles = rotMatrix2Euler(rot)
rot_angles = [rot_angles[0] for angle in rot_angles]
res, dummy2 = vrep.simxCreateDummy(clientID, 0.25, [0, 255, 0],
vrep.simx_opmode_blocking)
res, base_handler = vrep.simxGetObjectHandle(
clientID, 'Baxter_base_visible', vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(clientID, dummy2, base_handler, pos,
vrep.simx_opmode_blocking)
vrep.simxSetObjectOrientation(clientID, dummy2, base_handler,
rot_angles,
vrep.simx_opmode_blocking)
return theta
J = get_Jacobian(S_explist, S)
theta_dot = np.dot(la.pinv(J.T), V_screw)
theta_err = np.linalg.norm(theta_dot)
theta = theta - (theta_dot)
print "Simulation did not converge"
return theta
for i in range(len(paths[0])):
foundPath[i] = vert[paths[0][i]]
# Voronoi graph visualisation in V-Rep
# black = [0, 0, 0, 0, 0, 0, 64, 64, 64, 0, 0, 0]
# maxCost = max(costs)
# for i in range(1, len(vert)-1):
# if costs[i] == -1:
# _, temp = vrep.simxCreateDummy(clientID, 0.1, black, vrep.simx_opmode_oneshot_wait)
# else:
# color = ut.get_color(costs[i]/maxCost)+[0, 0, 0, 64, 64, 64, 0, 0, 0]
# _, temp = vrep.simxCreateDummy(clientID, 0.1, color, vrep.simx_opmode_oneshot_wait)
# vrep.simxSetObjectPosition(clientID, temp, -1, vert[i], vrep.simx_opmode_oneshot_wait)
# path visualisation in V-Rep
blue = [0, 0, 255, 0, 0, 0, 64, 64, 64, 0, 0, 0]
for i in range(1, len(foundPath) - 1):
_, temp = vrep.simxCreateDummy(clientID, 0.15, blue,
vrep.simx_opmode_oneshot_wait)
vrep.simxSetObjectPosition(clientID, temp, -1, foundPath[i],
vrep.simx_opmode_oneshot_wait)
# start boids program
t = boids.start(clientID, quads, targets, speed, sensors, foundPath, None,
leader)
print('Operation time: ' + str(t) + 's\n')
# end program and close the connection
print('Simulation end')
vrep.simxFinish(clientID)
def create_dummy(self, pos: List[float], size: float = 0.2):
ret, dummy_handle = vrep.simxCreateDummy(self.id, size, None,
vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(self.id, dummy_handle, -1, pos,
vrep.simx_opmode_blocking)
def main(args):
# Close all open connections (just in case)
vrep.simxFinish(-1)
# Connect to V-REP (raise exception on failure)
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if clientID == -1:
raise Exception('Failed connecting to remote API server')
# Start simulation
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
MLeft = np.array([[-1, 0, 0, -0.1278], [0, 0, 1, 1.3363],
[0, 1, 0, 1.2445], [0, 0, 0, 1]])
SLeft = np.zeros((6, 8))
SLeft[:, 0] = screw(0, 0, 1, 0.0103, 0.0147, 0)
SLeft[:, 1] = screw(0, 0, 1, -0.1278, 0.2630, 0)
SLeft[:, 2] = screw(-1, 0, 0, -0.1278, 0.310, 1.3244)
SLeft[:, 3] = screw(0, 1, 0, -0.1278, 0.4140, 1.3244)
SLeft[:, 4] = screw(-1, 0, 0, -0.1278, 0.6765, 1.2554)
SLeft[:, 5] = screw(0, 1, 0, -0.1278, 0.7801, 1.2554)
SLeft[:, 6] = screw(-1, 0, 0, -0.1278, 1.0508, 1.2454)
SLeft[:, 7] = screw(0, 1, 0, -0.1278, 1.1667, 1.2454)
MRight = np.array([[0, 0, 1, 1.332], [1, 0, 0, -0.12287],
[0, 1, 0, 1.2445], [0, 0, 0, 1]])
SRight = np.zeros((6, 8))
SRight[:, 0] = screw(0, 0, 1, 0.0103, 0.0147, 0)
SRight[:, 1] = screw(0, 0, 1, 0.2387, -0.1230, 0)
SRight[:, 2] = screw(0, 1, 0, 0.3077, -0.1230, 1.3244)
SRight[:, 3] = screw(1, 0, 0, 0.4097, -0.1230, 1.3244)
SRight[:, 4] = screw(0, 1, 0, 0.6722, -0.1230, 1.2554)
SRight[:, 5] = screw(1, 0, 0, 0.7758, -0.1230, 1.2554)
SRight[:, 6] = screw(0, 1, 0, 1.0465, -0.1230, 1.2454)
SRight[:, 7] = screw(1, 0, 0, 1.1624, -0.1230, 1.2454)
obstacle_centers = []
body_centers = []
arm_centers = []
for j in range(len(obstacle_names)):
result, dummy_handle = vrep.simxGetObjectHandle(
clientID, obstacle_names[j], vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception("Could not get object handle for the Dummy object")
status, position = vrep.simxGetObjectPosition(
clientID, dummy_handle, -1, vrep.simx_opmode_blocking)
obstacle_centers.append(np.array(position))
for k in range(len(body_names)):
result, dummy_handle = vrep.simxGetObjectHandle(
clientID, body_names[k], vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception("Could not get object handle for the Dummy object")
status, position = vrep.simxGetObjectPosition(
clientID, dummy_handle, -1, vrep.simx_opmode_blocking)
body_centers.append(np.array(position))
for h in range(len(arm_names)):
result, dummy_handle = vrep.simxGetObjectHandle(
clientID, arm_names[h], vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception("Could not get object handle for the Dummy object")
status, position = vrep.simxGetObjectPosition(
clientID, dummy_handle, -1, vrep.simx_opmode_blocking)
arm_centers.append(np.array(position))
result, table_handle = vrep.simxGetObjectHandle(clientID,
"customizableTable",
vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception("Could not get object handle for the Table object")
status, table_position = vrep.simxGetObjectPosition(
clientID, table_handle, -1, vrep.simx_opmode_blocking)
dummies = []
for i in range(13):
x_offset = np.array([i * 0.1 - 0.6, 0, 0])
for j in range(9):
y_offset = np.array([0, j * 0.1 - 0.4, 0])
result, handle = vrep.simxCreateDummy(clientID, 0.1, [0, 0, 255],
vrep.simx_opmode_blocking)
dummies.append(handle)
vrep.simxSetObjectPosition(
clientID, handle, -1,
np.array(table_position) + x_offset + y_offset,
vrep.simx_opmode_oneshot)
obstacle_centers.append(
np.array(table_position) + x_offset + y_offset)
obstacle_names.append("dummy")
obstacle_diam.append(0.1)
arm = "left"
if arm == "left":
M = MLeft
S = SLeft
elif arm == "right":
M = MRight
S = SRight
else:
print("Please enter left or right for the arm")
return
result, dumbell_handle = vrep.simxGetObjectHandle(
clientID, "15lbDumbell0", vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception("Could not get object handle for the Dumbell object")
# Go above the weight
status, position = vrep.simxGetObjectPosition(clientID, dumbell_handle, -1,
vrep.simx_opmode_blocking)
pose = np.eye(4)
pose[0:3, 3] = np.array(position) + np.array([0, -0.2, 0.25])
rotation = np.array([[-1, 0, 0], [0, 0, 1], [0, 1, 0]])
pose[0:3, 0:3] = rotation
moveArmAndFrame(clientID, M, S, SLeft, SRight, arm_centers, body_centers,
obstacle_centers, pose, arm, "ReferenceFrame0")
# Go just in front of the weight
pose2 = np.eye(4)
pose2[0:3, 3] = np.array(position) + np.array([0, -0.2, 0])
rotation = np.array([[-1, 0, 0], [0, 0, 1], [0, 1, 0]])
pose2[0:3, 0:3] = rotation
moveArmAndFrame(clientID,
M,
S,
SLeft,
SRight,
arm_centers,
body_centers,
obstacle_centers,
pose2,
arm,
"ReferenceFrame1",
plan=False)
# Get closer to the weight
pose3 = np.eye(4)
pose3[0:3, 3] = np.array(position) + np.array([0, -0.1, 0])
rotation = np.array([[-1, 0, 0], [0, 0, 1], [0, 1, 0]])
pose3[0:3, 0:3] = rotation
moveArmAndFrame(clientID,
M,
S,
SLeft,
SRight,
arm_centers,
body_centers,
obstacle_centers,
pose3,
arm,
"ReferenceFrame2",
plan=False)
time.sleep(1)
# Close the hands
vrep.simxSetIntegerSignal(clientID, "leftGripperClose", 1,
vrep.simx_opmode_oneshot)
#vrep.simxSetIntegerSignal(clientID, "rightGripperClose", 1, vrep.simx_opmode_oneshot)
time.sleep(3)
result, joint_handle = vrep.simxGetObjectHandle(clientID,
"Baxter_leftArm_joint2",
vrep.simx_opmode_blocking)
if result != vrep.simx_return_ok:
raise Exception(
"Could not get object handle for {} arm joint {}".format(
arm, i + 1))
# Set the desired value of the joint variable
vrep.simxSetJointTargetPosition(clientID, joint_handle, -3,
vrep.simx_opmode_oneshot)
time.sleep(3)
# Back to zero position
thetas = [degToRad(-170), 0, 0, 0, 0, 0, 0, 0]
moveTorso(clientID, thetas[0])
moveArmsAndFrames(clientID, MLeft, SLeft, MRight, SRight, thetas[1:])
time.sleep(3)
# Curl
thetas = [
degToRad(-170),
degToRad(-45),
degToRad(60),
degToRad(170), 0, 0, 0,
degToRad(220)
]
for i in range(10):
moveTorso(clientID, thetas[0])
moveArm(arm, clientID, thetas[1:])
if i == 0:
time.sleep(3)
thetas[4] += degToRad(15)
time.sleep(2)
# Back to zero position
thetas = [degToRad(-170), 0, 0, 0, 0, 0, 0, 0]
moveTorso(clientID, thetas[0])
moveArmsAndFrames(clientID, MLeft, SLeft, MRight, SRight, thetas[1:])
# Dab
moveArm("left", clientID, [
-0.35672754, -1.05517622, -1.43581716, 0.96819909, -0.85928368,
1.30047417, -1.54662531
])
time.sleep(2)
moveArm("right", clientID, [
0.29879645, -0.77735934, -1.1740211, 0.44697439, -1.1613436,
-0.00886994, 1.1017402
])
time.sleep(2)
# Open the hands
vrep.simxSetIntegerSignal(clientID, "leftGripperClose", 0,
vrep.simx_opmode_oneshot)
#vrep.simxSetIntegerSignal(clientID, "rightGripperClose", 0, vrep.simx_opmode_oneshot)
time.sleep(10)
clearNotifications(clientID, dummies)
# Stop simulation
vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot)
# Before closing the connection to V-REP, make sure that the last command sent out had time to arrive. You can guarantee this with (for example):
vrep.simxGetPingTime(clientID)
# Close the connection to V-REP
vrep.simxFinish(clientID)
def deriveFK(clientID, Larm_theta, Rarm_theta, body_theta, Larm_joints,
Rarm_joints, body_joints):
#convert each theta into radians
Larm_thetarad = []
Rarm_thetarad = []
body_thetarad = []
if Larm_theta != []:
for theta in Larm_theta:
theta = int(theta) * (np.pi / 180)
Larm_thetarad.append(theta)
if Rarm_theta != []:
for theta in Rarm_theta:
theta = int(theta) * (np.pi / 180)
Rarm_thetarad.append(theta)
if body_theta != []:
for theta in body_theta:
theta = int(theta) * (np.pi / 180)
body_thetarad.append(theta)
dummy_handler = []
dummyres = []
#declare handler parent
res, base_handler = vrep.simxGetObjectHandle(clientID,
'Baxter_base_visible',
vrep.simx_opmode_blocking)
res, Larm_handler = vrep.simxGetObjectHandle(clientID,
'Baxter_leftArm_tip',
vrep.simx_opmode_blocking)
res, Rarm_handler = vrep.simxGetObjectHandle(clientID, 'BaxterGripper',
vrep.simx_opmode_blocking)
res, monitor_handler = vrep.simxGetObjectHandle(clientID, 'Baxter_monitor',
vrep.simx_opmode_blocking)
#Display Coordinate frame via dummy
res, dummy_monitor = vrep.simxCreateDummy(clientID, 0.1, [],
vrep.simx_opmode_blocking)
dummyres.append(res)
dummy_handler.append(dummy_monitor)
res2, dummy_Rarm = vrep.simxCreateDummy(clientID, 0.1, [],
vrep.simx_opmode_blocking)
dummyres.append(res2)
dummy_handler.append(dummy_Rarm)
res3, dummy_Larm = vrep.simxCreateDummy(clientID, 0.1, [],
vrep.simx_opmode_blocking)
dummyres.append(res3)
dummy_handler.append(dummy_Larm)
res4, dummy_base = vrep.simxCreateDummy(clientID, 0.25, [],
vrep.simx_opmode_blocking)
#Will clean up later (place into loops)
positions = []
orientations = []
result, posB = vrep.simxGetObjectPosition(clientID, base_handler,
base_handler,
vrep.simx_opmode_blocking)
result, orientationB = vrep.simxGetObjectOrientation(
clientID, base_handler, base_handler, vrep.simx_opmode_blocking)
result, posL = vrep.simxGetObjectPosition(clientID, Larm_handler,
base_handler,
vrep.simx_opmode_blocking)
result, orientationL = vrep.simxGetObjectOrientation(
clientID, Larm_handler, base_handler, vrep.simx_opmode_blocking)
positions.append(posL)
orientations.append(orientationL)
result, posR = vrep.simxGetObjectPosition(clientID, Rarm_handler,
base_handler,
vrep.simx_opmode_blocking)
result, orientationR = vrep.simxGetObjectOrientation(
clientID, Rarm_handler, base_handler, vrep.simx_opmode_blocking)
positions.append(posR)
orientations.append(orientationR)
result, posM = vrep.simxGetObjectPosition(clientID, monitor_handler,
base_handler,
vrep.simx_opmode_blocking)
result, orientationM = vrep.simxGetObjectOrientation(
clientID, monitor_handler, base_handler, vrep.simx_opmode_blocking)
positions.append(posM)
orientations.append(orientationM)
#Set Original Tool Frame positions
vrep.simxSetObjectPosition(clientID, dummy_base, base_handler, posB,
vrep.simx_opmode_blocking)
vrep.simxSetObjectOrientation(clientID, dummy_base, base_handler,
orientationB, vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(clientID, dummy_Larm, base_handler, posL,
vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(clientID, dummy_Rarm, base_handler, posR,
vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(clientID, dummy_monitor, base_handler, posM,
vrep.simx_opmode_blocking)
vrep.simxSetObjectOrientation(clientID, dummy_Larm, base_handler,
orientationL, vrep.simx_opmode_blocking)
vrep.simxSetObjectOrientation(clientID, dummy_Rarm, base_handler,
orientationR, vrep.simx_opmode_blocking)
vrep.simxSetObjectOrientation(clientID, dummy_monitor, base_handler,
orientationM, vrep.simx_opmode_blocking)
#T_matrixlist = obtain_homogeneousTmatrix(clientID, positions, orientations)
for result in dummyres:
if result != vrep.simx_return_ok:
raise Exception('Could not get Dummy')
return dummy_handler, Larm_thetarad, Rarm_thetarad, body_thetarad
print('Make sure both are in the same folder as this file,')
print('or appropriately adjust the file "vrep.py"')
print('--------------------------------------------------------------')
print('')
import time
print('KUKA MOVER started')
vrep.simxFinish(-1) # just in case, close all opened connections
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000,
5) # Connect to V-REP
if clientID != -1:
print('Connected to KUKA Simulator')
# Make a Dummy
errorCode, dummy_handle_0 = vrep.simxCreateDummy(
clientID, 0.1, None, vrep.simx_opmode_oneshot_wait)
# Joint: 1 2 3 4 5 6 7
theta_list = np.array([1.0, 1.57, 1.0, 1.57, 0.0, 1.57, 0.0])
for i in range(7):
theta_list[i] = float(
input("Enter number for joint {}: ".format(i + 1)))
# theta_list[i] = (theta_list[i] * math.pi) / 180
# List to store our joint handles in
joint_handles = []
# Loop through all the robot's joints and get unique ID's for each
for i in range(7):
joint_name = 'LBR_iiwa_7_R800_joint' + str(i + 1)
# Get handles for all the movable collision detection dummies
errorCode, movable_dummy_handle1 = vrep.simxGetObjectHandle(
clientID, "Dummy1", vrep.simx_opmode_oneshot_wait)
errorCode, movable_dummy_handle2 = vrep.simxGetObjectHandle(
clientID, "Dummy2", vrep.simx_opmode_oneshot_wait)
errorCode, movable_dummy_handle3 = vrep.simxGetObjectHandle(
clientID, "Dummy3", vrep.simx_opmode_oneshot_wait)
errorCode, movable_dummy_handle4 = vrep.simxGetObjectHandle(
clientID, "Dummy4", vrep.simx_opmode_oneshot_wait)
errorCode, movable_dummy_handle5 = vrep.simxGetObjectHandle(
clientID, "Dummy5", vrep.simx_opmode_oneshot_wait)
# Create bounding volume dummies for the joints
BOUNDING_VOL_RADIUS = 0.15
END_RADIUS = 0.05
errorCode, bounding_vol1_handle = vrep.simxCreateDummy(
clientID, BOUNDING_VOL_RADIUS, None, vrep.simx_opmode_oneshot_wait)
errorCode, bounding_vol2_handle = vrep.simxCreateDummy(
clientID, BOUNDING_VOL_RADIUS, None, vrep.simx_opmode_oneshot_wait)
errorCode, bounding_vol3_handle = vrep.simxCreateDummy(
clientID, BOUNDING_VOL_RADIUS, None, vrep.simx_opmode_oneshot_wait)
errorCode, bounding_vol4_handle = vrep.simxCreateDummy(
clientID, BOUNDING_VOL_RADIUS, None, vrep.simx_opmode_oneshot_wait)
errorCode, bounding_vol5_handle = vrep.simxCreateDummy(
clientID, BOUNDING_VOL_RADIUS, None, vrep.simx_opmode_oneshot_wait)
errorCode, bounding_vol6_handle = vrep.simxCreateDummy(
clientID, BOUNDING_VOL_RADIUS, None, vrep.simx_opmode_oneshot_wait)
errorCode, bounding_vol7_handle = vrep.simxCreateDummy(
clientID, END_RADIUS, None, vrep.simx_opmode_oneshot_wait)
# Initialize variable which let us check if the dummy has been moved
prev_dummy_pos = None
import vrep
import math
import numpy as np
R = 30
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
_, dsethandle = vrep.simxCreateDummy(clientID, 0.5, None,
vrep.simx_opmode_blocking)
'''
for i in range(0,100):
_, dhandle = vrep.simxCreateDummy(clientID, 0.1, None, vrep.simx_opmode_blocking)
_ = vrep.simxSetObjectParent(clientID,dhandle,dsethandle,True,vrep.simx_opmode_blocking)
_ = vrep.simxSetObjectPosition(clientID, dhandle, -1, [-50 + i * 0.5, 0, 0], vrep.simx_opmode_oneshot)
for i in range(0,100):
_, dhandle = vrep.simxCreateDummy(clientID, 0.1, None, vrep.simx_opmode_blocking)
_ = vrep.simxSetObjectParent(clientID, dhandle, dsethandle, True, vrep.simx_opmode_blocking)
_ = vrep.simxSetObjectPosition(clientID, dhandle, -1, [0, 0 + i*0.5, 0], vrep.simx_opmode_oneshot)
for i in range(0,100):
print("i = " + str(i))
_, dhandle = vrep.simxGetObjectHandle(clientID,"Dummy"+str(100+i),vrep.simx_opmode_blocking)
_ = vrep.simxSetObjectPosition(clientID,dhandle,-1,[0,i*0.5,0],vrep.simx_opmode_streaming)
_ = vrep.simxSetObjectOrientation(clientID,dhandle,-1,[0,0,math.pi/2],vrep.simx_opmode_oneshot)
'''
'''
for theta in np.arange(0,2*math.pi,0.5/R):
_, dhandle = vrep.simxCreateDummy(clientID, 0.1, None, vrep.simx_opmode_blocking)
_ = vrep.simxSetObjectParent(clientID, dhandle, dsethandle, True, vrep.simx_opmode_blocking)
jointPoses = parseUrdfJoint(urdfTree, robotJointNames, robotTree)
vrep.simxFinish(-1) # 关闭所有连接
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5) # 连接到VREP
if clientID != -1:
print('Connected to remote API server')
# 关闭动力学
vrep.simxSetBooleanParameter(clientID,
vrep.sim_boolparam_dynamics_handling_enabled,
False, vrep.simx_opmode_blocking)
virtualJHandles = []
for jname, jpose in jointPoses.items():
translate, angles = jpose
res, dummy = vrep.simxCreateDummy(clientID, 0.05, None,
vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(clientID, dummy, -1, translate,
vrep.simx_opmode_blocking)
vrep.simxSetObjectOrientation(clientID, dummy, -1, angles,
vrep.simx_opmode_blocking)
virtualJHandles.append(dummy)
# 1.显示消息并返回反馈
emptyBuff = bytearray()
# clientID,脚本依附对象名称,脚本类型,函数名称,输入int型,输入float型,输入string型,输入bytearray型(应当为空),阻赛模式
# returnCode,输出int型,输出float型,输出string型,输出bytearray
res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(
clientID, 'remoteApiCommandServer', vrep.sim_scripttype_childscript,
'writeRobot', virtualJHandles, [], robotLinkNames, emptyBuff,
vrep.simx_opmode_blocking)
if res == vrep.simx_return_ok:
