max/robotics
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Update

Browse Source
This commit is contained in:
Max Hunt 2020-02-13 11:48:23 +00:00
parent d6eec2a969
commit c6b70b9186
3 changed files with 24 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
bricks_etc.py
View File

@ -1,5 +1,6 @@
from math import radians from math import radians
table = { table = {
'id':'t1', 'id':'t1',
'rframe':'world', 'rframe':'world',
@ -291,16 +292,30 @@ bricks_start_v1 =[{
bricks_start_v2 =[{ bricks_start_v2 =[{
'id':'a1', 'id':'a1',
'rframe':'t1', 'rframe':'t1',
'x':-0.119, 'x':-0.319,
'y':0.134, 'y':0.134,
'z':0.821, 'z':0.821,
'roll':radians(0), 'roll':radians(0),
'pitch':radians(90), 'pitch':radians(90),
'yaw':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(): def getBuildable():
return bricks_start_v2 return supervar
def getAll(): def getAll():
return bricks_start_v2+bricks_start_v1+bricks_end+[table] return bricks_start_v2+bricks_start_v1+bricks_end+[table]
@ -313,7 +328,7 @@ def getTargets():
def getNodes(): def getNodes():
nodes = ['baxter::base', 't1::Table'] nodes = ['baxter::base', 't1::Table']
for obj in bricks_start_v2 + bricks_end: for obj in supervar + bricks_end:
name = obj['id'] name = obj['id']
node_name = name+'::Brick' node_name = name+'::Brick'
nodes.append(node_name) nodes.append(node_name)

4
main.py
View File

@ -270,8 +270,8 @@ if __name__ == "__main__":
# otc.tf_lookup('f2') # otc.tf_lookup('f2')
# print() # print()
# print() # print()
left_pick = otc.tf_lookup('a1') left_pick = otc.tf_lookup('a3')
left_pick.position.z+=0.5 left_pick.position.z+=0.5
left_pnp.pick(left_pick) left_pnp.pick(left_pick)

6
object_tf_client.py
View File

@ -19,9 +19,9 @@ def tf_lookup(object_name):
continue continue
object_angles = tf.transformations.euler_from_quaternion([rot[3] , rot[0], rot[1], rot[2]], axes='sxyz') 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) xangle = object_angles[0] + radians(90)# + radians(200.569)
xangle1 = object_angles[1] + radians(90) xangle1 = object_angles[1]# + radians(90)
xangle2 = object_angles[2] + radians(-20.569) xangle2 = object_angles[2]# + radians(-200.569)
object_angles = (xangle, xangle1, xangle2) object_angles = (xangle, xangle1, xangle2)
print("Angles:") print("Angles:")
print(xangle) print(xangle)