def main(args): rospy.init_node('GummiArm', anonymous=True) r = rospy.Rate(60) gummi = Gummi() print('WARNING: Moving joints sequentially to equilibrium positions.') gummi.doGradualStartup() print('WARNING: Moving to resting pose, hold arm!') rospy.sleep(1) gummi.goRestingPose(True) for i in range(0,400): gummi.goRestingPose(False) r.sleep() #for i in range(0,100): # gummi.forearmRoll.servoTo(pi/2) # r.sleep() gummi.setCollisionResponses(False, False, False, False, False) print("GummiArm is live!") while not rospy.is_shutdown(): if gummi.teleop == 0: gummi.doUpdate() gummi.publishJointState() r.sleep()
def main(args): rospy.init_node('GummiIdentify', anonymous=True) r = rospy.Rate(60) gummi = Gummi() gummi.setMaxLoads(100, 100, 100, 100, 100) gummi.setStiffness(0.5, 0.5, 0.5, 0.5, 0.5) print('WARNING: Moving joints sequentially to equilibrium positions.') gummi.doGradualStartup() print('WARNING: Moving to resting pose, hold arm!') rospy.sleep(3) for i in range(0, 400): gummi.goRestingPose() r.sleep() print("GummiArm is live!") velocities = [0, 0, 0, 0, 0, 0, 0] start = rospy.Time.now() ask = True while not rospy.is_shutdown(): if ask: indexes = [int(x) for x in raw_input("Enter index of joint to identify (1-7): ").split()] velocities = [0, 0, 0, 0, 0, 0, 0] index = indexes[0] - 1 start = rospy.Time.now() ask = False angles = gummi.getJointAngles() angles = radToDeg(angles) duration = rospy.Time.now() - start secondsSinceStart = duration.to_sec() if secondsSinceStart > 20: ask = True else: if secondsSinceStart > 15: print("Done, moving to resting pose.") gummi.goRestingPose() else: vel = 0.0 if secondsSinceStart > 5: vel = -0.01 else: vel = 0.01 print('At angle: ' + str(angles[index]) + ' degrees, sending velocity: ' + str(vel) + '.') velocities[index] = vel gummi.setVelocity(velocities) gummi.doUpdate() r.sleep()
def main(args): rospy.init_node('GummiArm', anonymous=False) # anonymous=False because doesn't look like a good idea two GummiArm nodes... r = rospy.Rate(40) # Tried 100 and rostopic hz said it was working! # With 600 the elbow PID controller went crazy! gummi = Gummi() rospy.logwarn( 'Moving joints sequentially to startup equilibrium positions.') gummi.doGradualStartup() # gummi.testLimits() rospy.logwarn('Moving to resting pose, hold arm!') rospy.sleep(1) gummi.goRestingPose(True) for i in range(0, 400): gummi.goRestingPose(False) r.sleep() gummi.setCollisionResponses(shoulder_yaw=False, shoulder_roll=False, shoulder_pitch=False, elbow=False) rospy.loginfo("GummiArm is live!") # name = ['shoulder_yaw','shoulder_roll','shoulder_pitch','upperarm_roll','elbow','forearm_roll','wrist_pitch'] # effort = [0.5,0.5,0.5,100,0.5,100,0.5] # class Msg: # def __init__(self, name, effort): # self.name = name # self.effort = effort # msgs = [] # for i, j in zip(name, effort): # msgs.append(Msg(i, j)) # raw_input('press enter') # gummi.setCocontraction(msgs) # gummi.servoTo() while not rospy.is_shutdown(): try: rospy.has_param("/dynamixel_manager_arm/namespace") # rospy.has_param("/dynamixel_manager_arm/namespace") is a way to verify if the manager is running if gummi.teleop == 0 and gummi.velocity_control == 0: gummi.doUpdate() gummi.publishJointState() r.sleep() except: rospy.signal_shutdown( "gummi dynamixel manager seems to be dead... exiting!")
def main(args): rospy.init_node('GummiArm', anonymous=False) # anonymous=False because doesn't look like a good idea two GummiArm nodes... r = rospy.Rate(60) # Tried 100 and rostopic hz said it was working! # With 600 the elbow PID controller went crazy! gummi = Gummi() rospy.logwarn( 'Moving joints sequentially to startup equilibrium positions.') gummi.doGradualStartup() rospy.logwarn('Moving to resting pose, hold arm!') rospy.sleep(1) gummi.goRestingPose(True) for i in range(0, 400): gummi.goRestingPose(False) r.sleep() #for i in range(0,100): # gummi.forearmRoll.servoTo(pi/2) # r.sleep() gummi.setCollisionResponses(shoulder_yaw=False, shoulder_roll=False, shoulder_pitch=False, elbow=False) rospy.loginfo("GummiArm is live!") while not rospy.is_shutdown(): try: rospy.has_param("/dynamixel_manager_arm/namespace") # rospy.has_param("/dynamixel_manager_arm/namespace") is a way to verify if the manager is running if gummi.teleop == 0 and gummi.velocity_control == 0: gummi.doUpdate() gummi.publishJointState() r.sleep() except: rospy.signal_shutdown( "gummi dynamixel manager seems to be dead... exiting!")