From 7a4fd3133ada4c033c490a2bc7192365d7bf487d Mon Sep 17 00:00:00 2001 From: Max Hunt Date: Sat, 15 Feb 2020 00:55:01 +0000 Subject: [PATCH] Update --- brick/List of theoretical brick poses | 0 brick/meshes/Brick.STL | Bin 0 -> 12084 bytes brick/meshes/Brick_1.STL | Bin 0 -> 4884 bytes brick/model.config | 19 +++++++ brick/model.sdf | 62 ++++++++++++++++++++++ brick/model.sdfbk | 62 ++++++++++++++++++++++ new_main.py | 73 ++++++++++++++++++++------ redundant/brick_data.py | 4 ++ tuck_arms.py | 29 ++++++---- 9 files changed, 222 insertions(+), 27 deletions(-) create mode 100644 brick/List of theoretical brick poses create mode 100755 brick/meshes/Brick.STL create mode 100755 brick/meshes/Brick_1.STL create mode 100755 brick/model.config create mode 100755 brick/model.sdf create mode 100755 brick/model.sdfbk 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 0000000000000000000000000000000000000000..928d130b2e210df44a6bb855d53c6cd9d02c1762 GIT binary patch literal 12084 zcmb`Nzl&W}5XawIq)2S+(kKB3HpKg1l|E?YS_UNbQzFU6t&+D`AkK8@Oc&V{;^j!bfL`X&mC#9p$_T02)yV#ID zpF3<6{UG#H^2$Ao=lXwpu79r`I;M4m4f!5p+HkLGLbTZ!BiuXb=(D}Dc_UUNH1@eQ zQ_(H|7DD4>!|8ZlbYm^_{rAhS`Z|stI5UOq!K<ZgjN`#{I9izTDKBVh>_{~jZ6ym|hdujAk8J)4bJ!^)|J^`#x8gapQmQ5#EY z!NaujXK&0sR!0wPbay`Sh>tD%_Q)%GMFQEAY>=RqJ$HNBr)C|4gam4^wJ{LX;{Mk* zm<<*mrxk_$c}2n^?X>NAyB~vB)Pjw+K7yE$z_`t;-H1(iIIUNH;lX>Cst@^#@ut=M zLeHb-MU}IJ5)zg@K9j{Q*&va%qULL_^ixPkSdO`k_Dm!}EuIp!4fwto@i65TJn2Yu zSB`e_w)Khxwf;Qyj1z6FOl#WD)tQIBn>s(tUYxZ3QOZP0I4{<;2SF{IM2Vn;1fsqNK`rcTBG~oO z`}TQKlR$lRb$z6h+Uw3v17?0AP!k>JMLq68P|Kp=^`Y)ldu0)IiSoyG(8sC|R$5I^ zLIUGdA0xzq9@aJ};k@uRt%C%$##hehb|2kwdw)LebI(a2uaZ~Xb84Z|5tA+-td&+`pLBXhODu2 zSL@TbwLyuhD+t{NHLfzzy)t4ZiOn?wJyZSbdDS_c&>l!4uRt#qx1Lx>H&#AKNa&Oh z(RxMey8B2=u>$SS^t{B15)!&CNvmB432I?C-Mh67orC&GdrseBFRh#s60p70jQR|F z9;>aQ3lEbGN;ogpkqBmk<*|F9H5u*Wz4MBM-Qk?5I%nk7Fc-C8V^-E3E2o46#-2s% zSd9?6C%SE&uC)zH5C!E`c)N#JBrNJq?3!1@T-35$@aM|o+AHjo-qqX#`EFkMShZOb zl#sAkxu$x_I6*DUv>YY$<~7WV@nT=Z0v@JJWPPCKJ62k4gAx+fiQL2XEFnQH>u|mg zZSL}eM>&iHc7rH-a?l!uVkkEZagia#;uTMD>Nl?pn)-_v#5)xcru@E*$Pz!sW zyrP7}c&tpzcTG8}8;3lQ@8g+B2?^axMDH5iX-KGrnbOHX2?>n%M{J_f+z0tlc90Uz zi=LPWR8)0CLalfN);1_XtyFg{Y3noG8J7A<=WDu=`P}W3v zkQk2@32Gr@k_}2oaBmbDecBqdjOipvHJ`G?=JM!Div%hyc}0R+=B>}9o+!Ke6t5^D zVQX>^+mo6EwJe5iqa`RI!5o`R{3L)oZX=$6QY%GlgZ)t-f-gV zV=pdAXdGI2zNm;9C7I}HBjDB62(iHWEF)v(KOMZZMG0&;(MQ1uM0ZXCE3e)~CKK%7 z9DD9}s+bphV>XZ_6+tazs1eA-IVG8pJ1GgXw; zU`2=yEu(Gc%6qhXwWXh|2IbWt$;5gcwsI$|1}j2zXjv4@n|QCa=+5<%)u5s~NHQT> zHv`{6C#(i5LUd@^{`*Yj?qgZvgw>$3WRPS+bRjGgov<2ICJuzgp=DX=&&w^tvfT-* zL1p_O$%N=aSPeR1HK>Xj2#rGv&o{jWgHY85;r)X|KQ;nh>15cA(7nW;52ugYuo~3q zGf3c-6S7eVL{}%=Kw#zR-N0(lZS<$86FOfrAsgk~MV73PB|dLxAw#ELgOQ0VO0efs zM7MpA zgvOzT=XL_3bNcV=SLO!QIcs_4#1VCB{Kuyr5A4tAN4 zjdJd?50bzR>U&tPh|<;A2Pw&fY_xrl1TwKh-Pp=;vz$|si4v>a2T7o!^qUQ=11H0r zl1!}E!9GYrwmndlK`aR?I6@t%zmJrE!U9{>=|=$nwew`J0KbSKY!ZLUd?Z6nv&3x+{62_d!ZB zA-WKFo2UrUp=G<-uWzGdiQCZoASIa)J!GO2>VqUS4lT<{w^6zpI?fIynGoIfK@u8= z7M@$3vky{|iJmSegRi4M8Ny%wMalx}>+1JGN?^l@J_<(QmZ1|a;;!!#BUbvWCx1Si zqVAR6CMd~-Y!rfhkOX#6{ljgveUOq&$VS@-Ngxx|2mAH)UCn>e+Z`pDD6!h54-Rvo zqN?AkRyX9i_19sAnOLs_r%y$Q4lTP4_)J4|=Y0*%DanNBVF#Vi4wBF~wCq;XuWzGd zi4*FBlw?A5+XqQ#99niea~q|rq2p{(k_pjmA0(l1XyJM4gOp^Vr&AvsA=2*$<4st7 z5G$X;h7)}h$_W?I)lEO*Uj3aV_Q?qLK}rxS{q=9`rV+R;>5V1wr_w@(8i7pIdz2IE dADkCCW(4~n32LFzoT&O>zw-KTgi$L-{0E1Ezv%z~ literal 0 HcmV?d00001 diff --git a/brick/meshes/Brick_1.STL b/brick/meshes/Brick_1.STL new file mode 100755 index 0000000000000000000000000000000000000000..cfc1a6b33819e30a8fc74bf637743e1eed0ab9cf GIT binary patch literal 4884 zcmb`Kv2GMW42JibG#o|Xh76RAfY{711%+pih?F3 z5(N}Q!oO#{_pNU?n}Rg4#{T@z*q+&>Y5woenWtOt^y~8Y!^x``AGhy*UA3QX-Ce@o zR=d->Vh;%SOb}ZU@v~l8)N({adRp%_4v4G~JM4+|O3dZ)eVrpTwD+*7;k!B@lxvFc zllQyYS>A0u2MO6)_e>G3-|HZ6N zJG4w)&D0GEdPscx{KU_9a8Qc zJFKU3&_e?D%?Qm{)!rn69_F?4IT0_MW~5 z7Q0Ov^f0g4TlGmV-*8un=%di5q(RRx@BUR9YLMXDh2HEn{5w_i`fF)B)UGxA%R`(r``L( zike=`l6P&qx~w}1*lAZ2L2 z^qd~%MQ5iq;tQbu$M<|@5}DWTyicfZ@UT{xAKmjl3hQaE=pg|+Rd<5WlvKSs?LN0( z-#%K=!@PC9?e9k>+(V+;wq9PJ`j`IsgVT{HtL~zmB|H~s5~aGas~w|org=gdgP!B$ zMPsEtTCH}>J2F?6slL>ia1V)UySB2IzZbs$zIZ(n)v2Phgy#ZHqD)EbYQ-p=X`YbA zV3ts;q7nUYt$6!zA3e&}EmNLE)ge42s_oidE7$mQ(VmV(b?W9U;kiJQC_RsJ>=-rj z#Zjhl!aXFaZR>eTn&z+mKkMW%z4(S(USFRk+(V+;wqAC%pR185(_3`2gy#ZH!n@gc zul1HNs&>;fHuHouJXcIi(>(0|7geSv`l0u#eqT45*J}Gz%C<~-t}5Xk64mat%X@X_ z>AlmDs7@7~B|H~s5@|{pg)_|)($ISqRS4VG^IS2?-p8obJmI-WRNJ-Hj{RJXM0M)s bEaAC8lSp&LD4c1YkcQr?s8!gut||EkzrE#; literal 0 HcmV?d00001 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() +