def spiral_move(radius, height):
err, Qpos = vrep.simxGetObjectPosition(clientID, QuadricopterT, -1,
vrep.simx_opmode_oneshot_wait)
x = Qpos[0]
y = Qpos[1]
z = Qpos[2]
go_to_z(height)
z = height
theta = 0
go_to_x(x + radius * np.cos(theta))
time.sleep(2)
theta = 0
while True:
err = vrep.simxSetObjectPosition(
clientID, QuadricopterT, -1,
(x + radius * np.cos(theta), y + radius * np.sin(theta), z),
vrep.simx_opmode_oneshot_wait)
time.sleep(0.01)
theta += np.pi / (90 * radius + 1)
radius /= 1.001
err, Qpos = vrep.simxGetObjectPosition(clientID, QuadricopterT, -1,
vrep.simx_opmode_oneshot_wait)
print(Qpos[0]**2 + Qpos[1]**2)
def go_to_y(y1):
global x, y, z
err, Qpos = vrep.simxGetObjectPosition(clientID, QuadricopterT, -1,
vrep.simx_opmode_oneshot_wait)
x = Qpos[0]
y = Qpos[1]
z = Qpos[2]
if y < y1:
sign = -1
elif y > y1:
sign = 1
else:
return 0
while abs(y - y1) > 0.01:
err = vrep.simxSetObjectPosition(clientID, QuadricopterT, -1,
(x, y, z),
vrep.simx_opmode_oneshot_wait)
time.sleep(0.2)
y -= sign * 0.01
print(y)
print('\n')
err = vrep.simxSetObjectPosition(clientID, QuadricopterT, -1, (x, y1, z),
vrep.simx_opmode_oneshot_wait)
time.sleep(0.2)
err, Qpos = vrep.simxGetObjectPosition(clientID, QuadricopterT, -1,
vrep.simx_opmode_oneshot_wait)
print(Qpos)
y = y1
def go_to_z(z1):
global x, y, z
err, Qpos = vrep.simxGetObjectPosition(clientID, QuadricopterT, -1,
vrep.simx_opmode_oneshot_wait)
x = Qpos[0]
y = Qpos[1]
z = Qpos[2]
if z < z1:
sign = -1
elif z > z1:
sign = 1
else:
return 0
while abs(z - z1) > 0.01:
err = vrep.simxSetObjectPosition(clientID, QuadricopterT, -1,
(x, y, z),
vrep.simx_opmode_oneshot_wait)
time.sleep(0.2)
z -= sign * 0.01
print(z)
print('\n')
err = vrep.simxSetObjectPosition(clientID, QuadricopterT, -1, (x, y, z1),
vrep.simx_opmode_oneshot_wait)
time.sleep(0.2)
err, Qpos = vrep.simxGetObjectPosition(clientID, QuadricopterT, -1,
vrep.simx_opmode_oneshot_wait)
print(Qpos)
z = z1
def get_forward_kinematic(self):
fk_list = []
res, tran = sim.simxGetObjectPosition(self.clientID, self.joints[0],
-1, sim.simx_opmode_blocking)
res, rot = sim.simxGetObjectOrientation(self.clientID, self.joints[0],
-1, sim.simx_opmode_blocking)
fk_list.append(util.matrix_transform(np.array(rot), np.array(tran)))
for idx in range(1, 7):
# transformation to parent frame
res, tran = sim.simxGetObjectPosition(self.clientID,
self.joints[idx],
self.joints[idx - 1],
sim.simx_opmode_blocking)
res, rot = sim.simxGetObjectOrientation(self.clientID,
self.joints[idx],
self.joints[idx - 1],
sim.simx_opmode_blocking)
fk_list.append(util.matrix_transform(rot, np.array(tran)))
# endeffector
res, tran = sim.simxGetObjectPosition(
self.clientID, self.endEffector, self.joints[len(self.joints) - 1],
sim.simx_opmode_blocking)
res, rot = sim.simxGetObjectOrientation(
self.clientID, self.endEffector, self.joints[len(self.joints) - 1],
sim.simx_opmode_blocking)
fk_list.append(util.matrix_transform(rot, np.array(tran)))
self.fk = np.array(fk_list)
def forward_kinematics_test(self):
self.get_forward_kinematic(self)
self.get_current_pose(self)
for i in range(100):
config = np.random.rand(6)
self.apply_forward_kinematics(config)
self.apply_control(self.control)
self.compare_pose()
time.sleep(1)
return
# taget position
targetID, targetPos, targetRot = self.determine_target('target')
res, targetPos = sim.simxGetObjectPosition(self.clientID, targetID,
self.worldCoord,
sim.simx_opmode_oneshot)
res, targetRot = sim.simxGetObjectOrientation(self.clientID, targetID,
self.worldCoord,
sim.simx_opmode_oneshot)
x, y, z = targetPos
rx, ry, rz = targetRot
target = np.array([[x, y, z, rx, ry, rz]])
# get endeffector
res, tran = sim.simxGetObjectPosition(self.clientID, self.endEffector,
self.worldCoord,
sim.simx_opmode_oneshot)
res, rot = sim.simxGetObjectOrientation(self.clientID,
self.endEffector,
self.worldCoord,
sim.simx_opmode_oneshot)
end_pos = np.array([tran, rot]).flatten()
def compute_jacobian(self, targetPos, targetRot):
# target position in global frame
x, y, z = targetPos
rx, ry, rz = targetRot
target = np.array([[x, y, z, rx, ry, rz]])
# end effector position in global frame
res, endEffPos = sim.simxGetObjectPosition(self.clientID,
self.endEffector,
self.worldCoord,
sim.simx_opmode_blocking)
x, y, z = endEffPos
res, endEffRot = sim.simxGetObjectOrientation(self.clientID,
self.endEffector,
self.worldCoord,
sim.simx_opmode_blocking)
rx, ry, rz = endEffRot
endEffectorPos = np.array([[x, y, z, rx, ry, rz]])
error = target - endEffectorPos
# start computing jacobian
jacobian = []
joint_q = []
res, trans1 = sim.simxGetObjectPosition(
self.clientID, self.endEffector, self.worldCoord,
sim.simx_opmode_blocking) # end effector in world
for idx in range(len(self.joints)):
# compute local jacobian
local_rot_axis = self.rot_axis[idx]
# find the current joint configuration
res, local_config = sim.simxGetJointPosition(
self.clientID, self.joints[idx], sim.simx_opmode_blocking)
joint_q.append((local_config))
# find transform matrix
res, rot = sim.simxGetObjectOrientation(self.clientID,
self.joints[idx],
self.worldCoord,
sim.simx_opmode_blocking)
rot_mat = util.get_rotation_matrix(rot)
# rotation axis in world coordinate
local_rot_axis = rot_mat.dot(local_rot_axis).flatten()
# joint position in world
res, trans2 = sim.simxGetObjectPosition(self.clientID,
self.joints[idx],
self.worldCoord,
sim.simx_opmode_blocking)
# find the translation from local position to world position
zi = np.array(trans1) - np.array(trans2)
j1 = np.cross(local_rot_axis, zi)
local_jacobian = np.hstack([j1, local_rot_axis])
jacobian.append(local_jacobian)
jacobian = np.array(jacobian).T
joint_q = np.array([joint_q])
# INVERSE JACOBIAN
return jacobian, error, joint_q
def calc_distance(self):
"""Calculates the distance between the end effector and a target position
Args:
Returns:
right_distance
left_distance
"""
error_code, self.right_xyz_hand = vrep.simxGetObjectPosition(
self.clientID, self.right_hand, -1,
vrep.simx_opmode_buffer) # right hand
error_code, self.left_xyz_hand = vrep.simxGetObjectPosition(
self.clientID, self.left_hand, -1,
vrep.simx_opmode_buffer) # left hand
# removed getting right_target location each time- using the static initial location
# error_code, self.right_xyz_target = vrep.simxGetObjectPosition(self.clientID, self.right_target, -1,
# vrep.simx_opmode_buffer)
# TODO set this to main target, find static point.
# need to check if this formula is calculating distance properly
right_distance = 1 / math.sqrt(
pow((self.right_xyz_hand[0] - self.right_xyz_target[0]), 2) +
pow((self.right_xyz_hand[1] - self.right_xyz_target[1]), 2) +
pow((self.right_xyz_hand[2] - self.right_xyz_target[2]), 2))
left_distance = 1 / math.sqrt(
pow((self.left_xyz_hand[0] + self.left_xyz_target[0]), 2) +
pow((self.left_xyz_hand[1] + self.left_xyz_target[1]), 2) +
pow((self.left_xyz_hand[2] + self.left_xyz_target[2]), 2))
return right_distance, left_distance
def step(self, action, reset):
if sys.version_info[0] == 3:
_ = sim.simxSetObjectPosition(self.clientID, self.target, -1,
self.target_position,
sim.simx_opmode_oneshot)
sensor_data = np.zeros((self.total_sensors), dtype=np.float32)
vel_reading = np.zeros((2), dtype=np.float32)
angular_reading = 0
collision = np.zeros((2), dtype=np.float32)
target_location = np.zeros((3), dtype=np.float32)
target_location = np.round_(
np.subtract(np.array(self.position[:2]),
np.array(self.target_position[:2])), 3)
if (reset == False):
if (self.flag):
_, target_location = sim.simxGetObjectPosition(
self.clientID, self.target, -1, sim.simx_opmode_buffer)
_, bot_location = sim.simxGetObjectPosition(
self.clientID, self.bot, -1, sim.simx_opmode_buffer)
target_location = np.round_([
bot_location[0] - target_location[0],
bot_location[1] - target_location[1]
], 3)
self.flag = True
speed = (self.velocity * action[0]) / 100
turn = (self.angular_velocity * action[1]) / 100
l_wheel_vel = round(
(speed - self.wheel_basis * turn) / self.wheel_radius, 4)
r_wheel_vel = round(
(speed + self.wheel_basis * turn) / self.wheel_radius, 4)
_ = sim.simxSetJointTargetVelocity(self.clientID,
self.left_wheel,
l_wheel_vel,
sim.simx_opmode_streaming)
_ = sim.simxSetJointTargetVelocity(self.clientID,
self.right_wheel,
r_wheel_vel,
sim.simx_opmode_streaming)
#Collision
_, collision[0] = sim.simxGetIntegerSignal(
self.clientID, "collision_wall", sim.simx_opmode_buffer)
_, collision[1] = sim.simxGetIntegerSignal(
self.clientID, "collision_target", sim.simx_opmode_buffer)
sensor_data = self._readsensor_continue()
vel_reading, angular_reading = self._get_velocity_reading_continue(
)
else:
self._create(self.position)
#_ = sim.simxSetObjectPosition(self.clientID, self.target, self.bot, self.target_position, sim.simx_opmode_oneshot)
#target_location = self.position - self.target_position
sim.simxSynchronousTrigger(self.clientID)
return sensor_data, vel_reading, angular_reading, target_location, collision
else:
raw_input('Press <enter> key to step the !')
def andar_em_metros(d, v, m):
# d = 1 , andar para frente
# d =-1 , andar para trás
# v = velocidade
# m = valor em metros
erro, a_inicial = sim.simxGetObjectPosition(clientID, robo, -1,
sim.simx_opmode_blocking)
x_inicial = a_inicial[0]
y_inicial = a_inicial[1]
while (True):
erro, a = sim.simxGetObjectPosition(clientID, robo, -1,
sim.simx_opmode_blocking)
x = a[0]
y = a[1]
#print(x,y)
if (abs(x - x_inicial) >= m or abs(y - y_inicial) >= m):
break
sim.simxPauseCommunication(clientID, True)
sim.simxSetJointTargetVelocity(clientID, robotRightMotor, d * v,
sim.simx_opmode_oneshot)
sim.simxSetJointTargetVelocity(clientID, robotLeftMotor, d * v,
sim.simx_opmode_oneshot)
sim.simxPauseCommunication(clientID, False)
Stop()
def get_b_parameter(self):
_, rr = sim.simxGetObjectPosition(self.client_id,
self.right_motor_handle, -1,
sim.simx_opmode_oneshot_wait)
_, rl = sim.simxGetObjectPosition(self.client_id,
self.left_motor_handle, -1,
sim.simx_opmode_oneshot_wait)
return np.sqrt((rr[0] - rl[0])**2 + (rr[1] - rl[1])**2 +
(rr[2] - rl[2])**2) / 2
def ResetSimulationScene(self):
errorCode, resolution, image = sim.simxGetVisionSensorImage(
self.clientID, self.camera, 0, sim.simx_opmode_streaming)
pos = sim.simxGetObjectPosition(self.clientID, self.khepera, -1,
sim.simx_opmode_streaming)
ori_body = sim.simxGetObjectOrientation(self.clientID, self.khepera,
-1, sim.simx_opmode_streaming)
tar = sim.simxGetObjectPosition(self.clientID, self.target, -1,
sim.simx_opmode_streaming)
def drone_position(args):
drones = [[] for i in range(3)]
drones_names = [
'Quadricopter_base', 'Quadricopter_base#0', 'Quadricopter_base#1'
]
nodes = []
data = [[] for i in range(3)]
if len(args) > 1:
for n in range(1, len(args)):
nodes.append(args[n])
else:
info("No nodes defined")
exit()
info('Program started')
sim.simxFinish(-1) # just in case, close all opened connections
clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000,
5) # Connect to CoppeliaSim
if clientID != -1:
res = None
info('Connected to remote API server')
# Getting the ID of the drones from the simulation
for i in range(0, len(drones)):
[res,
drones[i]] = sim.simxGetObjectHandle(clientID, drones_names[i],
sim.simx_opmode_oneshot_wait)
if res == sim.simx_return_ok:
info('Connected with CoppeliaSim')
else:
info('Remote API function call returned with error code: ', res)
time.sleep(2)
# Starting the getPosition function streaming
for i in range(0, len(drones)):
sim.simxGetObjectPosition(clientID, drones[i], -1,
sim.simx_opmode_streaming)
while True:
# Getting the positions as buffers
for i in range(0, len(drones)):
# Try to retrieve the streamed data
returnCode, data[i] = sim.simxGetObjectPosition(
clientID, drones[i], -1, sim.simx_opmode_buffer)
# Storing the position in data files
for i in range(0, len(data)):
send_file(data[i], nodes[i])
time.sleep(1)
# Now close the connection to CoppeliaSim:
sim.simxFinish(clientID)
else:
info('Failed connecting to remote API server')
info('Program ended')
def init_robot(self):
for h in self.handles:
sim.simxSetJointPosition(self.clientID, h, 0,
sim.simx_opmode_oneshot)
print("termino de inicializar")
_, pos = sim.simxGetObjectPosition(self.clientID, self.j2, -1,
sim.simx_opmode_streaming)
_, pos = sim.simxGetObjectPosition(self.clientID, self.j1, -1,
sim.simx_opmode_streaming)
def __init__(self, queue_dict, sync=True):
"""
this class loads coppeliasim and is responsible for communication between the sim and python...
api functions can be found https://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm
"""
# open queues
self.queues = queue_dict
# close all connection that are remaining
sim.simxFinish(-1)
self.clientID = -1
attempt_num = 0
while self.clientID == -1:
print("attempting to connect to VREP...")
self.clientID = sim.simxStart('127.0.0.1', 19999,
True, True, 5000, 5)
attempt_num += 1
if attempt_num >= 3:
print("could not connect to vrep")
return
print("successful connection!")
self.sync = sync
if self.sync:
# set the simulation to synchronise with api
sim.simxSynchronous(self.clientID, True)
# get coppeliasim object handles
self.motor = {'left': self.getHandle("Pioneer_p3dx_leftMotor"),
'right': self.getHandle("Pioneer_p3dx_rightMotor")}
self.camHandle = self.getHandle("camera")
#self.gripperHandle = {"left": self.getHandle("left_joint"),
# "right": self.getHandle("right_joint")}
# init camera data stream with rgb and depth
sim.simxGetVisionSensorImage(self.clientID, self.camHandle, 0, sim.simx_opmode_streaming)
sim.simxGetVisionSensorDepthBuffer(self.clientID, self.camHandle, sim.simx_opmode_streaming)
# init position data stream with cartesian position and euler orientation
sim.simxGetObjectOrientation(self.clientID, self.camHandle, -1, sim.simx_opmode_streaming)
sim.simxGetObjectPosition(self.clientID, self.camHandle, -1, sim.simx_opmode_streaming)
# setting motor speed
self.speed = 1
self.lr = 0.3
self.images = None
# remote start simulation if not already
sim.simxStartSimulation(self.clientID, sim.simx_opmode_oneshot)
# used for making keyboard switches
self.keyboard_key = Clicker("c", activated=True)
self.keyboard_controlled = self.keyboard_key.activated
self.gripper_key = Clicker("g", activated=False)
self.gripper_activated = self.gripper_key.activated
def track(self):
print("start tracking")
while True:
if (self.pos[0] - self.pickupPath[-1][0])**2 + (
self.pos[1] - self.pickupPath[-1][1])**2 < 2:
self.robot.setLeftVelocity(0)
self.robot.setRightVelocity(0)
break
lookpoint = self.lookhead(self.pos, self.pickupPath)
curv = self.curvature(lookpoint)
wheels = self.turn(curv, self.width)
self.robot.setLeftVelocity(wheels[0])
self.robot.setRightVelocity(-wheels[1])
self.pos = self.robot.getPos()[0:2]
self.angle = self.robot.getAngle()
self.socketio.sleep(0)
self.reset()
print("start picking")
print(self.robot)
err, platform = sim.simxGetObjectHandle(
self.warehouse.client,
"platform_" + self.robot.id,
sim.simx_opmode_blocking,
)
err, pos = sim.simxGetObjectPosition(self.warehouse.client, platform,
-1, sim.simx_opmode_blocking)
pos[2] = pos[2] + 0.1
self.warehouse.movePackage(self.pkg, pos, platform)
self.warehouse.movePackage(self.pkg, pos, platform)
self.warehouse.movePackage(self.pkg, pos, platform)
while True:
if (self.pos[0] - self.dropPath[-1][0])**2 + (
self.pos[1] - self.dropPath[-1][1])**2 < 2:
self.robot.setLeftVelocity(0)
self.robot.setRightVelocity(0)
break
lookpoint = self.lookhead(self.pos, self.dropPath)
curv = self.curvature(lookpoint)
wheels = self.turn(curv, self.width)
self.robot.setLeftVelocity(wheels[0])
self.robot.setRightVelocity(-wheels[1])
self.pos = self.robot.getPos()[0:2]
self.angle = self.robot.getAngle()
self.socketio.sleep(0)
err, pos = sim.simxGetObjectPosition(self.warehouse.client, platform,
-1, sim.simx_opmode_blocking)
pos[2] = 0.1
pos[0] = pos[0] - 0.3
pos[1] = pos[1] - 0.3
self.warehouse.movePackage(self.pkg, pos, -1)
self.warehouse.movePackage(self.pkg, pos, -1)
def compare_pose(self):
# get the current 6-D position of robot
for idx in range(len(self.joints)):
res, tran = sim.simxGetObjectPosition(self.clientID,
self.joints[idx],
self.worldCoord,
sim.simx_opmode_blocking)
print(np.array(util.homo_coord(tran)) - self.cur_pos[idx])
res, tran = sim.simxGetObjectPosition(self.clientID, self.endEffector,
self.worldCoord,
sim.simx_opmode_blocking)
print(np.array(util.homo_coord(tran)) - self.cur_pos[6])
def ResetSimulationScene(self):
errorCode, resolution, image = sim.simxGetVisionSensorImage(
self.clientID, self.camera, 0, sim.simx_opmode_streaming)
pos = sim.simxGetObjectPosition(self.clientID, self.khepera, -1,
sim.simx_opmode_streaming)
ori_body = sim.simxGetObjectOrientation(self.clientID, self.khepera,
-1, sim.simx_opmode_streaming)
tar = sim.simxGetObjectPosition(self.clientID, self.target, -1,
sim.simx_opmode_streaming)
for i in range(8):
handle = self.sensor[i]
errorCode, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector = sim.simxReadProximitySensor(
self.clientID, handle, sim.simx_opmode_streaming)
def step(self, action):
if action <=5:
x,y,z = self.moves[action]
self.S = [self.S[0] + x, self.S[1] + y, self.S[2] + z]
if self.S[0]<0 or self.S[1]<0 or self.S[2]<0:
self.S = [max(0, self.S[0]), max(0, self.S[1]), max(0, self.S[2])]
return self.S,-10,False,{}
if self.S[0]>=5 or self.S[1]>=5 or self.S[2]>=10:
self.S = [min(self.S[0], 5 - 1), min(self.S[1], 5 - 1), min(self.S[2], 10 - 1)]
return self.S,-10,False,{}
else:
sim.simxSetIntegerSignal(self.clientID,'actuateGripperSignal',0,sim.simx_opmode_oneshot)
sim.simxSetObjectPosition(self.clientID, self.target,-1, [0+0.1*self.S[0],0+0.1*self.S[1],0+0.1*self.S[2]],sim.simx_opmode_oneshot) #starting point is (0.3,0.3,0.3)
while True:
distance = sim.simxGetObjectPosition(self.clientID,self.copter,self.target,sim.simx_opmode_oneshot_wait)
if np.abs(np.linalg.norm(np.array(distance[1])))<0.02:
time.sleep(2)
break
# if (sim.simxGetCollisionHandle(self.clientID,))
return self.S,-1,False,{}
elif action==6:
# pdb.set_trace()
gripperAngle = 150
angle_displacement = (gripperAngle/2-45)*math.pi/180
err, joint_6 = sim.simxGetObjectHandle(self.clientID, "Fourbar_joint6", sim.simx_opmode_oneshot)
# sim.simxClearIntegerSignal(self.clientID,'actuateGripperSignal',sim.simx_opmode_oneshot)
# sim.simxClearIntegerSignal(self.clientID,'gripperAngle',sim.simx_opmode_oneshot)
sim.simxSetIntegerSignal(self.clientID,'actuateGripperSignal',1,sim.simx_opmode_oneshot)
sim.simxSetIntegerSignal(self.clientID,'gripperAngle',gripperAngle,sim.simx_opmode_oneshot)
while True:
current_joint6_angle = sim.simxGetJointPosition(self.clientID, joint_6, sim.simx_opmode_oneshot)
if abs(abs(current_joint6_angle[1])-abs(angle_displacement))<0.05*math.pi/180:
time.sleep(5)
break
oldPos = sim.simxGetObjectPosition(self.clientID, self.sphere, -1, sim.simx_opmode_oneshot)
sim.simxSetObjectPosition(self.clientID, self.target, self.copter, [0,0,0.5], sim.simx_opmode_oneshot)
while True:
distance = sim.simxGetObjectPosition(self.clientID,self.copter,self.target,sim.simx_opmode_oneshot_wait)
if np.abs(np.linalg.norm(np.array(distance[1])))<0.02:
time.sleep(2)
break
newPos = sim.simxGetObjectPosition(self.clientID, self.sphere, -1, sim.simx_opmode_oneshot)
if np.linalg.norm(np.array(newPos[1])-np.array(oldPos[1]))>0.4 and np.linalg.norm(np.array(newPos)-np.array(oldPos))<0.5:
return self.S,100,True,{}
else:
return self.S,-100,True,{}
def get_current_pose(self):
cur_pos = []
# get the current 6-D position of robot
for idx in range(len(self.joints)):
res, tran = sim.simxGetObjectPosition(self.clientID,
self.joints[idx], -1,
sim.simx_opmode_blocking)
cur_pos.append(np.array(util.homo_coord(tran)))
# get the curretn end effector position
res, tran = sim.simxGetObjectPosition(self.clientID, self.endEffector,
-1, sim.simx_opmode_blocking)
cur_pos.append(np.array(util.homo_coord(tran)))
self.cur_pos = np.array(cur_pos)
def check_suction_distance(self):
error_code, self.right_xyz_hand = vrep.simxGetObjectPosition(
self.clientID, self.right_hand, -1,
vrep.simx_opmode_buffer) # right hand
error_code, self.left_xyz_hand = vrep.simxGetObjectPosition(
self.clientID, self.left_hand, -1,
vrep.simx_opmode_buffer) # left hand
distance = math.sqrt(
pow((self.right_xyz_hand[0] - self.left_xyz_hand[0]), 2) +
pow((self.right_xyz_hand[1] - self.left_xyz_hand[1]), 2) +
pow((self.right_xyz_hand[2] - self.left_xyz_hand[2]), 2))
return distance
def _getFinalPos(self, initialize=True):
if initialize:
ret, pos = sim.simxGetObjectPosition(self.clientID,
self.testDummyHandle, -1,
sim.simx_opmode_streaming)
else:
ret, pos = sim.simxGetObjectPosition(self.clientID,
self.testDummyHandle, -1,
sim.simx_opmode_buffer)
if ret == sim.simx_return_ok:
return pos
else:
# print('[WARNING] problem in getting tip position.')
return -1
def calcDistance(self):
errorCode, self.xyz_hand = vrep.simxGetObjectPosition(
self.clientID, self.hand, -1, vrep.simx_opmode_buffer)
errorCode, self.xyz_target = vrep.simxGetObjectPosition(
self.clientID, self.target, -1, vrep.simx_opmode_buffer)
# need to check if this formula is calculating distance properly
distance = math.sqrt(
pow((self.xyz_hand[0] - self.xyz_target[0]), 2) +
pow((self.xyz_hand[1] - self.xyz_target[1]), 2) +
pow((self.xyz_hand[2] - self.xyz_target[2]), 2))
return distance
def step(self, action):
assert self.action_space.contains(action)
joint = int(action / 3)
handle = self.joint_handles[joint]
value = self.action_map[action % 3]
if joint in self.faulty_joints:
value = 0
#for angle, handle in zip(action, self.joint_handles)
cur_pos = sim.simxGetJointPosition(self.clientID, handle, buffer)[1]
sim.simxSetJointTargetPosition(self.clientID, handle,
cur_pos + value * (3.14 / 180), oneshot)
# give reward for touching the box
reward, done = self.detect_collision()
box_distance = sim.simxGetObjectPosition(self.clientID, self.lamp_end,
self.green_box, buffer)[1]
state = [d for d in box_distance]
for handle in self.joint_handles:
state.append(
sim.simxGetJointPosition(self.clientID, handle, buffer)[1])
return np.array(state), reward, done, {}
def get_position(self) -> list:
"""Retrieves the orientation.
@rtype: Vec3
"""
code, pos = s.simxGetObjectPosition(self._id, self._handle, -1,
self._def_op_mode)
return pos
def __init__(self, clientID):
self.clientID = clientID
self.pioneer3DX_array = [None] * 19
self.position_coordX = [None] * 3
self.position_coordXc = [None] * 3
self.orientation = None
self.velocity = [None]* 2
self.ultrasonic = np.zeros((16, 3))
self.name = "Pioneer_p3dx"
self.left_motor = 'Pioneer_p3dx_leftMotor'
self.right_motor = 'Pioneer_p3dx_rightMotor'
self.ultrasonic_sensors = "Pioneer_p3dx_ultrasonicSensor"
### Load Mobile Robot Pioneer parameters
# self.pioneer3DX_array[0] represents the entire mobile robot block
error,self.pioneer3DX_array[0] = sim.simxGetObjectHandle(self.clientID,self.name,sim.simx_opmode_blocking)
# self.pioneer3DX_array[1] represents only the left motor
error,self.pioneer3DX_array[1] = sim.simxGetObjectHandle(self.clientID,self.left_motor,sim.simx_opmode_blocking)
# self.pioneer3DX_array[2] represents only the right motor
error,self.pioneer3DX_array[2] = sim.simxGetObjectHandle(self.clientID,self.right_motor,sim.simx_opmode_blocking)
# self.pioneer3DX_array[3:18] represents the 16 ultrasonic sensors
num = 1
while num < 17:
error,self.pioneer3DX_array[2+num] = sim.simxGetObjectHandle(self.clientID,self.ultrasonic_sensors+str(num),sim.simx_opmode_blocking)
error, DetectionState, Points ,detectedObjectHandle, Vector = sim.simxReadProximitySensor(self.clientID,self.pioneer3DX_array[2+num],sim.simx_opmode_streaming)
num+=1
error, linearVelocity, angularVelocity = sim.simxGetObjectVelocity(self.clientID,self.pioneer3DX_array[0], sim.simx_opmode_streaming)
error, position = sim.simxGetObjectPosition(self.clientID,self.pioneer3DX_array[0],-1, sim.simx_opmode_streaming)
error, angle = sim.simxGetObjectOrientation(self.clientID,self.pioneer3DX_array[0],-1,sim.simx_opmode_streaming)
print("Pioneer 3DX loaded")
def get_position(self):
error, position = sim.simxGetObjectPosition(self.clientId,
self.pioneerHandle, -1,
sim.simx_opmode_buffer)
if error == sim.simx_return_novalue_flag:
print(str(error) + "! simxGetObjectPosition_buffer")
return position
def get_label(sequence_sample, converter, clientID, Body):
sequence_sample = sequence_sample[0]
for duration in [0.02, 0.05]:
for idx_f in range(sequence_sample.shape[0]):
angle_recon = converter.frameRecon(sequence_sample[idx_f])
# angles: LSP, LSR, LEY, LER, RSP, RSR, REY, RER, LHP, LHR, LHYP, LKP, RHP, RHR, RHYP, RKP, LAP, LAR, RAP, RAR
angles = converter.generateWholeJoints(angle_recon)
assert (len(angles) == 20)
transmit(clientID, Body, angles)
time.sleep(duration)
returnCode, position = sim.simxGetObjectPosition(
clientID, Body['HeadYaw'], -1, sim.simx_opmode_buffer)
if position[2] < 0.4 and position[2] > 0: #fall down
print('fall')
sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot)
print('stop')
time.sleep(.1)
sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot)
print('start')
time.sleep(.1)
#returnCode, position=sim.simxGetObjectPosition(clientID, Body['HeadYaw'], -1, sim.simx_opmode_streaming)
return torch.tensor([0]).long()
sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot)
print('go on: stop')
time.sleep(.1)
sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot)
print('go on: start')
time.sleep(.1)
#returnCode, position=sim.simxGetObjectPosition(clientID, Body['HeadYaw'], -1, sim.simx_opmode_streaming)
return torch.tensor([1]).long()
def getSlist(clientID, joint_handles, base_handle):
q = np.zeros((6, 3))
for i in range(6):
q[i] = np.array(
sim.simxGetObjectPosition(clientID, joint_handles[i], base_handle,
sim.simx_opmode_blocking)[1])
w = np.zeros((6, 3))
w[0] = np.array([0, 0, 1]).T
w[1] = np.array([-1, 0, 0]).T
w[2] = np.array([-1, 0, 0]).T
w[3] = np.array([-1, 0, 0]).T
w[4] = np.array([0, 0, 1]).T
w[5] = np.array([-1, 0, 0]).T
v = np.zeros((6, 3))
for i in range(6):
v[i] = np.cross(-w[i], q[i])
Slist = np.zeros((6, 6))
for i in range(6):
Slist[i, :3] = w[i]
Slist[i, 3:] = v[i]
return Slist
def __init__(self):
try:
sim.simxFinish(-1) #close all opened connections
clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000,
5) # Connect to CoppeliaSim
if clientID != -1:
print('connect successfully')
else:
sys.exit("connect error")
except:
print('Check if CoppeliaSim is open')
_, Quadcopter_target = sim.simxGetObjectHandle(
clientID, 'Quadricopter_target', sim.simx_opmode_blocking)
_, targetPosition = sim.simxGetObjectPosition(clientID,
Quadcopter_target, -1,
sim.simx_opmode_buffer)
print(targetPosition)
ArrayPosition = [-0.18570, 0.99366, 0.615]
sim.simxSetObjectPosition(clientID, Quadcopter_target, -1,
ArrayPosition, sim.simx_opmode_oneshot)
self.clientID = clientID
self.Quadcopter_target = Quadcopter_target
self.targetPosition = targetPosition
def scan(self):
#Method for getting the coordinates of a cloud of detected points in absolute/world reference
xy = []
for sonar in self.sonarHandles[0:16]: #For all sonars do
# Ping the sonar
res, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector = sim.simxReadProximitySensor(
clientID, sonar, sim.simx_opmode_buffer)
# If a point is detected:
if detectionState == 1:
# Get the sensor orientation in Euler Angles for the sensor
res, eulerAngle = sim.simxGetObjectOrientation(
clientID, sonar, -1, sim.simx_opmode_blocking)
# Get the position for the sonar vs the World frame (-1 value sees to that)
res, position = sim.simxGetObjectPosition(
clientID, sonar, -1, sim.simx_opmode_blocking)
# Rotate the coordinates of the found point to receive absolute coordinates (minus the offset of the sensor location)
v1 = rotate(detectedPoint[0], detectedPoint[1],
detectedPoint[2], eulerAngle[0], eulerAngle[1],
eulerAngle[2])
# Append the x and y coordinates of the detected poing plus the location of the sensor to get absolute coordinates
xy.append((position[0] + v1[0], position[1] + v1[1]))
return xy