def _start_ros_node_and_wait_for_sim(self):
"""
Starts the ros node, waits for sim to startup by waiting to get a message
on the /joint_states channel
:return:
:rtype:
"""
print "sleeping for 5 seconds . . . "
time.sleep(5.0)
print "done sleeping"
# Get the arm state to check sim is running
rospy.init_node("iiwa_sim_test", anonymous=True)
self._robotSubscriber = JointStateSubscriber("/joint_states")
# wait for 5 seconds for robot movement service and /joint_states to come up
# with more joints than just the gripper
wait_time = 5
start_time = time.time()
while (time.time() - start_time) < wait_time:
if len(self._robotSubscriber.joint_timestamps.keys()) > 2:
break
print "Rostopic list: ",
os.system("rostopic list")
time.sleep(1)
self.assertTrue(
len(self._robotSubscriber.joint_timestamps.keys()) > 2,
"Never received full robot joint positions on /joint_states topic")
def setupSubscribers(self):
self.subscribers = dict()
s = JointStateSubscriber(topic=self.config['joint_states_topic'])
self.subscribers['joint_states'] = s
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
frame_id="iiwa_link_ee",
ee_frame_id="iiwa_link_ee")
# TODO: deal with rotations not being right
goal.gains.append(make_cartesian_gains_msg(0., 20.))
goal.force_guard.append(make_downward_force_guard_msg(25.))
return goal
# Useful scanning positions
central_view_pose = np.array([4.9, 13.7, -1.0, -95.5, -3.7, 51.4, 29.3
]) * np.pi / 180.
if __name__ == "__main__":
rospy.init_node('carrot_collector', anonymous=True)
robotSubscriber = JointStateSubscriber("/joint_states")
configBundleSubscriber = rosUtils.SimpleSubscriber(
"/carrot_configs", carrot_msgs.msg.CarrotConfigurationBundle)
configBundleSubscriber.start()
rospy.loginfo("Set up subscribers...")
rospy.sleep(1.0)
# EE CONTROL VERSION
client = actionlib.SimpleActionClient(
"plan_runner/CartesianTrajectory",
robot_msgs.msg.CartesianTrajectoryAction)
rospy.loginfo("Waiting for action servers...")
client.wait_for_server()
rospy.loginfo("Waiting for hearing full robot state...")
while len(robotSubscriber.joint_positions.keys()) < 3:
class IiwaSimulationTest(unittest.TestCase):
def setUp(self):
self.kuka_joint_names = spartan_utils.get_kuka_joint_names()
self._all_processes = []
self._terminate_all_processes()
self._launch_procman_and_start_simulator()
def tearDown(self):
self._terminate_all_processes()
pass
def _terminate_all_processes(self):
"""
Kills all processes related to procman
:return:
:rtype:
"""
print "PRE KILLING TREE"
os.system("pstree -ap")
children = self._all_processes #get_children_pids(os.getpid())
for proc in children:
proc.terminate()
proc.wait()
print "POST KILLING TREE:"
os.system("pstree -ap")
print "DONE WITH PRERUN CLEANUP"
def _launch_procman_and_start_simulator(self):
"""
Launches procman, starts the simulation
:return:
:rtype:
"""
deputy = self._launch_process_and_test(
["/usr/bin/env", "bot-procman-deputy", "--name", "localhost"])
self._all_processes.append(deputy)
sheriff = self._launch_process_and_test([
"/usr/bin/env", "bot-procman-sheriff", "--no-gui",
"--on-script-complete", "exit",
os.path.expandvars(
"${SPARTAN_SOURCE_DIR}/apps/iiwa/iiwa_hardware.pmd"),
"6.start_drake_iiwa_sim"
])
sheriff.wait()
print "Sheriff returned code %d" % (sheriff.returncode)
assert sheriff.returncode == 0, "Sheriff returned code %d" % (
sheriff.returncode)
def _launch_process_and_test(self, args):
"""
Launch a process, assert that it has started up normally
:param args: list of arguments, e.g. [executable, command line args, etc]
:type args: list of strings
:return:
:rtype:
"""
p = subprocess.Popen(args)
if p.poll() is not None:
# Process launched and terminated.
process_name = ''.join(args)
assert p.returncode == 0, "Process %s returned with nonzero code %d" % (
process_name, p.returncode)
return p
def _start_ros_node_and_wait_for_sim(self):
"""
Starts the ros node, waits for sim to startup by waiting to get a message
on the /joint_states channel
:return:
:rtype:
"""
print "sleeping for 5 seconds . . . "
time.sleep(5.0)
print "done sleeping"
# Get the arm state to check sim is running
rospy.init_node("iiwa_sim_test", anonymous=True)
self._robotSubscriber = JointStateSubscriber("/joint_states")
# wait for 5 seconds for robot movement service and /joint_states to come up
# with more joints than just the gripper
wait_time = 5
start_time = time.time()
while (time.time() - start_time) < wait_time:
if len(self._robotSubscriber.joint_timestamps.keys()) > 2:
break
print "Rostopic list: ",
os.system("rostopic list")
time.sleep(1)
self.assertTrue(
len(self._robotSubscriber.joint_timestamps.keys()) > 2,
"Never received full robot joint positions on /joint_states topic")
def setupTF(self):
self.tfBuffer = tf2_ros.Buffer()
self.tfListener = tf2_ros.TransformListener(self.tfBuffer)
def get_ee_frame_pose(self):
# check that desired position matches actual
self.setupTF()
ee_frame_name = "iiwa_link_ee"
world_frame_name = "base"
iiwa_link_ee_to_world = self.tfBuffer.lookup_transform(
world_frame_name, ee_frame_name, rospy.Time(0), rospy.Duration(1))
return iiwa_link_ee_to_world
def test_simulator_startup(self):
"""
Launches simulator, makes sure that it can ping roscore
:return:
:rtype:
"""
self._start_ros_node_and_wait_for_sim()
def test_move_arm(self):
"""
Move the arm to a given position
:return:
:rtype:
"""
self._start_ros_node_and_wait_for_sim()
# make robot service
robotService = RobotService.makeKukaRobotService()
targetPosition = [0.5] * 7
success = robotService.moveToJointPosition(targetPosition, timeout=5)
self.assertTrue(
success, msg="RobotService MoveToJointPosition returned failure ")
# check that we actually reached the target position
lastRobotJointPositions = self._robotSubscriber.get_position_vector_from_joint_names(
self.kuka_joint_names)
reached_target_position = np.linalg.norm(
np.array(targetPosition) -
np.array(lastRobotJointPositions[0:7])) < 0.1
if not reached_target_position:
print "last robot joint position", lastRobotJointPositions
self.assertTrue(reached_target_position,
"Robot didn't reach target position")
def test_ik_service(self):
"""
Test the ik service
:return:
:rtype:
"""
self._start_ros_node_and_wait_for_sim()
above_table_pre_grasp = [
0.04486168762069299, 0.3256606458812486, -0.033502080520812445,
-1.5769091802934694, 0.05899249087322813, 1.246379583616529,
0.38912999977004026
]
poseStamped = geometry_msgs.msg.PoseStamped()
pos = [0.51003723, 0.02411757, 0.30524811]
quat = [0.68763689, 0.15390449, 0.69872774, -0.12348466]
poseStamped.pose.position.x = pos[0]
poseStamped.pose.position.y = pos[1]
poseStamped.pose.position.z = pos[2]
poseStamped.pose.orientation.w = quat[0]
poseStamped.pose.orientation.x = quat[1]
poseStamped.pose.orientation.y = quat[2]
poseStamped.pose.orientation.z = quat[3]
robotService = rosUtils.RobotService.makeKukaRobotService()
response = robotService.runIK(poseStamped,
seedPose=above_table_pre_grasp,
nominalPose=above_table_pre_grasp)
print "IK solution found ", response.success
if response.success:
print "moving to joint position", response.joint_state.position
robotService.moveToJointPosition(response.joint_state.position)
self.assertTrue(response.success)
# check that desired position matches actual
self.setupTF()
ee_frame_name = "iiwa_link_ee"
world_frame_name = "base"
iiwa_link_ee_to_world = self.tfBuffer.lookup_transform(
world_frame_name, ee_frame_name, rospy.Time(0), rospy.Duration(1))
pos_actual_xyz = iiwa_link_ee_to_world.transform.translation
pos_actual = [0] * 3
pos_actual[0] = pos_actual_xyz.x
pos_actual[1] = pos_actual_xyz.y
pos_actual[2] = pos_actual_xyz.z
eps = 0.01
pos_achieved = np.linalg.norm(np.array(pos) -
np.array(pos_actual)) < eps
self.assertTrue(pos_achieved,
"Didn't achieve desired end-effector position")
def test_cartesian_space_plan(self):
"""
Test the cartesian space plan
"""
self._start_ros_node_and_wait_for_sim()
above_table_pre_grasp = [
0.04486168762069299, 0.3256606458812486, -0.033502080520812445,
-1.5769091802934694, 0.05899249087322813, 1.246379583616529,
0.38912999977004026
]
targetPosition = above_table_pre_grasp
robotService = rosUtils.RobotService.makeKukaRobotService()
success = robotService.moveToJointPosition(targetPosition, timeout=5)
self.assertTrue(
success, msg="RobotService MoveToJointPosition returned failure ")
# check that we actually reached the target position
lastRobotJointPositions = self._robotSubscriber.get_position_vector_from_joint_names(
self.kuka_joint_names)
reached_target_position = np.linalg.norm(
np.array(targetPosition) -
np.array(lastRobotJointPositions[0:7])) < 0.1
# now call the cartesian space plan service
client = actionlib.SimpleActionClient(
"plan_runner/CartesianTrajectory",
robot_msgs.msg.CartesianTrajectoryAction)
print "waiting for server"
client.wait_for_server()
print "connected to server"
goal = make_cartesian_trajectory_goal_world_frame()
goal.gains.append(make_cartesian_gains_msg())
goal.force_guard.append(make_force_guard_msg())
print "sending goal"
client.send_goal(goal)
rospy.loginfo("waiting for CartesianTrajectory action result")
client.wait_for_result()
result = client.get_result()
rospy.sleep(3) # wait for controller to settle
success = (result.status.status ==
robot_msgs.msg.PlanStatus.FINISHED_NORMALLY)
print "result:", result
self.assertTrue(success, msg="PlanStatus was not FINISHED_NORMALLY")
# check the position
# check that desired position matches actual
self.setupTF()
iiwa_link_ee_to_world = self.get_ee_frame_pose()
pos_actual = np.array(
rosUtils.pointMsgToList(
iiwa_link_ee_to_world.transform.translation))
pos_desired = np.array(
rosUtils.pointMsgToList(goal.trajectory.xyz_points[-1].point))
quat_actual = np.array(
rosUtils.quatMsgToList(iiwa_link_ee_to_world.transform.rotation))
quat_desired = np.array(
rosUtils.quatMsgToList(goal.trajectory.quaternions[0]))
pos_tol = 0.01 # within 5 cm
orientation_tol = 10 # within 5 degrees
pos_error = np.linalg.norm(pos_actual - pos_desired)
orientation_error_deg = 180 / np.pi * spartan_utils.compute_angle_between_quaternions(
quat_actual, quat_desired)
print "pos_error:\n", pos_error
print "orientation error:\n", orientation_error_deg
self.assertTrue(pos_error < pos_tol,
msg="position error was above tolerance")
self.assertTrue(orientation_error_deg < orientation_tol,
msg="orientation error was above tolerance")
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
traj_end = trajectory_msgs.msg.JointTrajectoryPoint()
traj_end.positions = q + v
traj_end.velocities = v
traj_end.time_from_start = rospy.Duration(0.2)
traj.points.append(traj_start)
traj.points.append(traj_end)
return traj
if __name__ == "__main__":
rospy.init_node('sandboxx')
robotSubscriber = JointStateSubscriber("/joint_states")
print("Waiting for full kuka state...")
while len(robotSubscriber.joint_positions.keys()) < 3:
rospy.sleep(0.1)
print("got full state")
print("Moving to start position")
above_table_pre_grasp = [
0.04486168762069299, 0.3256606458812486, -0.033502080520812445,
-1.5769091802934694, 0.05899249087322813, 1.246379583616529,
0.38912999977004026
]
targetPosition = above_table_pre_grasp
robotService = rosUtils.RobotService.makeKukaRobotService()
success = robotService.moveToJointPosition(targetPosition, timeout=5)
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
