88
99"Up" is the shoulder (J2) lifting by --lift-deg while the elbow (J3) opens by the same
1010amount, so the forearm keeps roughly its angle and the whole arm rises. Every other joint
11- holds zero. can0 must already be up at 1 Mbit/s; this script never touches the interface.
11+ holds zero, or with --hold-wrist holds the angle it started at (the PiPER rests with its
12+ wrist off zero, so this raises the arm without swinging the wrist). can0 must already be up
13+ at 1 Mbit/s; this script never touches the interface.
1214
1315Safety:
1416- Without --go nothing is sent to the arm.
1517- Refuses to start unless joint feedback is live and every joint is within --zero-tol-deg
16- of zero.
18+ of zero (only J2 and J3 with --hold-wrist) .
1719- Waits for each pose to be reached before the next; a pose that is not reached in
1820 --timeout-s aborts the cycle.
19- - Always commands zero before exiting, and disables the motors only once the arm is back
20- at zero. Disabling elsewhere would drop the arm, so then it is left enabled and holding.
21+ - Always commands the start pose before exiting, and disables the motors only once the arm
22+ is back there. Disabling elsewhere would drop the arm, so then it is left enabled and
23+ holding.
2124"""
2225import argparse
2326import os
@@ -74,6 +77,8 @@ def main():
7477 ap .add_argument ("--tol-deg" , type = float , default = 1.5 , help = "pose reached tolerance" )
7578 ap .add_argument ("--zero-tol-deg" , type = float , default = 5.0 , help = "start/end zero tolerance" )
7679 ap .add_argument ("--timeout-s" , type = float , default = 15.0 , help = "per-pose timeout" )
80+ ap .add_argument ("--hold-wrist" , action = "store_true" ,
81+ help = "hold J1 and J4-J6 at their starting angles instead of zero" )
7782 args = ap .parse_args ()
7883
7984 if not 0 < args .lift_deg <= 45 :
@@ -97,15 +102,18 @@ def main():
97102 print ("joints (deg):" , fmt (q0 ))
98103 print ("enabled:" , piper .GetArmEnableStatus ())
99104 print (piper .GetArmStatus ())
100- if any (abs (v ) > args .zero_tol_deg for v in q0 ):
105+ checked = q0 [1 :3 ] if args .hold_wrist else q0
106+ if any (abs (v ) > args .zero_tol_deg for v in checked ):
101107 sys .exit (f"arm is not within { args .zero_tol_deg } deg of zero; not moving" )
108+
109+ home = [q0 [0 ], 0.0 , 0.0 , * q0 [3 :]] if args .hold_wrist else [0.0 ] * 6
110+ up = [home [0 ], args .lift_deg , - args .lift_deg , * home [3 :]]
111+ print ("home (deg):" , fmt (home ))
112+ print ("up (deg):" , fmt (up ))
102113 if not args .go :
103114 print ("read-only run (no --go): nothing sent to the arm" )
104115 return
105116
106- zero = [0.0 ] * 6
107- up = [0.0 , args .lift_deg , - args .lift_deg , 0.0 , 0.0 , 0.0 ]
108-
109117 t0 = time .time ()
110118 while not piper .EnablePiper ():
111119 if time .time () - t0 > 5 :
@@ -121,21 +129,21 @@ def main():
121129 break
122130 time .sleep (args .pause_s )
123131 print (f"cycle { i } /{ args .cycles } : down" , flush = True )
124- if not move_to (piper , zero , args .speed , args .tol_deg , args .timeout_s ):
132+ if not move_to (piper , home , args .speed , args .tol_deg , args .timeout_s ):
125133 break
126134 time .sleep (args .pause_s )
127135 completed += 1
128136 finally :
129- print ("returning to zero " , flush = True )
130- move_to (piper , zero , args .speed , args .tol_deg , args .timeout_s )
137+ print ("returning home " , flush = True )
138+ move_to (piper , home , args .speed , args .tol_deg , args .timeout_s )
131139 q = joints_deg (piper )
132- if all (abs (v ) <= args .zero_tol_deg for v in q ):
140+ if all (abs (a - b ) <= args .zero_tol_deg for a , b in zip ( q , home ) ):
133141 t0 = time .time ()
134142 while piper .DisablePiper () and time .time () - t0 < 5 :
135143 time .sleep (0.01 )
136- print ("disabled at zero :" , fmt (q ), flush = True )
144+ print ("disabled at home :" , fmt (q ), flush = True )
137145 else :
138- print ("WARNING: not back at zero , leaving motors enabled so the arm does not drop:" ,
146+ print ("WARNING: not back home , leaving motors enabled so the arm does not drop:" ,
139147 fmt (q ), flush = True )
140148 print (f"completed { completed } /{ args .cycles } cycles" , flush = True )
141149
0 commit comments