def __init__(self, port=19997):
self.sim_client = utils.connectToSimulation('127.0.0.1', port)
self.rdd = RDD(self.sim_client, open_force=20, close_force=20)
self.ur5 = UR5(self.sim_client, self.rdd)
self.nA = 5
self.cube = None
self.cube_start_position = [-0.35, 0.7, 0.03]
self.cube_size = [0.1, 0.1, 0.1]
self.open_position = 0.3
def __init__(self, port=19997):
self.sim_client = utils.connectToSimulation('127.0.0.1', port)
# Create UR5 and restart simulator
self.rdd = RDD(self.sim_client, open_force=10)
self.ur5 = UR5(self.sim_client, self.rdd)
self.nA = 2
self.cube = None
self.cube_start_position = [-0.2, 0.9, 0.05]
self.cube_size = [0.1, 0.2, 0.04]
def __init__(self, port=19997, memory_size=60):
self.sim_client = utils.connectToSimulation('127.0.0.1', port)
# Create UR5 and restart simulator
self.rdd = RDD(self.sim_client)
self.ur5 = UR5(self.sim_client, self.rdd)
self.nA = 2
self.cube = None
self.cube_start_position = [-0.2, 0.85, 0.025]
self.cube_size = [0.1, 0.2, 0.04]
self.open_position = 0.3
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 __init__(self, port=19997, memory_size=60):
rospy.init_node('env', anonymous=True)
self.sim_client = utils.connectToSimulation('127.0.0.1', port)
# Create UR5 and restart simulator
self.rdd = RDD(self.sim_client)
self.ur5 = UR5(self.sim_client, self.rdd)
self.nA = 4
self.cube = None
self.cube_start_position = [-0.2, 0.85, 0.025]
self.cube_size = [0.1, 0.2, 0.04]
self.open_position = 0.3
self.narrow_position = None
self.wide_position = None
self.rdd_position = [0 for _ in range(2 * memory_size)]
self.rdd_force = [0 for _ in range(2 * memory_size)]
self.narrow_p = [0 for _ in range(memory_size)]
self.narrow_t = [0 for _ in range(memory_size)]
self.rdd_sub = rospy.Subscriber('sim/rdd_joints',
Float32MultiArray,
self.rddJointsCallback,
queue_size=1)
self.tip_position = None
self.tip_orientation = None
self.tip_pos_sub = rospy.Subscriber('sim/ur5_tip_pose',
Float32MultiArray,
self.tipPosCallback,
queue_size=1)
self.target_position = None
self.target_orientation = None
self.target_pos_sub = rospy.Subscriber('sim/ur5_target_pose',
Float32MultiArray,
self.targetPosCallback,
queue_size=1)
self.cube_position = None
self.cube_orientation = None
self.cube_pos_sub = rospy.Subscriber('sim/cube_pose',
Float32MultiArray,
self.cubePosCallback,
queue_size=1)
def __init__(self, port=19997):
np.random.seed(port)
self.sim_client = utils.connectToSimulation('127.0.0.1', port)
# Create UR5 and restart simulator
self.rdd = RDD(self.sim_client)
self.ur5 = UR5(self.sim_client, self.rdd)
self.sensor = VisionSensor(self.sim_client, 'Vision_sensor', None, None, True, False)
self.nA = 4
self.observation_space = np.zeros(4)
self.cube = None
self.cube_start_position = [-0.2, 0.85, 0.025]
self.cube_size = [0.1, 0.2, 0.04]
self.open_position = 0.3
def __init__(self, port=19997):
rospy.init_node('env', anonymous=True)
self.sim_client = utils.connectToSimulation('127.0.0.1', port)
# Create UR5 and restart simulator
self.rdd = RDD(self.sim_client, open_force=10, close_force=20)
self.ur5 = UR5(self.sim_client, self.rdd)
self.nA = 3
self.cube = None
self.cube_start_position = [-0.2, 0.9, 0.05]
self.cube_size = [0.1, 0.2, 0.04]
self.open_position = 0.3
self.state = [0 for _ in range(4 * 60)]
self.stateSub = rospy.Subscriber('sim/state', Float32MultiArray,
self.stateCallback)
def __init__(self, port=19997):
np.random.seed(port)
self.sim_client = utils.connectToSimulation('127.0.0.1', port)
# Create UR5 and restart simulator
self.rdd = RDD(self.sim_client)
self.ur5 = UR5(self.sim_client, self.rdd)
self.sensor = VisionSensor(self.sim_client, 'Vision_sensor_top', None,
None, True, False)
self.nA = 4
self.observation_space = (np.zeros((12, 64, 64)), np.zeros((4, 20)))
self.cube = None
self.cube_start_position = [-0.2, 0.85, 0.025]
self.cube_size = [0.1, 0.2, 0.04]
self.open_position = 0.3
self.img_his = [np.zeros((3, 64, 64)) for _ in range(4)]
self.theta_his = [np.zeros((1, 20)) for _ in range(4)]