def move_teach(duration):
tampy = Tampy()
# original
Pipe = [-1.24, -8.16, -4.30, 68.21, -17.01, 59.76, 0.03]
Trumpet = [-0.46, -19.55, 14.36, 78.28, 2.73, 60.06, 0.01]
# mod to min singularity of joint 6
Pipe = [-1.24, -8.16, -4.30, 68.21, -17.01, 39.76, 0.03]
Trumpet = [-0.46, -19.55, 14.36, 78.28, 2.73, 30.06, 0.01]
tampy.servo_on()
each_time = 5.00
motion_time = 10.00
joint_angle = 0.0
tampy.start_joints()
tampy.log_buf(duration)
tampy.start_timer()
tampy.move_joints(Pipe,
each_time=each_time,
motion_time=motion_time,
duration=duration)
move_reset()
def reset_error():
try:
tampy = Tampy()
except ToroboException as e:
while True:
if fix_joint_angle_limit(e.tampy):
tampy = e.tampy
break
tampy.camera_launch()
tampy.servo_on()
tampy.start_joints()
tampy.log_buf(duration=5.0)
tampy.start_timer()
tampy.move_joints([0.0] * 7, each_time=5.0, duration=5.0, motion_time=5.0)
tampy.servo_off()
print('done')
def turn_off_currents(duration):
tampy = Tampy()
# tampy.camera_launch()
tampy.servo_on()
tampy.start_currents()
tampy.start_timer()
tampy.log_buf(duration)
motion_time = 700.00
joint_angle = 0.0
for id in range(7):
tampy.start_currents(joint_id=id)
tampy.move_currents([0.0] * 7,
joint_id=id,
duration=duration,
motion_time=duration)
tampy.servo_off()
class ToroboExecutor(object):
def __init__(self, home_pos, urdf):
"""
in update(self, currents) currents are inindividual currents to each joint
According to Ryo-san, the max current is 0.5A and you should start with 0.05A or sth and gradually increase
"""
self.tampy = Tampy()
self.tampy.servo_on()
self.urdf = urdf
self.home_pos = home_pos
self.control_freq = 30.0
self.latest_control = time.time()
self.carts_to_send = rospy.ServiceProxy("/torobo_ik/solve_diff_ik",
SolveDiffIK)
def send_carts(self, msg):
rospy.wait_for_service("/torobo_ik/solve_diff_ik")
try:
resp = self.carts_to_send(msg)
return SolveDiffIKResponse(resp.q_out)
except rospy.ServiceException, e:
print("Service call failed: %s" % (e))