def do_main():
rospy.init_node('simple_teleop', anonymous=True)
# setup listener for tf2s (used for ee and controller poses)
tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)
robotSubscriber = JointStateSubscriber("/joint_states")
# wait until robot state found
robotSubscriber = ros_utils.JointStateSubscriber("/joint_states")
print("Waiting for full kuka state...")
while len(robotSubscriber.joint_positions.keys()) < 3:
rospy.sleep(0.1)
print("got full state")
# init gripper
handDriver = SchunkDriver()
# init mouse manager
mouse_manager = TeleopMouseManager()
roll_goal = 0.0
yaw_goal = 0.0
pitch_goal = 0.0
# Start by moving to an above-table pregrasp pose that we know
# EE control will work well from (i.e. far from singularity)
stored_poses_dict = spartan_utils.getDictFromYamlFilename("../station_config/RLG_iiwa_1/stored_poses.yaml")
above_table_pre_grasp = stored_poses_dict["Grasping"]["above_table_pre_grasp"]
robotService = ros_utils.RobotService.makeKukaRobotService()
success = robotService.moveToJointPosition(above_table_pre_grasp, timeout=5)
print("Moved to position")
gripper_goal_pos = 0.0
handDriver.sendGripperCommand(gripper_goal_pos, speed=0.1, timeout=0.01)
print("sent close goal to gripper")
time.sleep(2)
gripper_goal_pos = 0.1
handDriver.sendGripperCommand(gripper_goal_pos, speed=0.1, timeout=0.01)
print("sent open goal to gripper")
frame_name = "iiwa_link_ee" # end effector frame name
for i in range(10):
if i == 9:
print "Couldn't find robot pose"
sys.exit(0)
try:
ee_pose_above_table = ros_utils.poseFromROSTransformMsg(
tfBuffer.lookup_transform("base", frame_name, rospy.Time()).transform)
ee_tf_above_table = tf_matrix_from_pose(ee_pose_above_table)
break
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
print("Troubling looking up robot pose...")
time.sleep(0.1)
# Then kick off task space streaming
sp = rospy.ServiceProxy('plan_runner/init_task_space_streaming',
robot_msgs.srv.StartStreamingPlan)
init = robot_msgs.srv.StartStreamingPlanRequest()
init.force_guard.append(make_force_guard_msg(20.))
res = sp(init)
print("Started task space streaming")
pub = rospy.Publisher('plan_runner/task_space_streaming_setpoint',
robot_msgs.msg.CartesianGoalPoint, queue_size=1);
def cleanup():
rospy.wait_for_service("plan_runner/stop_plan")
sp = rospy.ServiceProxy('plan_runner/stop_plan',
std_srvs.srv.Trigger)
init = std_srvs.srv.TriggerRequest()
print sp(init)
print("Done cleaning up and stopping streaming plan")
rospy.on_shutdown(cleanup)
br = tf.TransformBroadcaster()
rate = rospy.Rate(100) # max rate at which control should happen
ee_tf_last_commanded = np.zeros((4,4))
def get_initial_pose():
while not rospy.is_shutdown():
# get current tf from ros world to ee
try:
ee_pose_current = ros_utils.poseFromROSTransformMsg(
tfBuffer.lookup_transform("base", frame_name, rospy.Time()).transform)
ee_tf_last_commanded = tf_matrix_from_pose(ee_pose_current)
return ee_tf_last_commanded
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
print("Troubling looking up robot pose...")
rate.sleep()
continue
print ee_tf_last_commanded
ee_tf_last_commanded = get_initial_pose()
print ee_tf_last_commanded
sys.path.append("../imitation_tools/scripts")
from capture_imitation_data_client import start_bagging_imitation_data_client, stop_bagging_imitation_data_client
start_bagging_imitation_data_client()
pose_save_counter = 0
saved_pose_dict = dict()
saved_pose_counter = 0
try:
# control loop
while not rospy.is_shutdown():
# # get teleop mouse
events = mouse_manager.get_events()
if events["r"]:
print above_table_pre_grasp
print "that was above_table_pre_grasp"
success = robotService.moveToJointPosition(above_table_pre_grasp, timeout=3)
roll_goal = 0.0
yaw_goal = 0.0
pitch_goal = 0.0
ee_tf_last_commanded = get_initial_pose()
continue
pose_save_counter += 1
if events["o"] and pose_save_counter >= 100: # this make it not happen to much
joint_name_list = ['iiwa_joint_1', 'iiwa_joint_2', 'iiwa_joint_3', 'iiwa_joint_4', 'iiwa_joint_5', 'iiwa_joint_6', 'iiwa_joint_7']
joint_position_vector = robotSubscriber.get_position_vector_from_joint_names(joint_name_list)
print joint_position_vector
print "joint positions saved"
new_pose_name = "pose_"+str(saved_pose_counter).zfill(3)
saved_pose_counter += 1
saved_pose_dict[new_pose_name] = joint_position_vector.tolist()
pose_save_counter = 0
if events["escape"]:
stop_bagging_imitation_data_client()
if len(saved_pose_dict) > 0:
spartan_utils.saveToYaml(saved_pose_dict, "saved_poses.yaml")
sys.exit(0)
scale_down = 0.0001
delta_x = events["delta_x"]*scale_down
delta_y = events["delta_y"]*-scale_down
delta_forward = 0.0
forward_scale = 0.001
if events["w"]:
delta_forward -= forward_scale
if events["s"]:
delta_forward += forward_scale
# extract and normalize quat from tf
if events["rotate_left"]:
roll_goal += 0.01
if events["rotate_right"]:
roll_goal -= 0.01
roll_goal = np.clip(roll_goal, a_min = -0.9, a_max = 0.9)
if events["side_button_back"]:
yaw_goal += 0.01
print("side button back")
if events["side_button_forward"]:
yaw_goal -= 0.01
print("side side_button_forward")
yaw_goal = np.clip(yaw_goal, a_min = -1.314, a_max = 1.314)
if events["d"]:
pitch_goal += 0.01
if events["a"]:
pitch_goal -= 0.01
pitch_goal = np.clip(pitch_goal, a_min = -1.314, a_max = 1.314)
R = transformations.euler_matrix(pitch_goal, roll_goal, yaw_goal, 'syxz')
# third is "yaw", when in above table pre-grasp
# second is "roll", ''
# first must be "pitch"
above_table_quat_ee = transformations.quaternion_from_matrix(R.dot(ee_tf_above_table))
above_table_quat_ee = np.array(above_table_quat_ee) / np.linalg.norm(above_table_quat_ee)
# calculate controller position delta and add to start position to get target ee position
target_translation = np.asarray([delta_forward, delta_x, delta_y])
empty_matrx = np.zeros_like(ee_tf_last_commanded)
empty_matrx[:3, 3] = target_translation
ee_tf_last_commanded += empty_matrx
target_trans_ee = ee_tf_last_commanded[:3, 3]
# publish target pose as cartesian goal point
new_msg = robot_msgs.msg.CartesianGoalPoint()
new_msg.xyz_point.header.stamp = rospy.Time.now()
new_msg.xyz_point.header.frame_id = "world"
new_msg.xyz_point.point.x = target_trans_ee[0]
new_msg.xyz_point.point.y = target_trans_ee[1]
new_msg.xyz_point.point.z = target_trans_ee[2]
new_msg.xyz_d_point.x = 0.0
new_msg.xyz_d_point.y = 0.0
new_msg.xyz_d_point.z = 0.0
new_msg.quaternion.w = above_table_quat_ee[0]
new_msg.quaternion.x = above_table_quat_ee[1]
new_msg.quaternion.y = above_table_quat_ee[2]
new_msg.quaternion.z = above_table_quat_ee[3]
new_msg.roll = roll_goal
new_msg.pitch = pitch_goal
new_msg.yaw = yaw_goal
new_msg.gain = make_cartesian_gains_msg(5., 10.)
new_msg.ee_frame_id = frame_name
pub.publish(new_msg)
#gripper
if events["mouse_wheel_up"]:
gripper_goal_pos += 0.006
if events["mouse_wheel_down"]:
gripper_goal_pos -= 0.006
if gripper_goal_pos < 0:
gripper_goal_pos = 0.0
if gripper_goal_pos > 0.1:
gripper_goal_pos = 0.1
handDriver.sendGripperCommand(gripper_goal_pos, speed=0.2, stream=True)
rate.sleep()
except Exception as e:
print "Suffered exception ", e
def do_main():
rospy.init_node('gamepad_teleop', anonymous=True)
tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)
robotSubscriber = JointStateSubscriber("/joint_states")
robotSubscriber = ros_utils.JointStateSubscriber("/joint_states")
rospy.loginfo("Waiting for full kuka state...")
while len(robotSubscriber.joint_positions.keys()) < 3:
rospy.sleep(0.1)
rospy.loginfo("got full state")
rospy.loginfo("Grabbing controller...")
pygame.init()
pygame.joystick.init()
joysticks = [
pygame.joystick.Joystick(x) for x in range(pygame.joystick.get_count())
]
rospy.loginfo("Found %d controllers" % len(joysticks))
if len(joysticks) == 0:
rospy.logerr("Didn't find a controller :(.")
sys.exit(-1)
joystick = joysticks[0]
joystick.init()
rospy.loginfo("Using joystick %s" % joystick.get_name())
handDriver = SchunkDriver()
gripper_goal_pos = 0.1
# Start by moving to an above-table pregrasp pose that we know
# EE control will work well from (i.e. far from singularity)
above_table_pre_grasp = [
0.04486168762069299, 0.3256606458812486, -0.033502080520812445,
-1.5769091802934694, 0.05899249087322813, 1.246379583616529,
0.38912999977004026
]
robotService = ros_utils.RobotService.makeKukaRobotService()
success = robotService.moveToJointPosition(above_table_pre_grasp,
timeout=5)
print("Moved to position")
# Then kick off task space streaming
sp = rospy.ServiceProxy('plan_runner/init_task_space_streaming',
robot_msgs.srv.StartStreamingPlan)
init = robot_msgs.srv.StartStreamingPlanRequest()
init.force_guard.append(make_force_guard_msg(20.))
print sp(init)
print("Started task space streaming")
pub = rospy.Publisher('plan_runner/task_space_streaming_setpoint',
robot_msgs.msg.CartesianGoalPoint,
queue_size=1)
tf_ee = None
def cleanup():
rospy.wait_for_service("plan_runner/stop_plan")
sp = rospy.ServiceProxy('plan_runner/stop_plan', std_srvs.srv.Trigger)
init = std_srvs.srv.TriggerRequest()
print sp(init)
print("Done cleaning up and stopping streaming plan")
frame_name = "iiwa_link_ee"
# origin_tf, in the above EE frame
origin_tf = transformations.euler_matrix(0.0, 0., 0.)
origin_tf[0:3, 3] = np.array([0.15, 0.0, 0.0])
origin_tf_inv = np.linalg.inv(origin_tf)
rospy.on_shutdown(cleanup)
br = tf.TransformBroadcaster()
try:
last_gripper_update_time = time.time()
last_update_time = time.time()
while not rospy.is_shutdown():
#for axis_i in range(joystick.get_numaxes()):
# rospy.loginfo("Axis %d: %f" % (axis_i, joystick.get_axis(axis_i)))
#for button_i in range(joystick.get_numbuttons()):
# rospy.loginfo("Button %d: %f" % (button_i, joystick.get_button(button_i)))
#time.sleep(0.5)
# Gamepad: Logitech 310
# DPad: Axis 1 +1 is down
# Axis 0 +1 is right
# Left stick: Axis 7 +1 is down
# Axis 6 +1 is right
# Right stick: Axis 3 +1 is right
# Axis 4 +1 is down
# Left bumper: Button 4
# Right bumper: Button 5
gripper_dt = time.time() - last_gripper_update_time
dt = time.time() - last_update_time
pygame.event.pump()
if gripper_dt > 0.2:
last_gripper_update_time = time.time()
gripper_goal_pos += (joystick.get_button(5) -
joystick.get_button(4)) * dt * 0.05
gripper_goal_pos = max(min(gripper_goal_pos, 0.1), 0.0)
handDriver.sendGripperCommand(gripper_goal_pos,
speed=0.1,
timeout=0.01)
print "Gripper goal pos: ", gripper_goal_pos
br.sendTransform(
origin_tf[0:3, 3],
ro(transformations.quaternion_from_matrix(origin_tf)),
rospy.Time.now(), "origin_tf", frame_name)
try:
current_pose_ee = ros_utils.poseFromROSTransformMsg(
tfBuffer.lookup_transform("base", frame_name,
rospy.Time()).transform)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
print("Troubling looking up tf...")
rate.sleep()
continue
if dt > 0.01:
last_update_time = time.time()
if tf_ee is None:
tf_ee = tf_matrix_from_pose(current_pose_ee)
tf_ee[0, 3] += -1. * joystick.get_axis(7) * dt * 0.25
tf_ee[1, 3] += -1. * joystick.get_axis(6) * dt * 0.25
tf_ee[2, 3] += -1. * joystick.get_axis(4) * dt * 0.25
dr = -1. * joystick.get_axis(0) * dt
dp = 1. * joystick.get_axis(1) * dt
dy = -1. * joystick.get_axis(3) * dt
tf_ee = tf_ee.dot(transformations.euler_matrix(dr, 0., 0.))
tf_ee = tf_ee.dot(transformations.euler_matrix(0, dp, 0.))
tf_ee = tf_ee.dot(transformations.euler_matrix(0., 0., dy))
target_trans_ee = tf_ee[:3, 3]
target_quat_ee = transformations.quaternion_from_matrix(tf_ee)
new_msg = robot_msgs.msg.CartesianGoalPoint()
new_msg.xyz_point.header.frame_id = "world"
new_msg.xyz_point.point.x = target_trans_ee[0]
new_msg.xyz_point.point.y = target_trans_ee[1]
new_msg.xyz_point.point.z = target_trans_ee[2]
new_msg.xyz_d_point.x = 0.
new_msg.xyz_d_point.y = 0.
new_msg.xyz_d_point.z = 0.0
new_msg.quaternion.w = target_quat_ee[0]
new_msg.quaternion.x = target_quat_ee[1]
new_msg.quaternion.y = target_quat_ee[2]
new_msg.quaternion.z = target_quat_ee[3]
new_msg.gain = make_cartesian_gains_msg(5., 10.)
new_msg.ee_frame_id = frame_name
pub.publish(new_msg)
except Exception as e:
print "Suffered exception ", e
def do_main():
rospy.init_node('sandbox', anonymous=True)
tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)
robotSubscriber = JointStateSubscriber("/joint_states")
robotSubscriber = ros_utils.JointStateSubscriber("/joint_states")
print("Waiting for full kuka state...")
while len(robotSubscriber.joint_positions.keys()) < 3:
rospy.sleep(0.1)
print("got full state")
hydraSubscriber = SimpleSubscriber("/hydra_calib", razer_hydra.msg.Hydra)
hydraSubscriber.start()
print("Waiting for hydra startup...")
hydraSubscriber.waitForNextMessage()
print("Got hydra.")
handDriver = SchunkDriver()
gripper_goal_pos = 0.1
# Start by moving to an above-table pregrasp pose that we know
# EE control will work well from (i.e. far from singularity)
above_table_pre_grasp = [0.04486168762069299, 0.3256606458812486, -0.033502080520812445, -1.5769091802934694, 0.05899249087322813, 1.246379583616529, 0.38912999977004026]
robotService = ros_utils.RobotService.makeKukaRobotService()
success = robotService.moveToJointPosition(above_table_pre_grasp, timeout=5)
print("Moved to position")
# Then kick off task space streaming
sp = rospy.ServiceProxy('plan_runner/init_task_space_streaming',
robot_msgs.srv.StartStreamingPlan)
init = robot_msgs.srv.StartStreamingPlanRequest()
init.force_guard.append(make_force_guard_msg(20.))
print sp(init)
print("Started task space streaming")
pub = rospy.Publisher('plan_runner/task_space_streaming_setpoint',
robot_msgs.msg.CartesianGoalPoint, queue_size=1)
# Control of robot works like this:
# When the user is pressing no buttons on the
# wand, nothing happens.
# When the user depresses the top front trigger of
# the wand (button index 0), the current wand position
# is memorized as the origin, and any movement of the
# wand from there is executed as incremental
# movements of the end effector of the robot.
# When the user releases the top front trigger, control
# is ceased and pressing the button again re-zeros the robot.
paddle_index = 0
enable_move_button_index = 0
enable_move_button_last_state = False
start_pose_wand = None
start_tf_ee = None
#safe_space_bounds = np.array([
# [-0.1525049945545604, -0.29228035558728516, 0.13544803250537002],
# [-0.4769166944956231, -0.35229338435348867, 0.19769445898112134],
# [-0.457830238829753, 0.20935562473070765, 0.21958282208421853],
# [-0.11156388045357436, 0.19961179929244402, 0.26618401754649285],
# [-0.10225744036375771, 0.22933143089985508, 0.48444059628986263],
# [-0.14861448807866284, -0.41030619355456643, 0.4648083304072826],
# [-0.5242759491071456, -0.4147275210829423, 0.4948555294112139],
# [-0.4847194053597296, 0.27176780719677074, 0.45391525596908033],
# [-0.17358753502356636, 0.18660040610810102, 0.15775990744092278],
# [-0.45109038331994794, 0.20434001341514574, 0.09804323148032473],
# [-0.4716416007082929, -0.3620164988593509, 0.12965905105466402],
# [-0.16130783846258154, -0.3208282816424661, 0.109649432666865]])
# Reasonable inner bounding box
safe_space_lower = np.array([-0.45, -0.35, 0.125])
safe_space_upper = np.array([-0.15, 0.2, 0.45])
safe_space_violation = False
last_safe_space_complaint = time.time() - 1000.
def cleanup():
rospy.wait_for_service("plan_runner/stop_plan")
sp = rospy.ServiceProxy('plan_runner/stop_plan',
std_srvs.srv.Trigger)
init = std_srvs.srv.TriggerRequest()
print sp(init)
print("Done cleaning up and stopping streaming plan")
frame_name = "iiwa_link_ee"
# origin_tf, in the above EE frame
origin_tf = transformations.euler_matrix(0.0, 0., 0.)
origin_tf[0:3, 3] = np.array([0.15, 0.0, 0.0])
origin_tf_inv = np.linalg.inv(origin_tf)
rospy.on_shutdown(cleanup)
br = tf.TransformBroadcaster()
try:
last_gripper_update_time = time.time()
last_button_1_status = False
while not rospy.is_shutdown():
latest_hydra_msg = hydraSubscriber.waitForNextMessage(sleep_duration=0.0001)
dt = time.time() - last_gripper_update_time
if dt > 0.2:
last_gripper_update_time = time.time()
gripper_goal_pos += latest_hydra_msg.paddles[0].joy[0]*dt*0.05
gripper_goal_pos = max(min(gripper_goal_pos, 0.1), 0.0)
handDriver.sendGripperCommand(gripper_goal_pos, speed=0.1, timeout=0.01)
print "Gripper goal pos: ", gripper_goal_pos
br.sendTransform(origin_tf[0:3, 3],
ro(transformations.quaternion_from_matrix(origin_tf)),
rospy.Time.now(),
"origin_tf",
frame_name)
try:
current_pose_ee = ros_utils.poseFromROSTransformMsg(
tfBuffer.lookup_transform("base", frame_name, rospy.Time()).transform)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
print("Troubling looking up tf...")
rate.sleep()
continue
# Check if the move button is press
hydra_status = latest_hydra_msg.paddles[paddle_index]
# Difference in tf from initial TF
current_pose_wand = ros_utils.poseFromROSTransformMsg(hydra_status.transform)
if hydra_status.buttons[1] and hydra_status.buttons[1] != last_button_1_status:
print "Current wand pose: ", current_pose_wand
wand_trans = np.array(current_pose_wand[0])
if np.any(wand_trans <= safe_space_lower) or np.any(wand_trans >= safe_space_upper):
if time.time() - last_safe_space_complaint > 0.5:
print "Safe space violation: ", wand_trans
last_safe_space_complaint = time.time()
safe_space_violation = True
if hydra_status.buttons[enable_move_button_index] is False:
enable_move_button_last_state = False
safe_space_violation = False
elif safe_space_violation is False:
if enable_move_button_last_state is False:
start_pose_wand = ros_utils.poseFromROSTransformMsg(hydra_status.transform)
start_tf_wand = tf_matrix_from_pose(start_pose_wand)
start_pose_ee = current_pose_ee
start_tf_ee = tf_matrix_from_pose(start_pose_ee)
enable_move_button_last_state = True
last_button_1_status = hydra_status.buttons[1]
# These expect quat in x y z w, rather than our normal w x y z
br.sendTransform(start_pose_wand[0], ro(start_pose_wand[1]),
rospy.Time.now(),
"start_pose_wand",
"base")
br.sendTransform(current_pose_wand[0], ro(current_pose_wand[1]),
rospy.Time.now(),
"current_pose_wand",
"base")
br.sendTransform(start_pose_ee[0], ro(start_pose_ee[1]),
rospy.Time.now(),
"start_pose_ee",
"base")
hydra_tf = get_relative_tf_between_poses(start_pose_wand, current_pose_wand)
# Target TF for the EE will be its start TF plus this offset
rot_slerp_amount = 1.0
trans_slerp_amount = 1.0
tf_in_ee_frame = origin_tf.dot(hydra_tf).dot(origin_tf_inv)
br.sendTransform(tf_in_ee_frame[0:3, 3],
ro(transformations.quaternion_from_matrix(tf_in_ee_frame)),
rospy.Time.now(),
"tf_in_ee_frame",
"base")
start_tf_ee_inv = np.linalg.inv(start_tf_ee)
target_tf_ee = start_tf_ee.copy()
target_tf_ee[0:3, 3] += tf_in_ee_frame[0:3, 3] # copy position change in world frame
target_tf_ee[0:3, 0:3] = tf_in_ee_frame[0:3, 0:3].dot(target_tf_ee[0:3, 0:3])
br.sendTransform(target_tf_ee[0:3, 3],
ro(transformations.quaternion_from_matrix(target_tf_ee)),
rospy.Time.now(),
"target_tf_ee",
"base")
target_trans_ee = trans_slerp_amount*target_tf_ee[:3, 3] + (1. - trans_slerp_amount)*np.array(start_pose_ee[0])
target_quat_ee = transformations.quaternion_slerp(
np.array(start_pose_ee[1]),
transformations.quaternion_from_matrix(target_tf_ee),
rot_slerp_amount)
target_quat_ee = np.array(target_quat_ee) / np.linalg.norm(target_quat_ee)
br.sendTransform(target_trans_ee,
ro(target_quat_ee),
rospy.Time.now(),
"target_tf_ee_interp",
"base")
new_msg = robot_msgs.msg.CartesianGoalPoint()
new_msg.xyz_point.header.frame_id = "world"
new_msg.xyz_point.point.x = target_trans_ee[0]
new_msg.xyz_point.point.y = target_trans_ee[1]
new_msg.xyz_point.point.z = target_trans_ee[2]
new_msg.xyz_d_point.x = 0.
new_msg.xyz_d_point.y = 0.
new_msg.xyz_d_point.z = 0.0
new_msg.quaternion.w = target_quat_ee[0]
new_msg.quaternion.x = target_quat_ee[1]
new_msg.quaternion.y = target_quat_ee[2]
new_msg.quaternion.z = target_quat_ee[3]
new_msg.gain = make_cartesian_gains_msg(5., 10.)
new_msg.ee_frame_id = frame_name
pub.publish(new_msg)
except Exception as e:
print "Suffered exception ", e