diff --git a/brick/List of theoretical brick poses b/brick/List of theoretical brick poses
new file mode 100644
index 0000000..e69de29
diff --git a/brick/meshes/Brick.STL b/brick/meshes/Brick.STL
new file mode 100755
index 0000000..928d130
Binary files /dev/null and b/brick/meshes/Brick.STL differ
diff --git a/brick/meshes/Brick_1.STL b/brick/meshes/Brick_1.STL
new file mode 100755
index 0000000..cfc1a6b
Binary files /dev/null and b/brick/meshes/Brick_1.STL differ
diff --git a/brick/model.config b/brick/model.config
new file mode 100755
index 0000000..9e574ea
--- /dev/null
+++ b/brick/model.config
@@ -0,0 +1,19 @@
+
+
+
+ Brick
+ 1.0
+ model-1_2.sdf
+ model-1_3.sdf
+ model-1_4.sdf
+ model.sdf
+
+
+ RON
+ r.saputra@imperial.ac.uk
+
+
+
+ Brick model
+
+
diff --git a/brick/model.sdf b/brick/model.sdf
new file mode 100755
index 0000000..ff643e2
--- /dev/null
+++ b/brick/model.sdf
@@ -0,0 +1,62 @@
+
+
+
+
+ 0 0 0 0 -0 0
+
+ 0 0 0.0 -0 0
+ 0.1
+
+ 0.0017
+ 0
+ 0
+ 0.0017
+ 0
+ 0
+
+
+
+ 0 0 0 1.5708 -0 0
+
+
+ 0.192 0.062 0.086
+
+
+
+
+
+ 100
+ 50
+ 0 0 1
+ 0.0
+ 0.0
+
+
+
+
+ 100000.000000
+ 10.00000
+ 2.000000
+ 0.0001
+
+
+
+
+
+ 0 0 0 1.5708 -0 0
+
+
+ 1 1 1
+ model://Brick/meshes/Brick_1.STL
+
+
+
+ 0.698 0.25 0.25 1
+ 0.698 0.25 0.25 1
+ 0.1 0.1 0.1 1
+ 0 0 0 0
+
+
+
+
+
diff --git a/brick/model.sdfbk b/brick/model.sdfbk
new file mode 100755
index 0000000..896b458
--- /dev/null
+++ b/brick/model.sdfbk
@@ -0,0 +1,62 @@
+
+
+
+
+ 0 0 0.033 0 -0 0
+
+ 0 -0 0.031 0 -0 0
+ 0.1
+
+ 0.0017
+ 0
+ 0
+ 0.0017
+ 0
+ 0.0017
+
+
+
+ 0 0 0 1.5708 -0 0
+
+
+ 0.192 0.062 0.086
+
+
+
+
+
+ 100
+ 50
+ 0 0 1
+ 0.0
+ 0.0
+
+
+
+
+ 100000.000000
+ 10.00000
+ 2.000000
+ 0.0001
+
+
+
+
+
+ 0 0 0 1.5708 -0 0
+
+
+ 1 1 1
+ model://Brick/meshes/Brick_1.STL
+
+
+
+ 0.698 0.25 0.25 1
+ 0.698 0.25 0.25 1
+ 0.1 0.1 0.1 1
+ 0 0 0 0
+
+
+
+
+
diff --git a/new_main.py b/new_main.py
index 30024a7..5698c70 100644
--- a/new_main.py
+++ b/new_main.py
@@ -16,6 +16,8 @@ import baxter_interface
import target_poses as tps
+import tuck_arms
+
brickstuff = tps.brick_directions_notf
class PickAndPlace(object):
@@ -146,28 +148,67 @@ class PickAndPlace(object):
# retract to clear object
self._retract()
+brick_ids = ['b1','b2','b3','b4','b5','b6','b7','b8','b9']
-print(brickstuff[1]['pose'].position.x)
+with open ("models/brick/model.sdf", "r") as brick_file:brick_sdf=brick_file.read().replace('\n', '')
+
+rospy.wait_for_service('/gazebo/spawn_sdf_model')
+spawn_sdf = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel)
+delete_model = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel)
+
+def cleanup():
+ for obj in brick_ids:
+ delete_model(obj)
+
+def spawn_brick(vertical=True):
+ brick_pose = Pose()
+ if vertical:
+ brick_pose.position.x = 0.4896
+ brick_pose.position.y = 0.7067
+ brick_pose.position.z = 0.8576
+ brick_pose.orientation.x = 0
+ brick_pose.orientation.y = 0.707
+ brick_pose.orientation.z = 0
+ brick_pose.orientation.w = 0.707
+ else:
+ pass
+
+ brick_reference_frame = 'world'
+ brick_id = brick_ids.pop()
+ spawn_sdf(brick_id, brick_sdf, "/", brick_pose, brick_reference_frame)
+
+
+tuck_arms.init_arms()
rospy.init_node("I_still_have_some_hope") # Am I wrong??
hover_distance = 0.2
left_pnp = PickAndPlace('left', hover_distance)
+
+spawn_brick(vertical=True)
left_pnp.pick(brickstuff[0]['pose'])
left_pnp.place(brickstuff[2]['pose'])
-left_pnp.pick(brickstuff[0]['pose'])
-left_pnp.place(brickstuff[3]['pose'])
-left_pnp.pick(brickstuff[0]['pose'])
-left_pnp.place(brickstuff[4]['pose'])
+# spawn_brick(vertical=True)
+# left_pnp.pick(brickstuff[0]['pose'])
+# left_pnp.place(brickstuff[3]['pose'])
+# spawn_brick(vertical=True)
+# left_pnp.pick(brickstuff[0]['pose'])
+# left_pnp.place(brickstuff[4]['pose'])
+# spawn_brick(vertical=False)
left_pnp.pick(brickstuff[1]['pose'])
-left_pnp.place(brickstuff[5]['pose'])
-left_pnp.pick(brickstuff[1]['pose'])
-left_pnp.place(brickstuff[6]['pose'])
-left_pnp.pick(brickstuff[0]['pose'])
-left_pnp.place(brickstuff[7]['pose'])
-left_pnp.pick(brickstuff[0]['pose'])
-left_pnp.place(brickstuff[8]['pose'])
-left_pnp.pick(brickstuff[1]['pose'])
-left_pnp.place(brickstuff[9]['pose'])
-left_pnp.pick(brickstuff[0]['pose'])
-left_pnp.place(brickstuff[10]['pose'])
+# left_pnp.place(brickstuff[5]['pose'])
+# spawn_brick(vertical=False)
+# left_pnp.pick(brickstuff[1]['pose'])
+# left_pnp.place(brickstuff[6]['pose'])
+# spawn_brick(vertical=True)
+# left_pnp.pick(brickstuff[0]['pose'])
+# left_pnp.place(brickstuff[7]['pose'])
+# spawn_brick(vertical=True)
+# left_pnp.pick(brickstuff[0]['pose'])
+# left_pnp.place(brickstuff[8]['pose'])
+# spawn_brick(vertical=False)
+# left_pnp.pick(brickstuff[1]['pose'])
+# left_pnp.place(brickstuff[9]['pose'])
+# spawn_brick(vertical=True)
+# left_pnp.pick(brickstuff[0]['pose'])
+# left_pnp.place(brickstuff[10]['pose'])
diff --git a/redundant/brick_data.py b/redundant/brick_data.py
index bc62940..7e949b0 100644
--- a/redundant/brick_data.py
+++ b/redundant/brick_data.py
@@ -24,6 +24,10 @@ def etq(roll, pitch, yaw):
def q_extrapolator(obj):
return obj.orientation.x, obj.orientation.y, obj.orientation.z, obj.orientation.w
+print(etq(-0.2347, 1.5707, -0.2347))
+
+exit(0)
+
##DELETE THIS
class Position():
def __init__(self):
diff --git a/tuck_arms.py b/tuck_arms.py
index f123f8c..48e932b 100755
--- a/tuck_arms.py
+++ b/tuck_arms.py
@@ -136,7 +136,7 @@ class Tuck(object):
at_goal = lambda: (abs(head.pan()) <=
baxter_interface.settings.HEAD_PAN_ANGLE_TOLERANCE)
- rospy.loginfo("Moving head to neutral position")
+ # rospy.loginfo("Moving head to neutral position")
while not at_goal() and not rospy.is_shutdown():
if start_disabled:
[pub.publish(Empty()) for pub in self._disable_pub.values()]
@@ -182,16 +182,16 @@ class Tuck(object):
# If arms are already tucked, report this to user and exit.
if all(self._arm_state['tuck'][limb] == 'tuck'
for limb in self._limbs):
- rospy.loginfo("Tucking: Arms already in 'Tucked' position.")
+ # rospy.loginfo("Tucking: Arms already in 'Tucked' position.")
self._done = True
return
else:
- rospy.loginfo("Tucking: One or more arms not Tucked.")
+ # rospy.loginfo("Tucking: One or more arms not Tucked.")
any_flipped = not all(self._arm_state['flipped'].values())
if any_flipped:
- rospy.loginfo(
- "Moving to neutral start position with collision %s.",
- "on" if any_flipped else "off")
+ # rospy.loginfo(
+ # "Moving to neutral start position with collision %s.",
+ # "on" if any_flipped else "off")
# Move to neutral pose before tucking arms to avoid damage
self._check_arm_state()
actions = dict()
@@ -203,7 +203,7 @@ class Tuck(object):
self._move_to(actions, disabled)
# Disable collision and Tuck Arms
- rospy.loginfo("Tucking: Tucking with collision avoidance off.")
+ # rospy.loginfo("Tucking: Tucking with collision avoidance off.")
actions = {'left': 'tuck', 'right': 'tuck'}
disabled = {'left': True, 'right': True}
self._move_to(actions, disabled)
@@ -214,8 +214,8 @@ class Tuck(object):
else:
# If arms are tucked disable collision and untuck arms
if any(self._arm_state['flipped'].values()):
- rospy.loginfo("Untucking: One or more arms Tucked;"
- " Disabling Collision Avoidance and untucking.")
+ # rospy.loginfo("Untucking: One or more arms Tucked;"
+ # " Disabling Collision Avoidance and untucking.")
self._check_arm_state()
suppress = deepcopy(self._arm_state['flipped'])
actions = {'left': 'untuck', 'right': 'untuck'}
@@ -224,8 +224,8 @@ class Tuck(object):
return
# If arms already untucked, move to neutral location
else:
- rospy.loginfo("Untucking: Arms already Untucked;"
- " Moving to neutral position.")
+ # rospy.loginfo("Untucking: Arms already Untucked;"
+ # " Moving to neutral position.")
self._check_arm_state()
suppress = deepcopy(self._arm_state['flipped'])
actions = {'left': 'untuck', 'right': 'untuck'}
@@ -264,3 +264,10 @@ def main():
if __name__ == "__main__":
main()
+
+def init_arms():
+ tuck = False
+ tucker = Tuck(tuck)
+ rospy.on_shutdown(tucker.clean_shutdown)
+ tucker.supervised_tuck()
+