def setupSubscribers(self):
self.subscribers = dict()
s = SimpleSubscriber(self.config['joint_states_topic'],
sensor_msgs.msg.JointState)
s.start()
self.subscribers['joint_states'] = s
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 3 seconds . . . "
time.sleep(3.0)
print "done sleeping"
# Get the arm state to check sim is running
rospy.init_node("iiwa_sim_test", anonymous=True)
self._robotSubscriber = SimpleSubscriber("/joint_states",
sensor_msgs.msg.JointState)
self._robotSubscriber.start()
# wait for 5 seconds for robot movement service and /joint_states to come up
wait_time = 5
start_time = time.time()
while (time.time() - start_time) < wait_time:
if self._robotSubscriber.hasNewMessage:
break
time.sleep(1)
self.assertTrue(
self._robotSubscriber.hasNewMessage,
"Never received robot joint positions on /joint_states topic")
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
class IiwaSimulationTest(unittest.TestCase):
def setUp(self):
self._kill_all_procman_processes()
self._all_processes = []
self._launch_procman_and_start_simulator()
def tearDown(self):
self._kill_all_processes()
self._kill_all_procman_processes()
pass
def _kill_all_procman_processes(self):
"""
Kills all processes related to procman
:return:
:rtype:
"""
cmd = ["pkill", "-f", "procman"]
process = subprocess.call(cmd)
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"])
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)
# print "sleeping for 3 seconds . . . "
# time.sleep(3.0)
# print "done sleeping"
#
# # Get the arm state to check sim is running
# rospy.init_node("iiwa_sim_test", anonymous=True)
# self._robotSubscriber = SimpleSubscriber("/joint_states", sensor_msgs.msg.JointState)
# self._robotSubscriber.start()
#
# # wait for 5 seconds for robot movement service and /joint_states to come up
# wait_time = 5
# start_time = time.time()
# while (time.time() - start_time) < wait_time:
# if self._robotSubscriber.hasNewMessage:
# break
# time.sleep(1)
#
# assert self._robotSubscriber.hasNewMessage, "Never received robot joint positions on /joint_states topic"
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)
self._all_processes.append(p)
return p
def _kill_all_processes(self, additional_processes=None):
"""
Kill all processes in self._all_processes
Also kill any additional processes that may have been passed in
:param additional_processes:
:type additional_processes:
:return:
:rtype:
"""
for p in self._all_processes:
if p.poll() is None:
p.kill()
if additional_processes is None:
additional_processes = []
for p in additional_processes:
if p.poll() is None:
p.kill()
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 3 seconds . . . "
time.sleep(3.0)
print "done sleeping"
# Get the arm state to check sim is running
rospy.init_node("iiwa_sim_test", anonymous=True)
self._robotSubscriber = SimpleSubscriber("/joint_states",
sensor_msgs.msg.JointState)
self._robotSubscriber.start()
# wait for 5 seconds for robot movement service and /joint_states to come up
wait_time = 5
start_time = time.time()
while (time.time() - start_time) < wait_time:
if self._robotSubscriber.hasNewMessage:
break
time.sleep(1)
self.assertTrue(
self._robotSubscriber.hasNewMessage,
"Never received robot joint positions on /joint_states topic")
def setupTF(self):
self.tfBuffer = tf2_ros.Buffer()
self.tfListener = tf2_ros.TransformListener(self.tfBuffer)
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.lastMsg.position
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")
class IiwaSimulationTest(unittest.TestCase):
def setUp(self):
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 = SimpleSubscriber("/joint_states", sensor_msgs.msg.JointState)
self._robotSubscriber.start()
# wait for 5 seconds for robot movement service and /joint_states to come up
wait_time = 5
start_time = time.time()
while (time.time() - start_time) < wait_time:
if self._robotSubscriber.hasNewMessage:
break
print "Rostopic list: ",
os.system("rostopic list")
time.sleep(1)
self.assertTrue(self._robotSubscriber.hasNewMessage, "Never received 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.lastMsg.position
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.lastMsg.position
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 try_to_move_arm_and_hand():
import rospy
import rosgraph
import socket
import numpy as np
import time
try:
rosgraph.Master('/rostopic').getPid()
except socket.error:
print "Unable to communicate with ROS master, even though it should be up!"
cleanup_and_exit(1)
rospy.init_node("test_full_simulation_stack", anonymous=True)
# Get the arm state
from spartan.utils.ros_utils import SimpleSubscriber
robotSubscriber = SimpleSubscriber("/joint_states",
sensor_msgs.msg.JointState)
robotSubscriber.start()
# Move the arm
from spartan.utils.ros_utils import RobotService
robotService = RobotService.makeKukaRobotService()
targetPosition = [0.5] * 7
success = robotService.moveToJointPosition(targetPosition, timeout=5)
if not success:
print "RobotService MoveToJointPosition returned failure ", success
rospy.sleep(0.1)
if robotSubscriber.hasNewMessage is False:
print "We never received robot joint positions... what gives."
cleanup_and_exit(1)
lastRobotJointPositions = robotSubscriber.lastMsg.position
if np.linalg.norm(
np.array(targetPosition) -
np.array(lastRobotJointPositions[0:7])) > 0.1:
print "Did not reach target position: ", targetPosition, " vs current pos ", lastRobotJointPositions
cleanup_and_exit(1)
# Test IK utilities, which go into pydrake
import geometry_msgs
import robot_msgs.srv
poseStamped = geometry_msgs.msg.PoseStamped()
poseStamped.pose.position.x = 0.5
poseStamped.pose.position.y = 0.0
poseStamped.pose.position.z = 1.5
poseStamped.pose.orientation.w = 1.0
poseStamped.pose.orientation.x = 0.0
poseStamped.pose.orientation.y = 0.0
poseStamped.pose.orientation.z = 0.0
srvRequest = robot_msgs.srv.RunIKRequest()
srvRequest.pose_stamped = poseStamped
success = robotService.moveToCartesianPosition(srvRequest, timeout=5)
if not success:
print "robotService moveToCartesianPosition returned failure ", success
cleanup_and_exit(1)
# Close the gripper
from spartan.manipulation.schunk_driver import SchunkDriver
schunkDriver = SchunkDriver()
rospy.sleep(1.0)
if schunkDriver.lastStatusMsg is None:
print "Got no starting gripper position. Is sim running?"
cleanup_and_exit(1)
startingGripperPosition = schunkDriver.lastStatusMsg.position_mm
schunkDriver.sendCloseGripperCommand()
rospy.sleep(1.0)
if schunkDriver.lastStatusMsg.position_mm >= 10.0:
print "After trying to close gripper, gripper position was %f (started at %f)." % (
schunkDriver.lastStatusMsg.position_mm, startingGripperPosition)
print "sendCloseGripperCommand appears ineffectual"
cleanup_and_exit(1)