def init_orient_constraints(x_tol, y_tol, z_tol):
if x_tol < 0.1:
x_tol = 0.1
if y_tol < 0.1:
y_tol = 0.1
if z_tol < 0.1:
z_tol = 0.1
print x_tol, y_tol, z_tol
cur_pose = manipulator.get_current_pose().pose
upright_constraints = Constraints()
upright_constraints.name = "upright"
orientation_constraint = OrientationConstraint()
# orientation_constraint.header = pose.header
orientation_constraint.header.frame_id = "world"
orientation_constraint.link_name = manipulator.get_end_effector_link()
print "end link: ", manipulator.get_end_effector_link()
orientation_constraint.orientation = cur_pose.orientation
#IIWA
orientation_constraint.absolute_x_axis_tolerance = x_tol
orientation_constraint.absolute_y_axis_tolerance = y_tol
orientation_constraint.absolute_z_axis_tolerance = z_tol
orientation_constraint.weight = 1.0
upright_constraints.orientation_constraints.append(orientation_constraint)
return upright_constraints
def set_pose_quat_target(self, pose):
############################
rospy.logwarn("set_pose_quat_target")
# Create a contraints list and give it a name
constraints = Constraints()
constraints.name = "Keep gripper horizontal"
start_pose = self.arm.get_current_pose(self.end_effector_link)
rospy.logwarn(self.end_effector_link)
rospy.logwarn("start_pose:")
rospy.logwarn(start_pose)
rospy.logwarn("pose:")
rospy.logwarn(pose)
# Create an orientation constraint for the right gripper
orientation_constraint = OrientationConstraint()
orientation_constraint.header = start_pose.header
orientation_constraint.link_name = self.end_effector_link
orientation_constraint.orientation.x = start_pose.pose.orientation.x
orientation_constraint.orientation.y = start_pose.pose.orientation.y
orientation_constraint.orientation.z = start_pose.pose.orientation.z
orientation_constraint.orientation.w = start_pose.pose.orientation.w
orientation_constraint.absolute_x_axis_tolerance = 0.1
orientation_constraint.absolute_y_axis_tolerance = 0.1
orientation_constraint.absolute_z_axis_tolerance = 3.14
orientation_constraint.weight = 1.0
# Append the constraint to the list of contraints
constraints.orientation_constraints.append(orientation_constraint)
rospy.logwarn("constraints:")
rospy.logwarn(constraints)
# Set the path constraints on the right_arm
self.arm.set_path_constraints(constraints)
############################
self.arm.set_pose_target(pose, self.end_effector_link)
def plan_motion_constrained(commander, goal_pose):
#Define Pose
goal = PoseStamped()
goal.header.frame_id = "base"
goal.pose.position.x = goal_pose[0]
goal.pose.position.y = goal_pose[1]
goal.pose.position.z = goal_pose[2]
goal.pose.orientation.x = goal_pose[3]
goal.pose.orientation.y = goal_pose[4]
goal.pose.orientation.z = goal_pose[5]
goal.pose.orientation.w = goal_pose[6]
commander.set_pose_target(goal)
commander.set_start_state_to_current_state()
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper";
orien_const.header.frame_id = "base";
orien_const.orientation.y = -1.0;
orien_const.absolute_x_axis_tolerance = 0.1;
orien_const.absolute_y_axis_tolerance = 0.1;
orien_const.absolute_z_axis_tolerance = 0.1;
orien_const.weight = 1.0;
consts = Constraints()
consts.orientation_constraints = [orien_const]
commander.set_path_constraints(consts)
#Plan a path
plan = commander.plan()
#Return plan
return plan
def __init__(self, program_file=PROGRAM_FILE):
# TODO: Either implement behavior that fixes programs when markers change
# or only let this callback run once
self._markers_sub = rospy.Subscriber(SUB_NAME,
Marker,
callback=self._markers_callback)
self._curr_markers = None
self._tf_listener = tf.TransformListener()
self._arm = fetch_api.Arm()
self._gripper = fetch_api.Gripper()
self._torso = fetch_api.Torso()
self._controller_client = actionlib.SimpleActionClient(
'/query_controller_states', QueryControllerStatesAction)
self._program_file = program_file
self._programs = self._read_in_programs()
self._joint_reader = JointStateReader()
mat = tft.identity_matrix()
mat[:, 0] = np.array([0, 0, -1, 0])
mat[:, 2] = np.array([1, 0, 0, 0])
o = tft.quaternion_from_matrix(mat)
self._constraint_pose = Pose(orientation=Quaternion(*o))
oc = OrientationConstraint()
oc.header.frame_id = 'base_link'
oc.link_name = 'gripper_link'
oc.orientation = self._constraint_pose.orientation
oc.weight = 1.0
oc.absolute_z_axis_tolerance = 1.0
oc.absolute_x_axis_tolerance = 1.0
oc.absolute_y_axis_tolerance = 1.0
self._constraint = None
def append_pose_to_move_group_goal(goal_to_append=None, goal_pose=Pose(), link_name=None):
""" Appends a pose to the given move_group goal, returns it appended
Goals for now are for the same link TODO: let it be for different links"""
if goal_to_append == None:
rospy.logerr("append_pose_to_move_group_goal needs a goal!")
return
goal_to_append = MoveGroupGoal()
goal_c = Constraints()
position_c = PositionConstraint()
position_c.header = goal_to_append.request.goal_constraints[0].header
position_c.link_name = goal_to_append.request.goal_constraints[0].link_name
position_c.constraint_region.primitives.append(SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01]))
position_c.constraint_region.primitive_poses.append(goal_pose)
position_c.weight = 1.0
goal_c.position_constraints.append(position_c)
orientation_c = OrientationConstraint()
orientation_c.header = goal_to_append.request.goal_constraints[0].header
orientation_c.link_name = goal_to_append.request.goal_constraints[0].link_name
orientation_c.orientation = goal_pose.orientation
orientation_c.absolute_x_axis_tolerance = 0.01
orientation_c.absolute_y_axis_tolerance = 0.01
orientation_c.absolute_z_axis_tolerance = 0.01
orientation_c.weight = 1.0
goal_c.orientation_constraints.append(orientation_c)
goal_to_append.request.goal_constraints.append(goal_c)
return goal_to_append
def init_upright_path_constraints(): cur_pose = manipulator.get_current_pose().pose upright_constraints = Constraints() upright_constraints.name = "upright" orientation_constraint = OrientationConstraint() # orientation_constraint.header = pose.header orientation_constraint.header.frame_id = "world" orientation_constraint.link_name = manipulator.get_end_effector_link() print "end link: ", manipulator.get_end_effector_link() orientation_constraint.orientation = cur_pose.orientation #IIWA orientation_constraint.absolute_x_axis_tolerance = 0.1 orientation_constraint.absolute_y_axis_tolerance = 0.1 orientation_constraint.absolute_z_axis_tolerance = 3.14 #UR5 # orientation_constraint.absolute_x_axis_tolerance = 3.14 # orientation_constraint.absolute_y_axis_tolerance = 0.1 # orientation_constraint.absolute_z_axis_tolerance = 0.1 orientation_constraint.weight = 1.0 upright_constraints.orientation_constraints.append(orientation_constraint) return upright_constraints
def set_orientation_goal(self,
quaternion,
tolerance=0.001,
weight=1.0,
frame=None):
"""
Set goal orientation of `frame`. Defaults to the end-effector `frame`.
"""
if frame == None:
frame = self.arm_end_effector
orientation_constraint = OrientationConstraint()
orientation_constraint.header.frame_id = self.arm_base_link
orientation_constraint.link_name = frame
orientation_constraint.orientation.x = quaternion[0]
orientation_constraint.orientation.y = quaternion[1]
orientation_constraint.orientation.z = quaternion[2]
orientation_constraint.orientation.w = quaternion[3]
orientation_constraint.absolute_x_axis_tolerance = tolerance
orientation_constraint.absolute_y_axis_tolerance = tolerance
orientation_constraint.absolute_z_axis_tolerance = tolerance
orientation_constraint.weight = weight
(self.kinematic_path_request.motion_plan_request.goal_constraints[-1].
orientation_constraints.append(orientation_constraint))
def moveAndPour(planner, fill_pos, const=[]):
orientation = getQuaternion(np.array([0, 1, 0]), np.pi / 2)
o = np.array([
orientation[1][0], orientation[1][1], orientation[1][2], orientation[0]
])
position = fill_pos + DESIRED_RELATIVE_POSITION
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper"
orien_const.header.frame_id = "base"
orien_const.orientation.x = o[0]
orien_const.orientation.y = o[1]
orien_const.orientation.z = o[2]
orien_const.orientation.w = o[3]
orien_const.absolute_x_axis_tolerance = 0.1
orien_const.absolute_y_axis_tolerance = 0.1
orien_const.absolute_z_axis_tolerance = 0.1
# orien_const.weight = 1.0;
executePositions(planner, [position], [o], [orien_const])
orients = []
for i in range(1, 11):
theta = -0.2 * i
rot = getQuaternion(np.array([1, 0, 0]), theta)
o_r = quatMult(rot, orientation)
o_r = o_r[1][0], o_r[1][1], o_r[1][2], o_r[0]
orients.append(o_r)
executePositions(planner, [position for _ in range(0, 10)], orients)
def move_ik(self, xyz):
move_group = self.arm_group
fixed_end_effector = OrientationConstraint()
fixed_end_effector.link_name = "end_effector_link"
fixed_end_effector.header.frame_id = "link1"
fixed_end_effector.orientation.w = 1.0
fixed_end_effector.absolute_x_axis_tolerance = 0.1
fixed_end_effector.absolute_y_axis_tolerance = 0.1
fixed_end_effector.absolute_z_axis_tolerance = 3.14
fixed_end_effector.weight = 1.0
constraint = Constraints()
constraint.name = "tilt constraint"
constraint.orientation_constraints = [fixed_end_effector]
move_group.set_path_constraints(constraint)
move_group.set_position_target(xyz)
try:
move_group.go(wait=True)
except MoveItCommanderException:
print "sorry cant move here!"
# move_group.stop()
self.moving = False
def move(self, num):
moveit_commander.roscpp_initialize(sys.argv)
# rospy.init_node("moveit_node")
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander('right_arm')
right_arm = baxter_interface.Limb("right")
right_arm_pose = right_arm.endpoint_pose()
# print(right_arm_pose)
initial_orient = [
right_arm_pose['orientation'].x, right_arm_pose['orientation'].y,
right_arm_pose['orientation'].z, right_arm_pose['orientation'].w
]
dig_class = dg.Digits()
digit_shapes = dig_class.digit
num_shape = digit_shapes[str(num)]
i = 0
# print(num_shape, num, len(num_shape))
while i < len(num_shape):
(x, y) = num_shape[i]
pose_target = PoseStamped()
pose_target.header.frame_id = "base"
# print(pose_target.pose.position)
right_arm_pose = right_arm.endpoint_pose()
x = right_arm_pose['position'].x
y = right_arm_pose['position'].y
z = right_arm_pose['position'].z
print(right_arm_pose)
pose_target.pose.position.x = x + num_shape[i][0] * 0.1
pose_target.pose.position.y = y + num_shape[i][1] * 0.1
pose_target.pose.position.z = z
pose_target.pose.orientation.x = initial_orient[0]
pose_target.pose.orientation.y = initial_orient[1]
pose_target.pose.orientation.z = initial_orient[2]
pose_target.pose.orientation.w = initial_orient[3]
group.set_pose_target(pose_target)
group.set_start_state_to_current_state()
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper"
orien_const.header.frame_id = "base"
orien_const.orientation.x = initial_orient[0]
orien_const.orientation.y = initial_orient[1]
orien_const.orientation.z = initial_orient[2]
orien_const.orientation.w = initial_orient[3]
orien_const.absolute_x_axis_tolerance = 0.1
orien_const.absolute_y_axis_tolerance = 0.1
orien_const.absolute_z_axis_tolerance = 0.1
orien_const.weight = 1.0
consts = Constraints()
consts.orientation_constraints = [orien_const]
group.set_path_constraints(consts)
plan = group.plan()
group.execute(plan)
i += 1
def cartesian2Point(self,
points,
eAngles,
ax='sxyz',
resolution=0.01,
jumpStep=0,
maxVelocityFactor=1,
end_effector='link_6'):
"""
Moves to a point in a straight line using end effector point space.\n
Usage:\n
- points - list of lists that contain x,y,z coordinates\n
- eAngles - list that contains a,b,g euler angles for constraint
- ax - specify convention to use for euler angles
- default: 'sxyz'
- resolution - maximum distance between 2 generated points
- default: 0.01
- jumpStep - maximum jump distance between 2 generated points
- default: 0
- maxVelocityFactor - scaling factor for reducing joint velocity (0,1]
- end_effector - link whose point you want to move
- default link_6
"""
print "Number of points recieved: ", len(points)
wpose = Pose()
self.manip.set_end_effector_link(end_effector)
self.manip.set_max_velocity_scaling_factor(maxVelocityFactor)
constraint = Constraints()
orientation_constraint = OrientationConstraint()
orientation_constraint.link_name = end_effector
orientation_constraint.orientation = Quaternion(
*qEuler(eAngles[0], eAngles[1], eAngles[2], ax))
constraint.orientation_constraints.append(orientation_constraint)
self.manip.set_path_constraints(constraint)
t1 = t.time()
for pt in points:
if not rp.is_shutdown():
print "Going to point: ", [round(p, 5) for p in pt]
wpose.position.x = pt[0]
wpose.position.y = pt[1]
wpose.position.z = pt[2]
(plan, factor) = self.manip.compute_cartesian_path([wpose],
resolution,
jumpStep)
trajLen = len(plan.joint_trajectory.points)
if trajLen <= 100 and factor == 1.0:
self.manip.execute(plan)
else:
rp.loginfo(
"Error while planning. No execution attempted.\nNo. points planned:{}\nPercentage of path found:{}%"
.format(trajLen, round(factor * 100, 2)))
rp.loginfo("Moving to multiple points finished.")
self.__displayDuration(t1, t.time())
self.manip.set_path_constraints(None)
def get_ik(target):
"""
:param target: a PoseStamped give the desired position of the endeffector.
"""
pose = group.get_current_pose(group.get_end_effector_link())
constraints = Constraints()
orientation_constraint = OrientationConstraint()
orientation_constraint.header = pose.header
orientation_constraint.link_name = group.get_end_effector_link()
orientation_constraint.orientation = pose.pose.orientation
orientation_constraint.absolute_x_axis_tolerance = 0.1
orientation_constraint.absolute_y_axis_tolerance = 0.1
orientation_constraint.absolute_z_axis_tolerance = 2*pi
current_orientation_list = [pose.pose.orientation.x,
pose.pose.orientation.y,
pose.pose.orientation.z,
pose.pose.orientation.w]
# get euler angle from quaternion
(roll, pitch, yaw) = euler_from_quaternion(current_orientation_list)
pitch = pi
roll = 0
orientation_constraint.weight = 1
[orientation_constraint.orientation.x, orientation_constraint.orientation.y, orientation_constraint.orientation.z, orientation_constraint.orientation.w] = \
quaternion_from_euler(roll, pitch, yaw)
constraints.orientation_constraints.append(orientation_constraint)
# group.set_path_constraints(constraints)
#####################################################################
rospy.wait_for_service('compute_ik')
request_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
service_request = PositionIKRequest()
service_request.group_name = 'robot'
service_request.pose_stamped.header.frame_id = 'base_footprint'
# service_request.pose_stamped = group.get_current_pose()
service_request.robot_state = robot.get_current_state()
service_request.ik_link_name = 'arm_link_5'
# Set position
service_request.pose_stamped.pose.position.x = target[0]
service_request.pose_stamped.pose.position.y = target[1]
service_request.pose_stamped.pose.position.z = target[2]
service_request.pose_stamped.pose.orientation.w =1
service_request.constraints.orientation_constraints.append(orientation_constraint)
service_request.timeout.secs= 4
service_request.attempts= 2
service_request.avoid_collisions = True
resp = request_ik(service_request)
return resp
def staticDip(self, z_pose=0.1, tolerance=0.075):
group = self.group
current_pose = group.get_current_pose().pose
position_constraint = PositionConstraint()
position_constraint.target_point_offset.x = 0.1
position_constraint.target_point_offset.y = 0.1
position_constraint.target_point_offset.z = 0.5
position_constraint.weight = 0.25
position_constraint.link_name = group.get_end_effector_link()
position_constraint.header.frame_id = group.get_planning_frame()
orientation_constraint = OrientationConstraint()
orientation_constraint.orientation = Quaternion(x=self.q[0],
y=self.q[1],
z=self.q[2],
w=self.q[3])
orientation_constraint.absolute_x_axis_tolerance = 0.3
orientation_constraint.absolute_y_axis_tolerance = 0.3
orientation_constraint.absolute_z_axis_tolerance = 0.3
orientation_constraint.weight = 0.5
orientation_constraint.link_name = group.get_end_effector_link()
orientation_constraint.header.frame_id = group.get_planning_frame()
constraint = Constraints()
constraint.orientation_constraints.append(orientation_constraint)
constraint.position_constraints.append(position_constraint)
group.set_path_constraints(constraint)
allow_replanning = False
planning_time = 12
before_dip = current_pose.position.z
# dip = False
# while not dip:
dip = self.go_to_pose_goal(
current_pose.orientation.x,
current_pose.orientation.y,
current_pose.orientation.z,
current_pose.orientation.w,
self.target_location_x, # accounting for tolerance error
self.target_location_y, # accounting for tolerance error
z_pose, # This is where we dip
allow_replanning,
planning_time,
tolerance / 3)
# current_pose = group.get_current_pose().pose
rospy.sleep(0.01)
group.clear_path_constraints()
after_dip = group.get_current_pose().pose.position.z
if dip and (before_dip > after_dip):
# print "\nBefore dip z pose: ",before_dip
# print "\nAfter dip z pose: ",after_dip
# print "\nSuccessfully dipped!"
return True
else:
self.staticDip()
def _to_ori_constraint(pose, reference_frame, link_name, orientation_tolerance=_DEFAULT_ORIENTATION_TOLERANCE):
"""Returns an orientation constraint suitable for ActionGoal's."""
ori_con = OrientationConstraint()
ori_con.header.frame_id = reference_frame
ori_con.link_name = link_name
ori_con.orientation = pose.orientation
ori_con.absolute_x_axis_tolerance = orientation_tolerance
ori_con.absolute_y_axis_tolerance = orientation_tolerance
ori_con.absolute_z_axis_tolerance = orientation_tolerance
ori_con.weight = 1
return ori_con
def mover_setup(self):
limb = Limb("right")
# Right arm planner
self.planner = PathPlanner("right_arm")
# Left arm planner
self.planner_left = PathPlanner("left_arm")
place_holder = {'images': [], 'camera_infos': []}
#Create the function used to call the service
compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
count = 0
print(limb.endpoint_pose())
calibration_points = []
if ROBOT == "sawyer":
Kp = 0.2 * np.array([0.4, 2, 1.7, 1.5, 2, 2, 3])
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.8, 0.8, 0.8])
Ki = 0.01 * np.array([1.4, 1.4, 1.4, 1, 0.6, 0.6, 0.6])
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9])
else:
Kp = 0.45 * np.array([0.8, 2.5, 1.7, 2.2, 2.4, 3, 4])
Kd = 0.015 * np.array([2, 1, 2, 0.5, 0.8, 0.8, 0.8])
Ki = 0.01 * np.array([1.4, 1.4, 1.4, 1, 0.6, 0.6, 0.6])
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9])
# Controller for right arm
self.c = Controller(Kp, Ki, Kd, Kw, Limb('right'))
self.orien_const = OrientationConstraint()
self.orien_const.link_name = "right_gripper";
self.orien_const.header.frame_id = "base";
self.orien_const.orientation.y = -1.0;
self.orien_const.absolute_x_axis_tolerance = 0.1;
self.orien_const.absolute_y_axis_tolerance = 0.1;
self.orien_const.absolute_z_axis_tolerance = 0.1;
self.orien_const.weight = 1.0;
box = PoseStamped()
box.header.frame_id = "base"
box.pose.position.x = self.box.pose.position.x
box.pose.position.y = self.box.pose.position.y
# box.pose.position.z = self.box.pose.position.z
box.pose.position.z = self.conveyor_z
# box.pose.position.x = 0.5
# box.pose.position.y = 0.0
# box.pose.position.z = 0.0
box.pose.orientation.x = 0.0
box.pose.orientation.y = 0.0
box.pose.orientation.z = 0.0
box.pose.orientation.w = 1.0
self.planner.add_box_obstacle((100, 100, 0.00001), "box", box)
# Controller for left arm
self.c_left = Controller(Kp, Ki, Kd, Kw, Limb('left'))
def move_to_coord(trans,
rot,
arm,
which_arm='left',
keep_oreint=False,
base="base"):
#coordinates are in baxter's torso!
goal = PoseStamped()
goal.header.frame_id = base
# x, y, and z position
goal.pose.position.x = trans[0]
goal.pose.position.y = trans[1]
goal.pose.position.z = trans[2]
# Orientation as a quaternion
goal.pose.orientation.x = rot[0]
goal.pose.orientation.y = rot[1]
goal.pose.orientation.z = rot[2]
goal.pose.orientation.w = rot[3]
# Set the goal state to the pose you just defined
arm.set_pose_target(goal)
# Set the start state for the arm
arm.set_start_state_to_current_state()
if keep_oreint:
# Create a path constraint for the arm
orien_const = OrientationConstraint()
orien_const.link_name = which_arm + "_gripper"
orien_const.header.frame_id = "base"
#constrain it to be the same as my goal state. Seems reasonable.
orien_const.orientation.x = rot[0]
orien_const.orientation.y = rot[1]
orien_const.orientation.z = rot[2]
orien_const.orientation.w = rot[3]
orien_const.absolute_x_axis_tolerance = 0.1
orien_const.absolute_y_axis_tolerance = 0.1
orien_const.absolute_z_axis_tolerance = 0.1
orien_const.weight = 1.0
consts = Constraints()
consts.orientation_constraints = [orien_const]
print(consts)
arm.set_path_constraints(consts)
# Plan a path
arm_plan = arm.plan()
# Execute the plan
arm.execute(arm_plan)
def liftgripper(self, threshold=0.055):
# approx centers of onions at 0.82, width of onion is 0.038 m. table is at 0.78
# length of gripper is 0.163 m The gripper should not go lower than
# (height_z of table w.r.t base+gripper-height/2+tolerance) = 0.78-0.93+0.08+0.01=-0.24
# pnp._limb.endpoint_pose returns {'position': (x, y, z), 'orientation': (x, y, z, w)}
# moving from z=-.02 to z=-0.1
# print "Attempting to lift gripper"
group = self.group
while self.target_location_x == -100:
rospy.sleep(0.05)
position_constraint = PositionConstraint()
position_constraint.target_point_offset.x = 0.1
position_constraint.target_point_offset.y = 0.1
position_constraint.target_point_offset.z = 0.5
position_constraint.weight = 0.25
position_constraint.link_name = group.get_end_effector_link()
position_constraint.header.frame_id = group.get_planning_frame()
orientation_constraint = OrientationConstraint()
orientation_constraint.orientation = Quaternion(x=self.q[0],
y=self.q[1],
z=self.q[2],
w=self.q[3])
orientation_constraint.absolute_x_axis_tolerance = 0.3
orientation_constraint.absolute_y_axis_tolerance = 0.3
orientation_constraint.absolute_z_axis_tolerance = 0.3
orientation_constraint.weight = 0.5 # Empirically estimated values for Sawyer Robot
orientation_constraint.link_name = group.get_end_effector_link()
orientation_constraint.header.frame_id = group.get_planning_frame()
constraint = Constraints()
constraint.orientation_constraints.append(orientation_constraint)
constraint.position_constraints.append(position_constraint)
group.set_path_constraints(constraint)
current_pose = group.get_current_pose().pose
allow_replanning = False
planning_time = 12
lifted = False
# print "Current z pose: ", current_pose.position.z
z_pose = current_pose.position.z + 0.25
while not lifted:
lifted = self.go_to_pose_goal(
current_pose.orientation.x, current_pose.orientation.y,
current_pose.orientation.z, current_pose.orientation.w,
current_pose.position.x, current_pose.position.y, z_pose,
allow_replanning, planning_time, threshold)
# current_pose = group.get_current_pose().pose
rospy.sleep(0.01)
group.clear_path_constraints()
# print "Successfully lifted gripper to z: ", current_pose.position.z
return True
def create_constraint(name): orien_const = OrientationConstraint() orien_const.link_name = name; orien_const.header.frame_id = "base"; orien_const.orientation.y = 2**.5/2 orien_const.orientation.w = 2**.5/2 orien_const.absolute_x_axis_tolerance = 0.1; orien_const.absolute_y_axis_tolerance = 0.1; orien_const.absolute_z_axis_tolerance = 0.1; orien_const.weight = 3.0; consts = Constraints() consts.orientation_constraints = [orien_const] return consts
def right_to_player(right_arm, right_gripper):
# get current number of cards player has
global player_cards
# get global deck coordinates
global deck_x
global deck_y
global deck_z
# create pose above drop point for player's card
card = PoseStamped()
card.header.frame_id = "base"
# x, y, and z position of card
card.pose.position.x = deck_x + .25
card.pose.position.y = deck_y + .2 + (.1 * player_cards)
card.pose.position.z = deck_z
# Orientation as a quaternion - straight down
card.pose.orientation.x = 0.0
card.pose.orientation.y = -1.0
card.pose.orientation.z = 0.0
card.pose.orientation.w = 0.0
# Plan and execute path to desired card position from current state
right_arm.set_pose_target(card)
right_arm.set_start_state_to_current_state()
# Add orientation constraint for path - straight down
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper"
orien_const.header.frame_id = "base"
orien_const.orientation.y = -1.0
orien_const.absolute_x_axis_tolerance = 0.1
orien_const.absolute_y_axis_tolerance = 0.1
orien_const.absolute_z_axis_tolerance = 0.1
orien_const.weight = 1.0
consts = Constraints()
consts.orientation_constraints = [orien_const]
right_arm.set_path_constraints(consts)
right_plan = right_arm.plan()
right_arm.execute(right_plan)
# drop card
right_gripper.stop()
# sleep for a moment
rospy.sleep(1.0)
# increment number of cards player has
player_cards += 1
def staticDip(self, z_pose=0.1, tolerance=0.05):
group = self.group
while self.target_location_x == -100:
rospy.sleep(0.05)
current_pose = group.get_current_pose().pose
position_constraint = PositionConstraint()
position_constraint.target_point_offset.x = 0.1
position_constraint.target_point_offset.y = 0.1
position_constraint.target_point_offset.z = 0.5
position_constraint.weight = 0.1
position_constraint.link_name = group.get_end_effector_link()
position_constraint.header.frame_id = group.get_planning_frame()
orientation_constraint = OrientationConstraint()
orientation_constraint.orientation = Quaternion(x=self.q[0],
y=self.q[1],
z=self.q[2],
w=self.q[3])
orientation_constraint.absolute_x_axis_tolerance = 0.1
orientation_constraint.absolute_y_axis_tolerance = 0.1
orientation_constraint.absolute_z_axis_tolerance = 0.1
orientation_constraint.weight = 0.3
orientation_constraint.link_name = group.get_end_effector_link()
orientation_constraint.header.frame_id = group.get_planning_frame()
constraint = Constraints()
constraint.orientation_constraints.append(orientation_constraint)
constraint.position_constraints.append(position_constraint)
group.set_path_constraints(constraint)
allow_replanning = False
planning_time = 10
# print "Now performing dip"
before_dip = current_pose.position.z
dip = self.go_to_pose_goal(
self.q[0],
self.q[1],
self.q[2],
self.q[3],
self.target_location_x + 0.01, # accounting for tolerance error
self.target_location_y, # accounting for tolerance error
z_pose, # This is where we dip
allow_replanning,
planning_time,
tolerance)
rospy.sleep(0.5)
dipped_pose = group.get_current_pose().pose.position.z
if dip and (dipped_pose < before_dip):
print "Successfully dipped! z pos: ", dipped_pose
group.clear_path_constraints()
return True
else:
self.staticDip()
def set_orientation_constraints(self): oc = OrientationConstraint() oc.header.frame_id = 'world' oc.link_name = 'ee_arm_link' quat = tf.transformations.quaternion_from_euler(math.radians(0), math.radians(-28), math.radians(0)) oc.orientation.x = quat[0] oc.orientation.y = quat[1] oc.orientation.z = quat[2] oc.orientation.w = quat[3] oc.absolute_x_axis_tolerance = 0.5 oc.absolute_y_axis_tolerance = 0.1 oc.absolute_z_axis_tolerance = 0.5 oc.weight = 1.0 self.constraints.orientation_constraints.append(oc)
def set_upright_constraints(self,pose):
upright_constraints = Constraints()
orientation_constraint = OrientationConstraint()
upright_constraints.name = "upright"
orientation_constraint.header = pose.header
orientation_constraint.link_name = group.get_end_effector_link()
orientation_constraint.orientation = pose.pose.orientation
orientation_constraint.absolute_x_axis_tolerance = 3.14
orientation_constraint.absolute_y_axis_tolerance = 0.1
orientation_constraint.absolute_z_axis_tolerance = 0.1
#orientation_constraint.absolute_z_axis_tolerance = 3.14 #ignore this axis
orientation_constraint.weight = 1
upright_constraints.orientation_constraints.append(orientation_constraint)
group.set_path_constraints(upright_constraints)
def right_to_deck(right_arm, right_gripper):
# get global deck coordinates
global deck_x
global deck_y
global deck_z
# create pose above deck of cards
deck = PoseStamped()
deck.header.frame_id = "base"
# x, y, and z position of deck
deck.pose.position.x = deck_x
deck.pose.position.y = deck_y
deck.pose.position.z = deck_z
# Orientation as a quaternion - straight down
deck.pose.orientation.x = 0.0
deck.pose.orientation.y = -1.0
deck.pose.orientation.z = 0.0
deck.pose.orientation.w = 0.0
# Plan and execute path from current state
right_arm.set_pose_target(deck)
right_arm.set_start_state_to_current_state()
# Add orientation constraint for path - straight down
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper"
orien_const.header.frame_id = "base"
orien_const.orientation.y = -1.0
orien_const.absolute_x_axis_tolerance = 0.1
orien_const.absolute_y_axis_tolerance = 0.1
orien_const.absolute_z_axis_tolerance = 0.1
orien_const.weight = 1.0
consts = Constraints()
consts.orientation_constraints = [orien_const]
right_arm.set_path_constraints(consts)
right_plan = right_arm.plan()
right_arm.execute(right_plan)
# pick up card
right_gripper.command_suction(True)
# increment z position as deck height decreases
deck_z -= .001
# sleep for a moment
rospy.sleep(1.0)
def createOrientationConstraint():
orientation_constraints = []
o = OrientationConstraint()
o.header.frame_id = 'base_link'
o.link_name = 'arm_right_tool_link'
o.orientation.x = 0.0
o.orientation.y = 0.0
o.orientation.z = 0.0
o.orientation.w = 1.0
o.weight = 1.0
o.absolute_x_axis_tolerance = 0.1
o.absolute_y_axis_tolerance = 0.1
o.absolute_z_axis_tolerance = 0.1
orientation_constraints.append(o)
return orientation_constraints
def set_path_ori_constraints(self, stampedPoseList):
self.__graspPathConstraints.orientation_constraints = []
ori_constraints = []
for stampedPose in stampedPoseList:
constraint = OrientationConstraint()
constraint.header = stampedPose.header
constraint.orientation = deepcopy(stampedPose.pose.orientation)
constraint.link_name = "world"
constraint.absolute_x_axis_tolerance = 0.2
constraint.absolute_y_axis_tolerance = 0.2
constraint.absolute_z_axis_tolerance = 0.2
constraint.weight = 0.8
ori_constraints.append(constraint)
# ipdb.set_trace()
self.__graspPathConstraints.orientation_constraints = ori_constraints
def __init__(self, path_planner, gripper, gripper_name, gripper_length):
self.planner = path_planner
self.gripper = gripper
# Create a path constraint for the arm
self.orien_const = OrientationConstraint()
self.orien_const.link_name = gripper_name
self.orien_const.header.frame_id = "base"
self.orien_const.orientation.y = -1.0
self.orien_const.absolute_x_axis_tolerance = 0.1
self.orien_const.absolute_y_axis_tolerance = 0.1
self.orien_const.absolute_z_axis_tolerance = 0.1
self.orien_const.weight = 1.0
self.gripper_length = gripper_length
def init_path_constraints(self): self.upright_constraints.name = 'upright' orientation_constraint = OrientationConstraint() current_pose = self.group.get_current_pose().pose planning_frame = self.group.get_planning_frame() orientation_constraint.header.frame_id = planning_frame orientation_constraint.link_name = self.group.get_end_effector_link() print 'Constraint initialized: ' + str(orientation_constraint.link_name) orientation_constraint.orientation = current_pose.orientation orientation_constraint.absolute_x_axis_tolerance = PATH_CONSTRAINT_TOLERANCE orientation_constraint.absolute_y_axis_tolerance = PATH_CONSTRAINT_TOLERANCE orientation_constraint.absolute_z_axis_tolerance = PATH_CONSTRAINT_TOLERANCE orientation_constraint.weight = 0.01 self.upright_constraints.orientation_constraints.append(orientation_constraint) self.group.set_path_constraints(self.upright_constraints)
def place(robot):
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0.7
p.pose.position.y = 0.0
p.pose.position.z = 0.5
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 1.0
g = PlaceLocation()
g.place_pose = p
g.pre_place_approach.direction.vector.z = -1.0
g.post_place_retreat.direction.vector.x = -1.0
g.post_place_retreat.direction.header.frame_id = robot.get_planning_frame()
g.pre_place_approach.direction.header.frame_id = "r_wrist_roll_link"
g.pre_place_approach.min_distance = 0.1
g.pre_place_approach.desired_distance = 0.2
g.post_place_retreat.min_distance = 0.1
g.post_place_retreat.desired_distance = 0.25
g.post_place_posture = open_gripper(g.post_place_posture)
group = robot.get_group("right_arm")
group.set_support_surface_name("table")
# Add path constraints
constr = Constraints()
constr.orientation_constraints = []
ocm = OrientationConstraint()
ocm.link_name = "r_wrist_roll_link"
ocm.header.frame_id = p.header.frame_id
ocm.orientation.x = 0.0
ocm.orientation.y = 0.0
ocm.orientation.z = 0.0
ocm.orientation.w = 1.0
ocm.absolute_x_axis_tolerance = 0.2
ocm.absolute_y_axis_tolerance = 0.2
ocm.absolute_z_axis_tolerance = math.pi
ocm.weight = 1.0
constr.orientation_constraints.append(ocm)
# group.set_path_constraints(constr)
group.set_planner_id("RRTConnectkConfigDefault")
group.place("part", g)
def plan_cartesian_path(self, xcor,ycor,zcor,pose_goal_base, vel_scale, scale=1 ):
move_group = self.move_group
waypoints = []
pose_goal = geometry_msgs.msg.Pose()
print("length of x coordinate ", len(xcor))
print("base pose = ", pose_goal_base.position)
eef_pose = Pose()
for i in range(0,len(xcor)):
eef_pose.position.x = -float(xcor[i])/1000.0
eef_pose.position.y = float(ycor[i])/1000.0
eef_pose.position.z = float(zcor[i])/1000.0
eef_in_baseframe = self.do_transform(eef_pose,'ee_link','world')
pose_goal.position.x = eef_in_baseframe.position.x
pose_goal.position.y = eef_in_baseframe.position.y
pose_goal.position.z = eef_in_baseframe.position.z
pose_goal.orientation.x = pose_goal_base.orientation.x
pose_goal.orientation.y = pose_goal_base.orientation.y
pose_goal.orientation.z = pose_goal_base.orientation.z
pose_goal.orientation.w = pose_goal_base.orientation.w
waypoints.append(copy.deepcopy(pose_goal))
orient = OrientationConstraint()
orient.link_name = self.eef_link
orient.header.frame_id = self.planning_frame
(plan, fraction) = move_group.compute_cartesian_path(
waypoints, # waypoints to follow
0.001, # eef_step
0.0) # jump_threshold
# move_group = self.move_group
# move_group.execute(plan, wait=True)
ref_state = self.robot.get_current_state()
replanned = move_group.retime_trajectory(ref_state,plan,velocity_scaling_factor=vel_scale,acceleration_scaling_factor=vel_scale)
return replanned, fraction
def create_move_group_pose_goal(goal_pose=Pose(),
group="right_arm_torso",
end_link_name=None,
plan_only=True):
""" Creates a move_group goal based on pose.
@arg group string representing the move_group group to use
@arg end_link_name string representing the ending link to use
@arg goal_pose Pose() representing the goal pose
@arg plan_only bool to for only planning or planning and executing
@returns MoveGroupGoal with the data given on the arguments"""
header = Header()
header.frame_id = 'base_link'
header.stamp = rospy.Time.now()
# We are filling in the MoveGroupGoal a MotionPlanRequest and a PlanningOptions message
# http://docs.ros.org/hydro/api/moveit_msgs/html/msg/MotionPlanRequest.html
# http://docs.ros.org/hydro/api/moveit_msgs/html/msg/PlanningOptions.html
moveit_goal = MoveGroupGoal()
goal_c = Constraints()
position_c = PositionConstraint()
position_c.header = header
if end_link_name != None: # For some groups the end_link_name can be deduced, but better add it manually
position_c.link_name = end_link_name
position_c.constraint_region.primitives.append(
SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[
0.01
])) # how big is the area where the end effector can be
position_c.constraint_region.primitive_poses.append(goal_pose)
position_c.weight = 1.0
goal_c.position_constraints.append(position_c)
orientation_c = OrientationConstraint()
orientation_c.header = header
if end_link_name != None:
orientation_c.link_name = end_link_name
orientation_c.orientation = goal_pose.orientation
orientation_c.absolute_x_axis_tolerance = 0.01 # Tolerances, MoveIt! by default uses 0.001 which may be too low sometimes
orientation_c.absolute_y_axis_tolerance = 0.01
orientation_c.absolute_z_axis_tolerance = 0.01
orientation_c.weight = 1.0
goal_c.orientation_constraints.append(orientation_c)
moveit_goal.request.goal_constraints.append(goal_c)
moveit_goal.request.num_planning_attempts = 5 # The number of times this plan is to be computed. Shortest solution will be reported.
moveit_goal.request.allowed_planning_time = 5.0
moveit_goal.planning_options.plan_only = plan_only
moveit_goal.planning_options.planning_scene_diff.is_diff = True # Necessary
moveit_goal.request.group_name = group
return moveit_goal
