Public Member Functions
def	__init__
def	check_cartesian_path
	check a Cartesian path for consistent, non-colliding IK solutions start_pose and end_pose are PoseStamped messages with the wrist poses start_angles are angles to try to stay close to num_steps is the number of interpolation steps to use (if 0, use pos_spacing and rot_spacing instead to calculate the number of steps) pos_spacing is max wrist translation in meters between trajectory points rot_spacing is max wrist rotation in radians between trajectory points consistent_angle is the max joint angle change before 2 steps are declared inconsistent collision_check_resolution is the resolution at which to check collisions (0 or 1 for every) steps_before_abort is the number of invalid steps found before aborting (-1 to ignore) if collision_aware is 0, ignore collisions ordered_collision_operations is an optional list of collision operations to feed to IK to modify the collision space link_padding is an optional list of link paddings to feed to IK to modify the robot collision padding IK_robot_state is an optional RobotState message to pass to IK if start_from_end is 1, find an IK solution for the end first and work backwards returns the joint angle trajectory and the error codes (0=good, 1=collisions, 2=inconsistent, 3=out of reach, 4=aborted before checking)
def	check_consistent
	check that all differences between angles1 and angles2 (lists of joint angles) are within consistent_range
def	check_services_and_get_ik_info
def	check_state_validity
	check whether a set of joint angles is in collision with the environment allows the same modifications as IK
def	draw_pose
	draw a PoseStamped in rviz as a set of arrows (x=red, y=green, z=blue) id is the id number for the x-arrow (y is id+1, z is id+2)
def	interpolate_cartesian
	interpolate a Cartesian path (expressed as pos and rot lists) pos_spacing is max wrist translation in meters between trajectory points rot_spacing is max wrist rotation in radians between trajectory points num_steps overrides the number of steps (if != 0, ignore pos_spacing and rot_spacing)
def	lists_to_pose_stamped
	convert pos and rot lists (relative to in_frame) to a poseStamped (relative to to_frame)
def	normalize_vect
	normalize a vector
def	point_stamped_to_list
	convert a pointStamped to a pos list in a desired frame
def	pose_stamped_to_lists
	convert a poseStamped to pos and rot (quaternion) lists in a desired frame
def	quat_angle
	angle between two quaternions (as lists)
def	quaternion_stamped_to_list
	convert a QuaternionStamped to a quat list in a desired frame
def	run_fk
	run forward kinematics on a set of 7 joint angles link_name specifies the desired output frame returns a PoseStamped
def	run_ik
	run inverse kinematics on a PoseStamped (7-dof pose (position + quaternion orientation) + header specifying the frame of the pose) tries to stay close to double start_angles[7] returns the solution angles as double solution[7] link_name is the link frame to position at pose if collision_aware is 1, runs ik_service_with_collision ordered_collision_operations is a list of collision operations to feed to IK to modify the collision space
def	run_query
	get the joint names and limits, and the possible link names for running IK
def	trajectory_times_and_vels
	generate appropriate times and joint velocities for a joint path (such as that output by check_cartesian_path) max_joint_vels is a list of maximum velocities to move the arm joints max_joint_accs is a list of maximum accelerations to move the arm joints (can be ignored) starts and ends in stop
def	vect_norm
	vector norm of a list
def	vector3_stamped_to_list
	convert a Vector3Stamped to a rot list in a desired frame
Public Attributes
	error_code_dict
	link_name
	marker_pub
	perception_running
	pose_id_set
	srvroot
	start_angles_list
	tf_listener
Private Attributes
	_check_state_validity_service
	_fk_service
	_ik_service
	_ik_service_with_collision
	_query_service

Detailed Description

Definition at line 60 of file ik_utilities.py.

Constructor & Destructor Documentation

def interpolated_ik_motion_planner.ik_utilities.IKUtilities.__init__	(	self,
		whicharm = `None`,
		tf_listener = `None`,
		wait_for_services = `1`
	)

Definition at line 64 of file ik_utilities.py.

Member Function Documentation

def interpolated_ik_motion_planner.ik_utilities.IKUtilities.check_cartesian_path	(	self,
		start_pose,
		end_pose,
		start_angles,
		pos_spacing = `0.01`,
		rot_spacing = `0.1`,
		consistent_angle = `math.pi/9.`,
		collision_aware = `1`,
		collision_check_resolution = `1`,
		steps_before_abort = `-1`,
		num_steps = `0`,
		ordered_collision_operations = `None`,
		start_from_end = `0`,
		IK_robot_state = `None`,
		link_padding = `None`,
		use_additional_start_angles = `0`
	)

check a Cartesian path for consistent, non-colliding IK solutions start_pose and end_pose are PoseStamped messages with the wrist poses start_angles are angles to try to stay close to num_steps is the number of interpolation steps to use (if 0, use pos_spacing and rot_spacing instead to calculate the number of steps) pos_spacing is max wrist translation in meters between trajectory points rot_spacing is max wrist rotation in radians between trajectory points consistent_angle is the max joint angle change before 2 steps are declared inconsistent collision_check_resolution is the resolution at which to check collisions (0 or 1 for every) steps_before_abort is the number of invalid steps found before aborting (-1 to ignore) if collision_aware is 0, ignore collisions ordered_collision_operations is an optional list of collision operations to feed to IK to modify the collision space link_padding is an optional list of link paddings to feed to IK to modify the robot collision padding IK_robot_state is an optional RobotState message to pass to IK if start_from_end is 1, find an IK solution for the end first and work backwards returns the joint angle trajectory and the error codes (0=good, 1=collisions, 2=inconsistent, 3=out of reach, 4=aborted before checking)

Definition at line 591 of file ik_utilities.py.

def interpolated_ik_motion_planner.ik_utilities.IKUtilities.check_consistent	(	self,
		angles1,
		angles2,
		consistent_range
	)

check that all differences between angles1 and angles2 (lists of joint angles) are within consistent_range

Definition at line 483 of file ik_utilities.py.

def interpolated_ik_motion_planner.ik_utilities.IKUtilities.check_services_and_get_ik_info ( self )

Definition at line 130 of file ik_utilities.py.

def interpolated_ik_motion_planner.ik_utilities.IKUtilities.check_state_validity	(	self,
		joint_angles,
		ordered_collision_operations = `None`,
		robot_state = `None`,
		link_padding = `None`
	)

check whether a set of joint angles is in collision with the environment allows the same modifications as IK

Definition at line 291 of file ik_utilities.py.

def interpolated_ik_motion_planner.ik_utilities.IKUtilities.draw_pose	(	self,
		pose_stamped,
		id
	)

draw a PoseStamped in rviz as a set of arrows (x=red, y=green, z=blue) id is the id number for the x-arrow (y is id+1, z is id+2)

Definition at line 151 of file ik_utilities.py.

def interpolated_ik_motion_planner.ik_utilities.IKUtilities.interpolate_cartesian	(	self,
		start_pos,
		start_rot,
		end_pos,
		end_rot,
		pos_spacing,
		rot_spacing,
		num_steps = `0`
	)

interpolate a Cartesian path (expressed as pos and rot lists) pos_spacing is max wrist translation in meters between trajectory points rot_spacing is max wrist rotation in radians between trajectory points num_steps overrides the number of steps (if != 0, ignore pos_spacing and rot_spacing)

Definition at line 448 of file ik_utilities.py.

def interpolated_ik_motion_planner.ik_utilities.IKUtilities.lists_to_pose_stamped	(	self,
		pos,
		rot,
		in_frame,
		to_frame
	)

convert pos and rot lists (relative to in_frame) to a poseStamped (relative to to_frame)

Definition at line 407 of file ik_utilities.py.

def interpolated_ik_motion_planner.ik_utilities.IKUtilities.normalize_vect	(	self,
		vect
	)

normalize a vector

Definition at line 429 of file ik_utilities.py.

def interpolated_ik_motion_planner.ik_utilities.IKUtilities.point_stamped_to_list	(	self,
		point,
		frame
	)

convert a pointStamped to a pos list in a desired frame

Definition at line 319 of file ik_utilities.py.

def interpolated_ik_motion_planner.ik_utilities.IKUtilities.pose_stamped_to_lists	(	self,
		pose,
		frame
	)

convert a poseStamped to pos and rot (quaternion) lists in a desired frame

Definition at line 382 of file ik_utilities.py.

def interpolated_ik_motion_planner.ik_utilities.IKUtilities.quat_angle	(	self,
		quat1,
		quat2
	)

angle between two quaternions (as lists)

Definition at line 434 of file ik_utilities.py.

def interpolated_ik_motion_planner.ik_utilities.IKUtilities.quaternion_stamped_to_list	(	self,
		quaternion,
		frame
	)

convert a QuaternionStamped to a quat list in a desired frame

Definition at line 361 of file ik_utilities.py.

def interpolated_ik_motion_planner.ik_utilities.IKUtilities.run_fk	(	self,
		angles,
		link_name
	)

run forward kinematics on a set of 7 joint angles link_name specifies the desired output frame returns a PoseStamped

Definition at line 213 of file ik_utilities.py.

def interpolated_ik_motion_planner.ik_utilities.IKUtilities.run_ik	(	self,
		pose_stamped,
		start_angles,
		link_name,
		collision_aware = `1`,
		ordered_collision_operations = `None`,
		IK_robot_state = `None`,
		link_padding = `None`
	)

run inverse kinematics on a PoseStamped (7-dof pose (position + quaternion orientation) + header specifying the frame of the pose) tries to stay close to double start_angles[7] returns the solution angles as double solution[7] link_name is the link frame to position at pose if collision_aware is 1, runs ik_service_with_collision ordered_collision_operations is a list of collision operations to feed to IK to modify the collision space

Definition at line 244 of file ik_utilities.py.

def interpolated_ik_motion_planner.ik_utilities.IKUtilities.run_query ( self )

get the joint names and limits, and the possible link names for running IK

Definition at line 194 of file ik_utilities.py.

def interpolated_ik_motion_planner.ik_utilities.IKUtilities.trajectory_times_and_vels	(	self,
		joint_path,
		max_joint_vels = `[.2]*7`,
		max_joint_accs = `[.5]*7`
	)

generate appropriate times and joint velocities for a joint path (such as that output by check_cartesian_path) max_joint_vels is a list of maximum velocities to move the arm joints max_joint_accs is a list of maximum accelerations to move the arm joints (can be ignored) starts and ends in stop

Definition at line 493 of file ik_utilities.py.