def reset(self):
"""
reset the environment
:return: the observation, List[List[float], List[float]]
"""
vrep.simxStopSimulation(self.sim_client, VREP_BLOCKING)
time.sleep(1)
vrep.simxStartSimulation(self.sim_client, VREP_BLOCKING)
time.sleep(1)
sim_ret, self.cube = utils.getObjectHandle(self.sim_client, 'cube')
utils.setObjectPosition(self.sim_client, self.ur5.UR5_target, [-0.2, 0.6, 0.08])
dy = 0.3 * np.random.random()
# dy = 0
# dz = 0.1 * np.random.random() - 0.05
current_pose = self.ur5.getEndEffectorPose()
target_pose = current_pose.copy()
target_pose[1, 3] += dy
# target_pose[2, 3] += dz
self.rdd.setFingerPos(-0.1)
self.ur5.moveTo(target_pose)
self.narrow_p = []
self.target_position = None
while self.target_position is None:
time.sleep(0.1)
return [0. for _ in range(20)]
def reset(self):
vrep.simxStopSimulation(self.sim_client, VREP_BLOCKING)
time.sleep(1)
vrep.simxStartSimulation(self.sim_client, VREP_BLOCKING)
time.sleep(1)
# Generate a cube
# position = self.cube_start_position
# orientation = [np.radians(90), 0, np.radians(90)]
# # orientation = [0, 0, 0]
# size = self.cube_size
# mass = 0.1
# color = [255, 0, 0]
# self.cube = utils.importShape(self.sim_client, 'cube', CUBE_MESH, position, orientation, color)
# self.cube = utils.generateShape(self.sim_client, 'cube', 0, size, position, orientation, mass, color)
# time.sleep(1)
sim_ret, self.cube = utils.getObjectHandle(self.sim_client, 'cube')
utils.setObjectPosition(self.sim_client, self.ur5.UR5_target,
[-0.2, 0.6, 0.07])
dy = 0.3 * np.random.random()
# dy = 0.3
current_pose = self.ur5.getEndEffectorPose()
target_pose = current_pose.copy()
target_pose[1, 3] += dy
self.ur5.moveTo(target_pose)
# time.sleep(0.5)
self.rdd.openFinger(RDD.NARROW)
return self.rdd.getFingerPosition(RDD.NARROW)
def reset(self):
"""
reset the environment
:return: the observation, List[List[float], List[float]]
"""
vrep.simxStopSimulation(self.sim_client, utils.VREP_BLOCKING)
time.sleep(1)
vrep.simxStartSimulation(self.sim_client, utils.VREP_BLOCKING)
time.sleep(1)
sim_ret, self.cube = utils.getObjectHandle(self.sim_client, 'cube')
self.rdd.setFingerPos(-0.1)
utils.setObjectPosition(self.sim_client, self.ur5.UR5_target, [-0.2, 0.6, 0.08])
# utils.setObjectPosition(self.sim_client, self.ur5.UR5_target, [-0.2, 0.6, 0.15])
dy = 0.3 * np.random.random()
# dz = 0.1 * np.random.random() - 0.05
current_pose = self.ur5.getEndEffectorPose()
target_pose = current_pose.copy()
target_pose[1, 3] += dy
# target_pose[2, 3] += dz
self.sendClearSignal()
self.ur5.moveTo(target_pose)
self.rdd.setFingerPos()
return self.getObs()
def reset(self):
vrep.simxStopSimulation(self.sim_client, VREP_BLOCKING)
time.sleep(1)
vrep.simxStartSimulation(self.sim_client, VREP_BLOCKING)
time.sleep(1)
sim_ret, self.cube = utils.getObjectHandle(self.sim_client, 'cube')
utils.setObjectPosition(self.sim_client, self.ur5.UR5_target,
[-0.2, 0.6, 0.08])
dy = 0.3 * np.random.random()
# dy = 0
# dz = 0.1 * np.random.random() - 0.05
current_pose = self.ur5.getEndEffectorPose()
target_pose = current_pose.copy()
target_pose[1, 3] += dy
# target_pose[2, 3] += dz
self.ur5.moveTo(target_pose)
# self.rdd.open(self.open_position)
self.target_position = None
while self.target_position is None:
time.sleep(0.1)
return self.state
def main():
rospy.init_node('simple_test')
# Attempt to connect to simulator
sim_client = utils.connectToSimulation('127.0.0.1', 19997)
vrep.simxStopSimulation(sim_client, VREP_BLOCKING)
time.sleep(1)
vrep.simxStartSimulation(sim_client, VREP_BLOCKING)
time.sleep(1)
rdd = RDD(sim_client)
ur5 = UR5(sim_client, rdd)
# Generate a cube
position = [-0.2, 0.9, 0.05]
orientation = [0, 0, 0]
# orientation = [0, 0, 0]
size = [0.1, 0.2, 0.05]
mass = 1
color = [255, 0, 0]
cube = utils.generateShape(sim_client, 'cube', 0, size, position,
orientation, mass, color)
time.sleep(1)
pose = ur5.getEndEffectorPose()
target_pose = pose.copy()
# rdd.openFinger(RDD.NARROW)
narrow_finger_joint = rdd.finger_joint_narrow.joint
def print_joint_pos(joint):
while True:
# print utils.getJointPosition(sim_client, joint)
res, cube_pose = utils.getObjectPosition(sim_client, cube)
while any(np.isnan(cube_pose)):
res, cube_pose = utils.getObjectPosition(sim_client, cube)
print cube_pose
thread.start_new_thread(print_joint_pos, (narrow_finger_joint, ))
target_pose[1, 3] += 0.3
rdd.openFinger(RDD.NARROW)
ur5.moveTo(target_pose)
ur5.moveTo(pose)
def reset(self):
vrep.simxStopSimulation(self.sim_client, VREP_BLOCKING)
time.sleep(1)
vrep.simxStartSimulation(self.sim_client, VREP_BLOCKING)
time.sleep(1)
# Generate a cube
position = self.cube_start_position
# orientation = [np.radians(90), 0, np.radians(90)]
orientation = [0, 0, 0]
size = self.cube_size
mass = 10
color = [255, 0, 0]
self.cube = utils.generateShape(self.sim_client, 'cube', 0, size,
position, orientation, mass, color)
self.rdd.open(self.open_position)
utils.setObjectPosition(self.sim_client, self.ur5.UR5_target,
[-0.2, 0.5, 0.05])
# time.sleep(1)
return self.getState()
def restartSimulation(sim_client):
vrep.simxStopSimulation(sim_client, VREP_BLOCKING)
time.sleep(1)
vrep.simxStartSimulation(sim_client, VREP_BLOCKING)
time.sleep(1)
def stopSimulation(sim_client):
vrep.simxStopSimulation(sim_client, VREP_BLOCKING)