# strawberry_radius = 0.015 + 0.06 # 0.075
# start_pose[2] = start_pose[2] + stem_length + strawberry_radius
# start_ori = p.getQuaternionFromEuler([(-m.pi / 1.8), 0, 0])
# strawberry_2 = strawberry_cluster.StrawberryCluster(p, start_pose, start_ori, "models/clusters_urdf/strawberry_cluster.urdf")
## load strawberry cluster:
start_pose = []
start_pose = copy.deepcopy(strawberry_pose)
stem_length = 0.15
strawberry_radius = 0.015 + 0.06 # 0.075
start_pose[2] = start_pose[2] + stem_length + strawberry_radius
start_pose[0] += 0.02
start_ori = p.getQuaternionFromEuler([(-m.pi / 2), 0, 0])
# strawberry_3 = strawberry_cluster.StrawberryCluster(p, start_pose, start_ori, "models/clusters_urdf/strawberry_cluster.urdf")
strawberry_3 = strawberry_cluster.StrawberryCluster(
p, start_pose, start_ori,
"models/clusters_urdf/strawberry_multi_link.urdf")
# ## load strawberry cluster:
# start_pose = []
# start_pose = copy.deepcopy(strawberry_pose)
# stem_length = 0.15
# strawberry_radius = 0.015 + 0.06 # 0.075
# start_pose[2] = start_pose[2] + stem_length + strawberry_radius
# start_pose[0] -= 0.02
# start_ori = p.getQuaternionFromEuler([(-m.pi / 2), 0, 0])
# strawberry_4 = strawberry_cluster.StrawberryCluster(p, start_pose, start_ori, "models/clusters_urdf/strawberry_cluster.urdf")
for i in range(0, 1000):
finished = False
p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING)
if current_cluster[0] == 1:
start_pose_1 = []
start_pose_1 = copy.deepcopy(strawberry_pose)
stem_length = 0.15
strawberry_radius = 0.015 + 0.06 # 0.075
start_pose_1[
2] = start_pose_1[2] + stem_length + strawberry_radius + (
random.uniform(-1, 1) * 0.01)
start_pose_1[1] += (random.uniform(-1, 1) * 0.01)
start_pose_1[0] += (random.uniform(-1, 1) * 0.01)
r1 = (random.uniform(-10, 10) * m.pi) / 180
r2 = (random.uniform(-10, 10) * m.pi) / 180
r3 = (random.uniform(-180, 180) * m.pi) / 180
start_ori_1 = p.getQuaternionFromEuler([(-m.pi / 2) + r1, r2, r3])
strawberry_1 = strawberry_cluster.StrawberryCluster(
p, start_pose_1, start_ori_1,
"models/clusters_urdf/strawberry_cluster.urdf")
else:
start_pose_1 = [False]
start_ori_1 = [False]
# load H - LEFT strawberry:
if current_cluster[1] == 1:
start_pose_2 = []
start_pose_2 = copy.deepcopy(strawberry_pose)
stem_length = 0.15
strawberry_radius = 0.015 + 0.06 # 0.075
start_pose_2[
2] = start_pose_2[2] + stem_length + strawberry_radius + (
random.uniform(0, -2) * 0.01)
start_pose_2[1] += (random.uniform(-1, 1) * 0.01)
p.setGravity(0, 0, -9.81)
timeStep = 1. / 240.
p.setTimeStep(timeStep)
planeId = p.loadURDF("plane.urdf")
## Load franka:
start_pos = [0, 0, 0]
Joint_start_state = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.02]
panda = franka_panda_new_EE.FrankaPanda(p, start_pos, timeStep,
Joint_start_state)
## load strawberry cluster:
start_pose = [0.5, 0.5, 1.0]
start_ori = [0, 0, 1, 0]
cluster = strawberry_cluster.StrawberryCluster(
p, start_pose, start_ori, "models/clusters_urdf/cluster_1.urdf")
# start_pose = [0.53, 0.53, 1.0]
# start_ori = [0, 0, 0, 1]
# cluster2 = strawberry_cluster.StrawberryCluster(p, start_pose, start_ori, "models/strawberry_cluster.urdf")
if with_ros == True:
c, addr = s.accept()
print('Got connection from', addr)
for i in range(0, 10000):
if with_ros == True:
j1 = p.getJointState(panda.franka, 0)[0]
j2 = p.getJointState(panda.franka, 1)[0]
j3 = p.getJointState(panda.franka, 2)[0]
j4 = p.getJointState(panda.franka, 3)[0]