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 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_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 on_enter(self, userdata):
self._failed = False
request = GetCartesianPathRequest()
request.group_name = self._move_group
request.avoid_collisions = not self._ignore_collisions
request.max_step = 0.05
request.header = userdata.eef_pose.header
request.waypoints.append(userdata.eef_pose.pose)
now = rospy.Time.now()
try:
self._tf.waitForTransform('base_link', 'gripper_cam_link', now, rospy.Duration(1))
(p,q) = self._tf.lookupTransform('gripper_cam_link', 'base_link', now)
c = OrientationConstraint()
c.header.frame_id = 'base_link'
c.header.stamp = now
c.link_name = 'gripper_cam_link'
c.orientation.x = q[0]
c.orientation.y = q[1]
c.orientation.z = q[2]
c.orientation.w = q[3]
c.weight = 1
c.absolute_x_axis_tolerance = 0.1
c.absolute_y_axis_tolerance = 0.1
c.absolute_z_axis_tolerance = 0.1
#request.path_constraints.orientation_constraints.append(c)
self._result = self._srv.call(self._topic, request)
except Exception as e:
Logger.logwarn('Exception while calculating path:\n%s' % str(e))
self._failed = True
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 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_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 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 create_move_group_pose_goal(self, goal_pose=Pose(), group="manipulator", end_link_name=None, plan_only=True):
header = Header()
header.frame_id = 'torso_base_link'
header.stamp = rospy.Time.now()
moveit_goal = MoveGroupGoal()
goal_c = Constraints()
position_c = PositionConstraint()
position_c.header = header
if end_link_name != None:
position_c.link_name = end_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 = 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
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
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
def append_traj_to_move_group_goal(goal_to_append=None, goal_pose=Pose(), link_name=None):
""" Appends a trajectory_point to the given move_group goal, returns it appended"""
if goal_to_append == None:
rospy.logerr("append_trajectory_point_to_move_group_goal needs a goal!")
return
#goal_to_append = MoveGroupGoal()
#rospy.logwarn("goal_to_append is: \n" + str(goal_to_append))
traj_c = TrajectoryConstraints()
goal_c = Constraints()
goal_c.name = "traj_constraint"
# Position constraint
position_c = PositionConstraint()
position_c.header = goal_to_append.request.goal_constraints[0].position_constraints[0].header
position_c.link_name = goal_to_append.request.goal_constraints[0].position_constraints[0].link_name if link_name == None else 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 = 2.0
goal_c.position_constraints.append(position_c)
# Orientation constraint
orientation_c = OrientationConstraint()
orientation_c.header = goal_to_append.request.goal_constraints[0].position_constraints[0].header
orientation_c.link_name = goal_to_append.request.goal_constraints[0].position_constraints[0].link_name if link_name == None else 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)
traj_c.constraints.append(goal_c)
goal_to_append.request.trajectory_constraints = traj_c
return goal_to_append
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 _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 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 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 execute(self, userdata):
# Prepare the position
goal_pose = Pose()
goal_pose.position = userdata.manip_goal_pose
# Set the rotation of the tool link, all 0 means for the right hand palm looking left straight
# roll = rotation over X, pitch = rotation over Y, yaw = rotation over Z
quat = quaternion_from_euler(0.0, 0.0, 0.0) # roll, pitch, yaw
goal_pose.orientation = Quaternion(*quat.tolist())
header = Header()
header.frame_id = 'base_link'
header.stamp = rospy.Time.now()
userdata.move_it_goal = MoveGroupGoal()
goal_c = Constraints()
position_c = PositionConstraint()
position_c.header = header
if userdata.manip_end_link != None: # For some groups the end_link_name can be deduced, but better add it manually
position_c.link_name = userdata.manip_end_link
# how big is the area where the end effector can be
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 = header
if userdata.manip_end_link != None:
orientation_c.link_name = userdata.manip_end_link
orientation_c.orientation = goal_pose.orientation
# Tolerances, MoveIt! by default uses 0.001 which may be too low sometimes
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)
userdata.move_it_goal.request.goal_constraints.append(goal_c)
userdata.move_it_goal.request.num_planning_attempts = 5 # The number of times this plan is to be computed. Shortest solution will be reported.
userdata.move_it_goal.request.allowed_planning_time = 5.0
userdata.move_it_goal.planning_options.plan_only = False # True: Plan-Only..... False : Plan + execute
userdata.move_it_goal.planning_options.planning_scene_diff.is_diff = True # Necessary
userdata.move_it_goal.request.group_name = userdata.manip_group
return 'succeeded'
def move_to_coord(trans, rot, keep_oreint=False):
#coordinates are in baxter's torso!
global arm
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 = 'left'+"_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.02;
orien_const.absolute_y_axis_tolerance = 0.02;
orien_const.absolute_z_axis_tolerance = 0.02;
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)
arm.go(arm_plan,wait=True)
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 planMoveToPosHoldOrientation(pos=None, orientation=None, arm=None):
arm = arm or left_arm
if arm is left_arm:
pos = pos or left_arm_default_state[0]
orientation = orientation or left_arm_default_state[1]
if arm is right_arm:
pos = pos or right_arm_default_state[0]
orientation = orientation or right_arm_default_state[1]
#Second goal pose -----------------------------------------------------
goal_2 = PoseStamped()
goal_2.header.frame_id = "base"
#x, y, and z position
goal_2.pose.position.x = pos[0]
goal_2.pose.position.y = pos[1]
goal_2.pose.position.z = pos[2]
#Orientation as a quaternion
orientation = orientation/np.linalg.norm(orientation)
goal_2.pose.orientation.x = orientation[0]
goal_2.pose.orientation.y = orientation[1]
goal_2.pose.orientation.z = orientation[2]
goal_2.pose.orientation.w = orientation[3]
#Set the goal state to the pose you just defined
arm.set_pose_target(goal_2)
#Set the start state for the left arm
arm.set_start_state_to_current_state()
# #Create a path constraint for the arm
# #UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS
orien_const = OrientationConstraint()
orien_const.link_name = "left_gripper";
orien_const.header.frame_id = "base";
orien_const.orientation.x = orientation[0]
orien_const.orientation.y = orientation[1]
orien_const.orientation.z = orientation[2]
orien_const.orientation.w = orientation[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]
arm.set_path_constraints(consts)
#Plan a path
return (arm, arm.plan())
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 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_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 planPath(positions, orientation=None, arm=None, holdOrientation=False, step=0.01, threshold=1000):
arm = arm or left_arm
if arm is left_arm:
positions = (left_arm_default_state[0],) if positions is None else positions
orientation = left_arm_default_state[1] if orientation is None else orientation
if arm is right_arm:
positions = (right_arm_default_state[0],) if positions is None else positions
orientation = right_arm_default_state[1] if orientation is None else orientation
# Compute path
waypoints = []
# start with the current pose
#waypoints.append(arm.get_current_pose().pose)
wpose = Pose()
orientation = orientation/np.linalg.norm(orientation)
wpose.orientation.x = orientation[0]
wpose.orientation.y = orientation[1]
wpose.orientation.z = orientation[2]
wpose.orientation.w = orientation[3]
for pos in positions:
wpose.position.x = pos[0]
wpose.position.y = pos[1]
wpose.position.z = pos[2]
waypoints.append(copy.deepcopy(wpose))
if holdOrientation:
# #Create a path constraint for the arm
# #UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS
orien_const = OrientationConstraint()
orien_const.link_name = "left_gripper";
orien_const.header.frame_id = "base";
orien_const.orientation.x = orientation[0]
orien_const.orientation.y = orientation[1]
orien_const.orientation.z = orientation[2]
orien_const.orientation.w = orientation[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]
arm.set_path_constraints(consts)
#Plan a path
(plan, fraction) = arm.compute_cartesian_path(waypoints, step, threshold)
return (arm, plan, fraction)
def add_robot_constraints():
constraint = Constraints()
constraint.name = "camera constraint"
roll_constraint = OrientationConstraint()
# 'base_link' is equal to the world link
roll_constraint.header.frame_id = 'world'
# The link that must be oriented upwards
roll_constraint.link_name = "camera"
roll_constraint.orientation = Quaternion(*tf.transformations.quaternion_from_euler(0,np.pi/3,0))
# Allow rotation of 45 degrees around the x and y axis
roll_constraint.absolute_x_axis_tolerance = np.pi #Allow max rotation of x degrees
roll_constraint.absolute_y_axis_tolerance = np.pi
roll_constraint.absolute_z_axis_tolerance = np.pi/2
# The roll constraint is the only constraint
roll_constraint.weight = 1
#constraint.orientation_constraints = [roll_constraint]
joint_1_constraint = JointConstraint()
joint_1_constraint.joint_name = "joint_1"
joint_1_constraint.position = 0
joint_1_constraint.tolerance_above = np.pi/2
joint_1_constraint.tolerance_below = np.pi/2
joint_1_constraint.weight = 1
joint_4_constraint = JointConstraint()
joint_4_constraint.joint_name = "joint_4"
joint_4_constraint.position = 0
joint_4_constraint.tolerance_above = np.pi/2
joint_4_constraint.tolerance_below = np.pi/2
joint_4_constraint.weight = 1
joint_5_constraint = JointConstraint()
joint_5_constraint.joint_name = "joint_5"
joint_5_constraint.position = np.pi/2
joint_5_constraint.tolerance_above = np.pi/2
joint_5_constraint.tolerance_below = np.pi/2
joint_5_constraint.weight = 1
joint_6_constraint = JointConstraint()
joint_6_constraint.joint_name = "joint_6"
joint_6_constraint.position = np.pi-0.79
joint_6_constraint.tolerance_above = np.pi
joint_6_constraint.tolerance_below = np.pi
joint_6_constraint.weight = 1
constraint.joint_constraints = [joint_1_constraint, joint_6_constraint]
return constraint
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
def addConstrains():
pose = group.get_current_pose()
constraint = Constraints()
constraint.name = "downRight"
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 = 0.1
#orientation_constraint.absolute_z_axis_tolerance = 3.14 #ignore this axis
orientation_constraint.weight = 1
constraint.orientation_constraints.append(orientation_constraint)
group.set_path_constraints(constraint)
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_test')
arm = MoveGroupCommander('arm')
end_effector_link = arm.get_end_effector_link()
arm.allow_replanning(True)
arm.set_planning_time(5)
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_init_orientation = Quaternion()
target_init_orientation = quaternion_from_euler(0.0, 1.57, 0.0)
self.setPose(target_pose, [0.5, 0.2, 0.5],list(target_init_orientation))
current_pose = arm.get_current_pose(end_effector_link)
self.setPose(current_pose, [current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z], list(target_init_orientation))
arm.set_pose_target(current_pose)
arm.go()
rospy.sleep(2)
constraints = Constraints()
orientation_constraint = OrientationConstraint()
constraints.name = 'gripper constraint'
orientation_constraint.header = target_pose.header
orientation_constraint.link_name = end_effector_link
orientation_constraint.orientation.x = target_init_orientation[0]
orientation_constraint.orientation.y = target_init_orientation[1]
orientation_constraint.orientation.z = target_init_orientation[2]
orientation_constraint.orientation.w = target_init_orientation[3]
orientation_constraint.absolute_x_axis_tolerance = 0.03
orientation_constraint.absolute_y_axis_tolerance = 0.03
orientation_constraint.absolute_z_axis_tolerance = 3.14
orientation_constraint.weight = 1.0
constraints.orientation_constraints.append(orientation_constraint)
arm.set_path_constraints(constraints)
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(1)
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def setOrientationConstraints(qw,qx,qy,qz, group_handle,weight): orient_constrain = OrientationConstraint(); orient_constrain.header.frame_id = "base_link"; orient_constrain.link_name = group_handle.get_end_effector_link(); orient_constrain.orientation = Quaternion(); orient_constrain.orientation.w = qw; orient_constrain.orientation.x = qx; orient_constrain.orientation.y = qy; orient_constrain.orientation.z = qz; orient_constrain.absolute_x_axis_tolerance = 0.01; orient_constrain.absolute_y_axis_tolerance = 0.01; orient_constrain.absolute_z_axis_tolerance = 2*pi; orient_constrain.weight = weight; return orient_constrain;
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
def move_to_coord(trans, rot, arm, which_arm='left', keep_oreint=False):
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";
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]
arm.set_path_constraints(consts)
# Plan a path
arm_plan = arm.plan()
# Execute the plan
arm.execute(arm_plan)
def create_move_group_pose_goal(
self, goal_pose=Pose(), group="left_arm", end_link_name="arm_left_tool_link", 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"""
# Specifying the header makes the planning fail...
header = Header()
header.frame_id = "base_link"
header.stamp = rospy.Time.now()
moveit_goal = MoveGroupGoal()
goal_c = Constraints()
position_c = PositionConstraint()
position_c.header = header
position_c.link_name = end_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 = header
orientation_c.link_name = end_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)
moveit_goal.request.goal_constraints.append(goal_c)
moveit_goal.request.num_planning_attempts = 3
moveit_goal.request.allowed_planning_time = 1.0
moveit_goal.planning_options.plan_only = plan_only
moveit_goal.planning_options.planning_scene_diff.is_diff = True
moveit_goal.request.group_name = group
return moveit_goal
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_constraints_demo', anonymous=True)
robot = RobotCommander()
# Connect to the arm move group
arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Increase the planning time since constraint planning can take a while
arm.set_planning_time(5)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame
arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow some leeway in position(meters) and orientation (radians)
arm.set_goal_position_tolerance(0.05)
arm.set_goal_orientation_tolerance(0.1)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Start in the "resting" configuration stored in the SRDF file
arm.set_named_target('l_arm_init')
# Plan and execute a trajectory to the goal configuration
arm.go()
rospy.sleep(1)
# Open the gripper
gripper.set_joint_value_target(GRIPPER_NEUTRAL)
gripper.go()
rospy.sleep(1)
# Set an initial target pose with the arm up and to the right
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.263803774718
target_pose.pose.position.y = 0.295405791959
target_pose.pose.position.z = 0.690438884208
q = quaternion_from_euler(0, 0, -1.57079633)
target_pose.pose.orientation.x = q[0]
target_pose.pose.orientation.y = q[1]
target_pose.pose.orientation.z = q[2]
target_pose.pose.orientation.w = q[3]
# Set the start state and target pose, then plan and execute
arm.set_start_state(robot.get_current_state())
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(2)
# Close the gripper
gripper.set_joint_value_target(GRIPPER_CLOSED)
gripper.go()
rospy.sleep(1)
# Store the current pose
start_pose = arm.get_current_pose(end_effector_link)
# Create a contraints list and give it a name
constraints = Constraints()
constraints.name = "Keep gripper horizontal"
# Create an orientation constraint for the right gripper
orientation_constraint = OrientationConstraint()
orientation_constraint.header = start_pose.header
orientation_constraint.link_name = arm.get_end_effector_link()
orientation_constraint.orientation.w = 1.0
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 = 1.0
# q = quaternion_from_euler(0, 0, -1.57079633)
# orientation_constraint.orientation.x = q[0]
# orientation_constraint.orientation.y = q[1]
# orientation_constraint.orientation.z = q[2]
# orientation_constraint.orientation.w = q[3]
# Append the constraint to the list of contraints
constraints.orientation_constraints.append(orientation_constraint)
# Set the path constraints on the arm
arm.set_path_constraints(constraints)
# Set a target pose for the arm
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.39000848183
target_pose.pose.position.y = 0.185900663329
target_pose.pose.position.z = 0.732752341378
target_pose.pose.orientation.w = 1
# Set the start state and target pose, then plan and execute
arm.set_start_state_to_current_state()
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(1)
# Clear all path constraints
arm.clear_path_constraints()
# Open the gripper
gripper.set_joint_value_target(GRIPPER_NEUTRAL)
gripper.go()
rospy.sleep(1)
# Return to the "resting" configuration stored in the SRDF file
arm.set_named_target('l_arm_init')
# Plan and execute a trajectory to the goal configuration
arm.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
def main():
#Initialize moveit_commander
moveit_commander.roscpp_initialize(sys.argv)
#Start a node
rospy.init_node('moveit_node')
#Set up the left gripper
left_gripper = baxter_gripper.Gripper('left')
#Calibrate the gripper (other commands won't work unless you do this first)
print('Calibrating...')
left_gripper.calibrate()
rospy.sleep(2.0)
#Initialize both arms
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
left_arm = moveit_commander.MoveGroupCommander('left_arm')
right_arm = moveit_commander.MoveGroupCommander('right_arm')
left_arm.set_planner_id('RRTConnectkConfigDefault')
left_arm.set_planning_time(10)
right_arm.set_planner_id('RRTConnectkConfigDefault')
right_arm.set_planning_time(10)
#First goal pose ------------------------------------------------------
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = 0.2
goal_1.pose.position.y = 0.6
goal_1.pose.position.z = 0.2
#Orientation as a quaternion
goal_1.pose.orientation.x = 0.0
goal_1.pose.orientation.y = -1.0
goal_1.pose.orientation.z = 0.0
goal_1.pose.orientation.w = 0.0
#Set the goal state to the pose you just defined
left_arm.set_pose_target(goal_1)
#Set the start state for the left arm
left_arm.set_start_state_to_current_state()
#Plan a path
left_plan = left_arm.plan()
#Execute the plan
raw_input('Press <Enter> to move the left arm to goal pose 1 (path constraints are never enforced during this motion): ')
left_arm.execute(left_plan)
#Second goal pose -----------------------------------------------------
rospy.sleep(2.0)
#Close the left gripper
#print('Closing...')
#left_gripper.close(block=True)
#rospy.sleep(0.5)
#Open the left gripper
#print('Opening...')
#left_gripper.open(block=True)
#rospy.sleep(1.0)
print('Done!')
goal_2 = PoseStamped()
goal_2.header.frame_id = "base"
#x, y, and z position
goal_2.pose.position.x = 0.6
goal_2.pose.position.y = 0.4
goal_2.pose.position.z = 0.0
#Orientation as a quaternion
goal_2.pose.orientation.x = 0.0
goal_2.pose.orientation.y = -1.0
goal_2.pose.orientation.z = 0.0
goal_2.pose.orientation.w = 0.0
#Set the goal state to the pose you just defined
left_arm.set_pose_target(goal_2)
#Set the start state for the left arm
left_arm.set_start_state_to_current_state()
# #Create a path constraint for the arm
# #UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS
orien_const = OrientationConstraint()
orien_const.link_name = "left_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]
left_arm.set_path_constraints(consts)
#Plan a path
left_plan = left_arm.plan()
#Execute the plan
raw_input('Press <Enter> to move the left arm to goal pose 2: ')
left_arm.execute(left_plan)
#Third goal pose -----------------------------------------------------
rospy.sleep(2.0)
#Close the left gripper
#print('Closing...')
#left_gripper.close(block=True)
#rospy.sleep(0.5)
#Open the left gripper
#print('Opening...')
#left_gripper.open(block=True)
#rospy.sleep(1.0)
#print('Done!')
goal_3 = PoseStamped()
goal_3.header.frame_id = "base"
#x, y, and z position
goal_3.pose.position.x = 0.0
goal_3.pose.position.y = 0.7
goal_3.pose.position.z = 0.0
#Orientation as a quaternion
goal_3.pose.orientation.x = 0.0
goal_3.pose.orientation.y = -1.0
goal_3.pose.orientation.z = 0.0
goal_3.pose.orientation.w = 0.0
#Set the goal state to the pose you just defined
left_arm.set_pose_target(goal_3)
#Set the start state for the left arm
left_arm.set_start_state_to_current_state()
#Create a path constraint for the arm
# #UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS
orien_const = OrientationConstraint()
orien_const.link_name = "left_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]
left_arm.set_path_constraints(consts)
#Plan a path
left_plan = left_arm.plan()
#Execute the plan
raw_input('Press <Enter> to move the left arm to goal pose 3: ')
left_arm.execute(left_plan)
#Fourth goal pose -----------------------------------------------------
rospy.sleep(2.0)
#Close the left gripper
#print('Closing...')
#left_gripper.close(block=True)
#rospy.sleep(0.5)
#Open the left gripper
#print('Opening...')
#left_gripper.open(block=True)
#rospy.sleep(1.0)
#print('Done!')
goal_4 = PoseStamped()
goal_4.header.frame_id = "base"
#x, y, and z position
goal_4.pose.position.x = 0.4
goal_4.pose.position.y = 0.7
goal_4.pose.position.z = 0.3
#Orientation as a quaternion
goal_4.pose.orientation.x = 0.0
goal_4.pose.orientation.y = -1.0
goal_4.pose.orientation.z = 0.0
goal_4.pose.orientation.w = 0.0
#Set the goal state to the pose you just defined
left_arm.set_pose_target(goal_4)
#Set the start state for the left arm
left_arm.set_start_state_to_current_state()
#Create a path constraint for the arm
# #UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS
#orien_const = OrientationConstraint()
#orien_const.link_name = "left_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]
#left_arm.set_path_constraints(consts)
#Plan a path
left_plan = left_arm.plan()
#Execute the plan
raw_input('Press <Enter> to move the left arm to goal pose 4: ')
left_arm.execute(left_plan)
rospy.sleep(2.0)
#Close the left gripper
print('Closing...')
left_gripper.close(block=True)
rospy.sleep(0.5)
#Open the left gripper
print('Opening...')
left_gripper.open(block=True)
rospy.sleep(1.0)
print('Done!')
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
robot = RobotCommander()
# Connect to the right_arm move group
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Increase the planning time since contraint planning can take a while
right_arm.set_planning_time(15)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow some leeway in position(meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.05)
right_arm.set_goal_orientation_tolerance(0.1)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Start in the "resting" configuration stored in the SRDF file
right_arm.set_named_target('resting')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
rospy.sleep(1)
# Open the gripper
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Set an initial target pose with the arm up and to the right
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.237012590198
target_pose.pose.position.y = -0.0747191267505
target_pose.pose.position.z = 0.901578401949
target_pose.pose.orientation.w = 1.0
# Set the start state and target pose, then plan and execute
right_arm.set_start_state(robot.get_current_state())
right_arm.set_pose_target(target_pose, end_effector_link)
right_arm.go()
rospy.sleep(2)
# Close the gripper
right_gripper.set_joint_value_target(GRIPPER_CLOSED)
right_gripper.go()
rospy.sleep(1)
# Store the current pose
start_pose = right_arm.get_current_pose(end_effector_link)
# Create a contraints list and give it a name
constraints = Constraints()
constraints.name = "Keep gripper horizontal"
# Create an orientation constraint for the right gripper
orientation_constraint = OrientationConstraint()
orientation_constraint.header = start_pose.header
orientation_constraint.link_name = right_arm.get_end_effector_link()
orientation_constraint.orientation.w = 1.0
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)
# Set the path constraints on the right_arm
right_arm.set_path_constraints(constraints)
# Set a target pose for the arm
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.173187824708
target_pose.pose.position.y = -0.0159929871606
target_pose.pose.position.z = 0.692596608605
target_pose.pose.orientation.w = 1.0
# Set the start state and target pose, then plan and execute
right_arm.set_start_state_to_current_state()
right_arm.set_pose_target(target_pose, end_effector_link)
right_arm.go()
rospy.sleep(1)
# Clear all path constraints
right_arm.clear_path_constraints()
# Open the gripper
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Return to the "resting" configuration stored in the SRDF file
right_arm.set_named_target('resting')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
def callback(message):
global last_x
global last_y
global last_z
global error
try:
if message.transforms[0].child_frame_id == 'ar_marker_23':
#Read the position of artag
(trans,rot) = listener.lookupTransform('/base', '/ar_marker_23', rospy.Time(0))
#Execute only when the difference of the current position and the last position exceed the threshold
if ((abs(trans[0]-last_x) < threshold) and (abs(trans[1]-last_y) < threshold) and (abs(trans[2]-last_z) < threshold)):
pass
else:
last_x = trans[0]
last_y = trans[1]
last_z = trans[2]
print trans
#Goal position
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]-0.1
#Orientation as a quaternion: default orientation
goal.pose.orientation.x = 0.5
goal.pose.orientation.y = 0.5
goal.pose.orientation.z = 0.5
goal.pose.orientation.w = -0.5
#Set the goal state
right_arm.set_pose_target(goal)
#Set the start state
right_arm.set_start_state_to_current_state()
#Create a orientation constraint for the arm
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper";
orien_const.header.frame_id = "base";
orien_const.orientation.x = 0.5;
orien_const.orientation.y = 0.5;
orien_const.orientation.z = 0.5;
orien_const.orientation.w = -0.5;
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)
#Plan a path
right_plan = right_arm.plan()
#Execute the plan
right_arm.execute(right_plan)
else:
pass
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print 'exception'
