def __init__(self, home_pos):
"""
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.home_pos = home_pos
self.control_freq = 30.0
self.latest_control = time.time()
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)
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))
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')
class ToroboEnvironment(object):
def __init__(self, home_pos):
"""
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.home_pos = home_pos
self.control_freq = 30.0
self.latest_control = time.time()
def set_position(self, positions):
self.tampy.move_to(positions)
def update(self, currents):
self.tampy.send_currents(currents)
time.sleep(max(
self.latest_control + 1.0 / self.control_freq - time.time(),
0
))
self.latest_control = time.time()
return self.get_state()
def get_state(self):
rx = self.tampy.get_latest_rx()
positions = [j.position for j in rx.joints]
velocities = [j.velocity for j in rx.joints]
return positions, velocities
def __enter__(self):
self.set_position(self.home_pos)
self.tampy.send(ORDER_RUN_MODE, value1=CTRL_MODE_CURRENT)
self.tampy.send(ORDER_SERVO_ON)
self.latest_control = time.time()
return self
def __exit__(self, type, value, tb):
self.tampy.send_currents([0] * 7)
# TODO: why do we need multiple calls to kill?
for _ in range(3):
self.tampy.send(ORDER_SERVO_OFF)
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 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()
import os
import cv2
import time
import argparse
import numpy as np
from tampy.tampy import Tampy
from os.path import expanduser, join
from torobo_forward import forward_kinematics
parser = argparse.ArgumentParser(description='teacher')
parser.add_argument('--teach', '-te', type=bool, default=0)
parser.add_argument('--publish', '-pb', type=bool, default=0)
parser.add_argument('--turn_off_currents', '-co', type=bool, default=0)
args = parser.parse_args()
tampy = Tampy()
def move_home(duration):
home = [0.00] * 7
tampy.servo_on()
each_time = 5.00
motion_time = 50.00
joint_angle = 0.0
tampy.start_joints()
tampy.log_buf(duration)
tampy.start_timer()
tampy.move_joints(home,