class PlannerArm(Arm):
"""
Planner class for the Arm.
"""
def __init__ (self, pr2, rl):
Arm.__init__(self, pr2, rl)
self.planner = IKInterpolationPlanner(self, self.lr)
def goInDirection (self, d, dist, steps=10):
"""
Moves the tool tip in the specified direction in the gripper frame.
Direction of movement -> d
f -> forward (along the tip of the gripper)
b -> backward
u -> up
d -> down
l -> left
r -> right
Distance traveled by tool tip -> dist
Number of points of linear interpolation -> steps
"""
self.pr2.update_rave()
trajectory = self.planner.goInDirection(d, dist,steps)
if trajectory:
self.follow_joint_trajectory (trajectory)
else: raise IKFail
def goInWorldDirection (self, d, dist, steps=10):
"""
Moves the tool tip in the specified direction in the base_footprint frame.
Direction of movement -> d
f -> forward
b -> backward
u -> up
d -> down
l -> left
r -> right
Distance traveled by tool tip -> dist
Number of points of linear interpolation -> steps
"""
self.pr2.update_rave()
trajectory = self.planner.goInWorldDirection(d, dist,steps)
if trajectory:
self.follow_joint_trajectory (trajectory)
else: raise IKFail
def circleAroundRadius (self, pose, rad, finAng, steps=10):
"""
Moves the gripper in a circle.
Pose of needle in hand (as given below in the
function ArmPlannerPR2.moveNeedleToGripperPose) -> pose
Radius of circle -> rad
Final angle covered by circle -> finAng
Number of points of linear interpolation of angle -> steps
"""
self.pr2.update_rave()
if pose not in [1,2,3,4]:
rospy.logwarn ("Invalid option for pose.")
return
if self.lr == 'r':
pose = {1:2, 2:1, 3:4, 4:3}[pose]
d,t = {1:(-1,-1), 2:(1,1), 3:(-1,1), 4:(1,-1)}[pose]
trajectory = self.planner.circleAroundRadius (d, t, rad, finAng, steps)
if trajectory:
self.follow_joint_trajectory (trajectory)
else: raise IKFail
def goto_pose_matrix (self, matrix4, ref_frame, targ_frame, filter_options=-1):
"""
Moves the arm such that the transform between ref_frame and
targ_frame is matrix4. Also uses filter_options as specified
in self.planner (by default).
"""
if filter_options==-1:
filter_options = self.planner.filter_options
Arm.goto_pose_matrix(self, matrix4, ref_frame, targ_frame, filter_options)
def goto_pose_matrix_rave (self, matrix4, ref_frame, targ_frame, filter_options=-1):
"""
Moves the arm in openrave only. This is only for planning. Please be very careful when
doing this and make sure to update rave properly back to the real PR2 after finishing.
"""
if filter_options==-1:
filter_options = self.planner.filter_options
joints = self.cart_to_joint(matrix4, ref_frame, targ_frame, filter_options)
if joints is not None: self.pr2.robot.SetJointValues(joints, self.manip.GetArmIndices())
else: raise IKFail
def cart_to_joint(self, matrix4, ref_frame, targ_frame, filter_options=-1):
"""
Calculates IK for given transform between ref_frame and targ_frame.
"""
if filter_options==-1:
filter_options = self.planner.filter_options
return Arm.cart_to_joint(self, matrix4, ref_frame, targ_frame, filter_options)
def __init__ (self, pr2, rl):
Arm.__init__(self, pr2, rl)
self.planner = IKInterpolationPlanner(self, self.lr)
