diff --git a/bricks_etc.py b/bricks_etc.py index 274448d..4a5e9d3 100644 --- a/bricks_etc.py +++ b/bricks_etc.py @@ -1,5 +1,6 @@ from math import radians + table = { 'id':'t1', 'rframe':'world', @@ -291,16 +292,30 @@ bricks_start_v1 =[{ bricks_start_v2 =[{ 'id':'a1', 'rframe':'t1', - 'x':-0.119, + 'x':-0.319, 'y':0.134, 'z':0.821, 'roll':radians(0), 'pitch':radians(90), 'yaw':radians(90) - }] + }, + { + 'id':'a2', + 'rframe':'ta1', + 'x':0, + 'y':-0.15, + 'z':0, + 'roll':0, + 'pitch':0, + 'yaw':0 + } + + ] + +supervar = bricks_start_v1 def getBuildable(): - return bricks_start_v2 + return supervar def getAll(): return bricks_start_v2+bricks_start_v1+bricks_end+[table] @@ -313,7 +328,7 @@ def getTargets(): def getNodes(): nodes = ['baxter::base', 't1::Table'] - for obj in bricks_start_v2 + bricks_end: + for obj in supervar + bricks_end: name = obj['id'] node_name = name+'::Brick' nodes.append(node_name) diff --git a/main.py b/main.py index 1d41d6c..e93a9ca 100755 --- a/main.py +++ b/main.py @@ -270,8 +270,8 @@ if __name__ == "__main__": # otc.tf_lookup('f2') # print() # print() - - left_pick = otc.tf_lookup('a1') + + left_pick = otc.tf_lookup('a3') left_pick.position.z+=0.5 left_pnp.pick(left_pick) diff --git a/object_tf_client.py b/object_tf_client.py index 04ec4e1..a857c01 100755 --- a/object_tf_client.py +++ b/object_tf_client.py @@ -19,9 +19,9 @@ def tf_lookup(object_name): continue object_angles = tf.transformations.euler_from_quaternion([rot[3] , rot[0], rot[1], rot[2]], axes='sxyz') - xangle = object_angles[0] + radians(200.569)# + radians(77.878) - xangle1 = object_angles[1] + radians(90) - xangle2 = object_angles[2] + radians(-20.569) + xangle = object_angles[0] + radians(90)# + radians(200.569) + xangle1 = object_angles[1]# + radians(90) + xangle2 = object_angles[2]# + radians(-200.569) object_angles = (xangle, xangle1, xangle2) print("Angles:") print(xangle)