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!")
