def create_basic_mp_position_request(
planning_frame,
link_name,
target_point_offset,
planner_id):
motion_plan_request = MotionPlanRequest()
motion_plan_request.group_name = move_group
motion_plan_request.num_planning_attempts = 1
motion_plan_request.allowed_planning_time = 5.0
motion_plan_request.workspace_parameters = WorkspaceParameters()
motion_plan_request.max_velocity_scaling_factor = 0.5
motion_plan_request.max_acceleration_scaling_factor = 0.5
motion_plan_request.planner_id = planner_id
position_constraints = []
position_constraint = PositionConstraint()
header = std_msgs.msg.Header()
header.frame_id = planning_frame
position_constraint.header = header
position_constraint.link_name = link_name
position_constraint.target_point_offset = target_point_offset
position_constraints = [position_constraint]
constraint = Constraints()
constraint.position_constraints = position_constraints
constraints = [constraint]
motion_plan_request.goal_constraints = constraints
return motion_plan_request
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 set_position_goal(self,
position,
tolerance=0.001,
weight=1.0,
frame=None):
"""
Set goal position of `frame` in Cartesian space. Defaults to the end-effector `frame`.
"""
if frame == None:
frame = self.arm_base_link
position_constraint = PositionConstraint()
position_constraint.header.frame_id = frame
position_constraint.link_name = self.arm_end_effector
position_constraint.constraint_region.primitive_poses.append(Pose())
position_constraint.constraint_region.primitive_poses[
0].position.x = position[0]
position_constraint.constraint_region.primitive_poses[
0].position.y = position[1]
position_constraint.constraint_region.primitive_poses[
0].position.z = position[2]
# Goal is defined as a sphere with radius equal to tolerance
position_constraint.constraint_region.primitives.append(
SolidPrimitive())
position_constraint.constraint_region.primitives[0].type = 2
position_constraint.constraint_region.primitives[0].dimensions = [
tolerance
]
position_constraint.weight = weight
(self.kinematic_path_request.motion_plan_request.goal_constraints[-1].
position_constraints.append(position_constraint))
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_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 createPositionConstraint():
position_constraints = []
p = PositionConstraint()
p.header.frame_id = 'base_link'
p.link_name = 'arm_right_tool_link'
p.target_point_offset.x = 0.5
p.target_point_offset.y = -0.5
p.target_point_offset.z = 1.0
p.weight = 1.0
position_constraints.append(p)
return position_constraints
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 createPositionConstraint():
position_constraints = []
p = PositionConstraint()
p.header.frame_id = 'base_link'
p.link_name = 'arm_right_tool_link'
p.target_point_offset.x = 0.5
p.target_point_offset.y = -0.5
p.target_point_offset.z = 1.0
p.weight = 1.0
position_constraints.append(p)
return position_constraints
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 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 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 rotate(cls):
mg = ManipulatorActions.get_move_group()
ManipulatorActions.with_planning_scene()
pose = mg.get_current_pose()
# pose.pose.position.z = 0.5
quat = [
pose.pose.orientation.x, pose.pose.orientation.y,
pose.pose.orientation.z, pose.pose.orientation.w
]
current_euler = euler_from_quaternion(quat)
pose.pose.orientation = ManipulatorActions.getGraspOrientation(
0,
0, (current_euler[2] / math.pi * 180 + 180) % 360,
return_msg=True)
from moveit_msgs.msg import Constraints, PositionConstraint
eef_on_line = PositionConstraint()
eef_on_line.header.frame_id = 'base_link'
eef_on_line.link_name = mg.get_end_effector_link()
eef_on_line.target_point_offset.x = 0.0
eef_on_line.target_point_offset.y = 0.0
eef_on_line.target_point_offset.z = 0.0
eef_on_line.weight = 1
tolerance = 0.2
line = SolidPrimitive()
line.type = SolidPrimitive.BOX
line.dimensions.append(0.1)
line.dimensions.append(0.1)
line.dimensions.append(0.1)
eef_on_line.constraint_region.primitives.append(line)
box_pose = mg.get_current_pose().pose
box_pose.orientation = Quaternion()
box_pose.orientation.w = 1
eef_on_line.constraint_region.primitive_poses.append(box_pose)
flip_ct = Constraints()
flip_ct.position_constraints.append(eef_on_line)
mg.set_path_constraints(flip_ct)
mg.set_pose_target(pose)
plan = mg.plan()
if len(plan.joint_trajectory.points) > 1:
mg.execute(plan)
return True, 'rotate successfull'
else:
return False, 'rotate not successfull'
pass
def _to_pose_constraint(pose, reference_frame, link_name, position_tolerance=_DEFAULT_POSITION_TOLERANCE):
"""Returns an position constraint suitable for ActionGoal's."""
pos_con = PositionConstraint()
pos_con.header.frame_id = reference_frame
pos_con.link_name = link_name
pos_con.constraint_region.primitive_poses.append(pose)
pos_con.weight = 1
region = shape_msgs.SolidPrimitive()
region.type = shape_msgs.SolidPrimitive.SPHERE
region.dimensions.append(position_tolerance)
pos_con.constraint_region.primitives.append(region)
return pos_con
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 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 create_move_group_pose_goal(self,
goal_pose=Pose(),
group="right_arm",
end_link_name="arm_right_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 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 create_move_group_pose_goal(self,
goal_pose=Pose(),
group="manipulator",
end_link_name=None,
plan_only=True):
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
if end_link_name != None:
position_c.link_name = end_link_name
position_c.constraint_region.primitives.append(
SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.1]))
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 = 0.01
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 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 left_arm_go_to_pose_goal(self):
# 设置动作对象变量,此处为arm
left_arm = self.left_arm
# 获取当前末端执行器位置姿态
pose_goal = left_arm.get_current_pose().pose
# 限制末端夹具运动
left_joint_const = JointConstraint()
left_joint_const.joint_name = "gripper_l_joint_r"
if Leftfinger > -32:
left_joint_const.position = 0.024
else:
left_joint_const.position = 0
left_joint_const.weight = 1.0
# 限制末端位移
left_position_const = PositionConstraint()
left_position_const.header = Header()
left_position_const.link_name = "gripper_l_joint_r"
left_position_const.target_point_offset.x = 0.5
left_position_const.target_point_offset.y = -0.5
left_position_const.target_point_offset.z = 1.0
left_position_const.weight = 1.0
# # 限制1轴转动
left_joint1_const = JointConstraint()
left_joint1_const.joint_name = "yumi_joint_1_l"
left_joint1_const.position = 0
left_joint1_const.tolerance_above = 1.76
left_joint1_const.tolerance_below = 0
left_position_const.weight = 1.0
# 限制2轴转动
left_joint2_const = JointConstraint()
left_joint2_const.joint_name = "yumi_joint_2_l"
left_joint2_const.position = 0
left_joint2_const.tolerance_above = 0
left_joint2_const.tolerance_below = 150
left_joint2_const.weight = 1.0
# 限制3轴转动
left_joint3_const = JointConstraint()
left_joint3_const.joint_name = "yumi_joint_3_l"
left_joint3_const.position = 0
left_joint3_const.tolerance_above = 35
left_joint3_const.tolerance_below = 55
left_joint3_const.weight = 1.0
# 限制4轴转动
left_joint4_const = JointConstraint()
left_joint4_const.joint_name = "yumi_joint_4_l"
left_joint4_const.position = 0
left_joint4_const.tolerance_above = 60
left_joint4_const.tolerance_below = 75
left_joint4_const.weight = 1.0
# 限制5轴转动
right_joint5_const = JointConstraint()
right_joint5_const.joint_name = "yumi_joint_5_l"
right_joint5_const.position = 40
right_joint5_const.tolerance_above = 50
right_joint5_const.tolerance_below = 20
right_joint5_const.weight = 1.0
# 限制6轴转动
left_joint6_const = JointConstraint()
left_joint6_const.joint_name = "yumi_joint_6_l"
left_joint6_const.position = 0
left_joint6_const.tolerance_above = 10
left_joint6_const.tolerance_below = 35
left_joint6_const.weight = 1.0
# 限制7轴转动
left_joint7_const = JointConstraint()
left_joint7_const.joint_name = "yumi_joint_7_l"
left_joint7_const.position = -10
left_joint7_const.tolerance_above = 0
left_joint7_const.tolerance_below = 160
left_joint7_const.weight = 1.0
# 限制末端位移
left_position_const = PositionConstraint()
left_position_const.header = Header()
left_position_const.link_name = "gripper_l_joint_r"
left_position_const.target_point_offset.x = 0.5
left_position_const.target_point_offset.y = 0.25
left_position_const.target_point_offset.z = 0.5
left_position_const.weight = 1.0
# 添加末端姿态约束:
left_orientation_const = OrientationConstraint()
left_orientation_const.header = Header()
left_orientation_const.orientation = pose_goal.orientation
left_orientation_const.link_name = "gripper_l_finger_r"
left_orientation_const.absolute_x_axis_tolerance = 0.5
left_orientation_const.absolute_y_axis_tolerance = 0.25
left_orientation_const.absolute_z_axis_tolerance = 0.5
left_orientation_const.weight = 1
# 施加全约束
consts = Constraints()
consts.joint_constraints = [left_joint_const]
# consts.orientation_constraints = [left_orientation_const]
# consts.position_constraints = [left_position_const]
left_arm.set_path_constraints(consts)
# 设置动作对象目标位置姿态
pose_goal.orientation.x = Left_Qux
pose_goal.orientation.y = Left_Quy
pose_goal.orientation.z = Left_Quz
pose_goal.orientation.w = Left_Quw
pose_goal.position.x = (Neurondata[11] - 0.05) * 1.48 + 0.053
pose_goal.position.y = (Neurondata[9] - 0.18) * 1.48 + 0.12
pose_goal.position.z = (Neurondata[10] - 0.53) * 1.48 + 0.47
left_arm.set_pose_target(pose_goal)
print "End effector pose %s" % pose_goal
# # 设置动作对象目标位置姿态
# pose_goal.orientation.x = pose_goal.orientation.x
# pose_goal.orientation.y = pose_goal.orientation.y
# pose_goal.orientation.z = pose_goal.orientation.z
# pose_goal.orientation.w = pose_goal.orientation.w
# pose_goal.position.x = pose_goal.position.x
# pose_goal.position.y = pose_goal.position.y - 0.01
# pose_goal.position.z = pose_goal.position.z
# left_arm.set_pose_target(pose_goal)
# print "End effector pose %s" % pose_goal
# 规划和输出动作
traj = left_arm.plan()
left_arm.execute(traj, wait=False)
# 动作完成后清除目标信息
left_arm.clear_pose_targets()
# 清除路径约束
left_arm.clear_path_constraints()
# 确保没有剩余未完成动作在执行
left_arm.stop()
def liftgripper(self, target_z=0.10):
# 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
group = self.group
while self.target_location_x == -100:
rospy.sleep(0.05)
current_pose = group.get_current_pose().pose
current_orientation = group.get_current_pose().pose.orientation
curr_q = [
current_orientation.x, current_orientation.y,
current_orientation.z, current_orientation.w
]
position_constraint = PositionConstraint()
position_constraint.target_point_offset.x = 0.1
position_constraint.target_point_offset.y = 0.2
position_constraint.target_point_offset.z = 0.3
position_constraint.weight = 0.8
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=curr_q[0],
y=curr_q[1],
z=curr_q[2],
w=curr_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.2
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 = True
planning_time = 5
threshold = 0.02
time_limit = 120
start_time = time.time()
target_x = current_pose.position.x
target_y = current_pose.position.y
delta_z = (target_z - current_pose.position.z) / 4
print "Attempting to lift gripper"
i = 1
# if (delta_z < threshold):
# return True
# while (current_pose.position.z < target_z) and ((target_z - current_pose.position.z) > 3*threshold) and ((time.time()-start_time) < time_limit):
# status = self.go_to_pose_goal(curr_q[0], curr_q[1], curr_q[2], curr_q[3],
# target_x, target_y, current_pose.position.z + i*delta_z,
# allow_replanning, planning_time, thresh=threshold)
# rospy.sleep(0.1)
# current_pose = group.get_current_pose().pose
# i += 1
status = self.go_to_pose_goal(curr_q[0],
curr_q[1],
curr_q[2],
curr_q[3],
target_x,
target_y,
target_z,
allow_replanning,
planning_time,
thresh=threshold)
rospy.sleep(0.25)
print "Successfully lifted gripper to z: ", current_pose.position.z
group.clear_path_constraints()
# return ((target_z-current_pose.position.z) < 3*threshold or (current_pose.position.z > target_z))
return True
def flip(cls):
'''
rotate gripper 180 degree about y
:return:
'''
# return ManipulatorActions.flip_kinova()
mg = ManipulatorActions.get_move_group()
ManipulatorActions.with_planning_scene()
pose = mg.get_current_pose()
pose.pose.position.z = 0.4
quat = [
pose.pose.orientation.x, pose.pose.orientation.y,
pose.pose.orientation.z, pose.pose.orientation.w
]
current_euler = euler_from_quaternion(quat)
pose.pose.orientation = ManipulatorActions.getGraspOrientation(
0, 100, 0, return_msg=True)
mg.set_pose_target(pose)
from moveit_msgs.msg import Constraints, PositionConstraint
eef_on_line = PositionConstraint()
eef_on_line.header.frame_id = 'base_link'
eef_on_line.link_name = mg.get_end_effector_link()
eef_on_line.target_point_offset.x = 0.0
eef_on_line.target_point_offset.y = 0.0
eef_on_line.target_point_offset.z = 0.0
eef_on_line.weight = 1
tolerance = 0.2
line = SolidPrimitive()
line.type = SolidPrimitive.SPHERE
line.dimensions.append(0.5)
# line.dimensions.append(2.1)
# line.dimensions.append(2.1)
eef_on_line.constraint_region.primitives.append(line)
box_pose = mg.get_current_pose().pose
box_pose.orientation = Quaternion()
box_pose.orientation.w = 1
eef_on_line.constraint_region.primitive_poses.append(box_pose)
eef_on_line.header.frame_id = mg.get_planning_frame()
flip_ct = Constraints()
flip_ct.position_constraints.append(eef_on_line)
# mg.set_path_constraints(flip_ct)
goal_tol = mg.get_goal_position_tolerance()
goal_tol_rot = mg.get_goal_orientation_tolerance()
mg.set_goal_position_tolerance(0.3)
rospy.sleep(0.5)
mg.set_goal_orientation_tolerance(0.3)
mg.set_planning_time(10.0)
rospy.sleep(0.5)
plan = mg.plan()
mg.set_goal_position_tolerance(goal_tol)
mg.set_goal_orientation_tolerance(goal_tol_rot)
mg.set_planning_time(5.0)
if len(plan.joint_trajectory.points) > 1:
mg.execute(plan)
return True, 'flip successful'
else:
return False, 'flip failed'
pass
def right_arm_go_to_pose_goal(self):
# 设置动作对象变量,此处为arm
right_arm = self.right_arm
# 获取当前末端执行器位置姿态
pose_goal = right_arm.get_current_pose().pose
# 限制末端夹具运动
right_joint_const = JointConstraint()
right_joint_const.joint_name = "gripper_r_joint_r"
if Rightfinger > -55:
right_joint_const.position = 0.024
else:
right_joint_const.position = 0
right_joint_const.weight = 1.0
# 限制1轴转动
right_joint1_const = JointConstraint()
right_joint1_const.joint_name = "yumi_joint_1_r"
right_joint1_const.position = 0
right_joint1_const.tolerance_above = 120
right_joint1_const.tolerance_below = 0
right_joint1_const.weight = 1.0
# 限制2轴转动
right_joint2_const = JointConstraint()
right_joint2_const.joint_name = "yumi_joint_2_r"
right_joint2_const.position = 0
right_joint2_const.tolerance_above = 0
right_joint2_const.tolerance_below = 150
right_joint2_const.weight = 1.0
# 限制3轴转动
right_joint3_const = JointConstraint()
right_joint3_const.joint_name = "yumi_joint_3_r"
right_joint3_const.position = 0
right_joint3_const.tolerance_above = 35
right_joint3_const.tolerance_below = 55
right_joint3_const.weight = 1.0
# 限制4轴转动
right_joint4_const = JointConstraint()
right_joint4_const.joint_name = "yumi_joint_4_r"
right_joint4_const.position = 0
right_joint4_const.tolerance_above = 60
right_joint4_const.tolerance_below = 75
right_joint4_const.weight = 1.0
# 限制5轴转动
right_joint5_const = JointConstraint()
right_joint5_const.joint_name = "yumi_joint_5_r"
right_joint5_const.position = 40
right_joint5_const.tolerance_above = 50
right_joint5_const.tolerance_below = 20
right_joint5_const.weight = 1.0
# 限制6轴转动
right_joint6_const = JointConstraint()
right_joint6_const.joint_name = "yumi_joint_6_r"
right_joint6_const.position = 0
right_joint6_const.tolerance_above = 10
right_joint6_const.tolerance_below = 35
right_joint6_const.weight = 1.0
# 限制7轴转动
right_joint7_const = JointConstraint()
right_joint7_const.joint_name = "yumi_joint_7_r"
right_joint7_const.position = -10
right_joint7_const.tolerance_above = 0
right_joint7_const.tolerance_below = 160
right_joint7_const.weight = 1.0
# 限制末端位移
right_position_const = PositionConstraint()
right_position_const.header = Header()
right_position_const.link_name = "gripper_r_joint_r"
right_position_const.target_point_offset.x = 0.5
right_position_const.target_point_offset.y = -0.5
right_position_const.target_point_offset.z = 1.0
right_position_const.weight = 1.0
# 添加末端姿态约束:
right_orientation_const = OrientationConstraint()
right_orientation_const.header = Header()
right_orientation_const.orientation = pose_goal.orientation
right_orientation_const.link_name = "gripper_r_finger_r"
right_orientation_const.absolute_x_axis_tolerance = 0.50
right_orientation_const.absolute_y_axis_tolerance = 0.25
right_orientation_const.absolute_z_axis_tolerance = 0.50
right_orientation_const.weight = 100
# 施加全约束
consts = Constraints()
consts.joint_constraints = [right_joint_const]
# consts.orientation_constraints = [right_orientation_const]
# consts.position_constraints = [right_position_const]
right_arm.set_path_constraints(consts)
# 设置动作对象目标位置姿态
pose_goal.orientation.x = Right_Qux
pose_goal.orientation.y = Right_Quy
pose_goal.orientation.z = Right_Quz
pose_goal.orientation.w = Right_Quw
pose_goal.position.x = (Neurondata[5] - 0.05) * 1.48 + 0.053
pose_goal.position.y = (Neurondata[3] + 0.18) * 1.48 - 0.12
pose_goal.position.z = (Neurondata[4] - 0.53) * 1.48 + 0.47
right_arm.set_pose_target(pose_goal)
print "End effector pose %s" % pose_goal
# # 设置动作对象目标位置姿态
# pose_goal.orientation.x = 0.1644
# pose_goal.orientation.y = 0.3111
# pose_goal.orientation.z = 0.9086
# pose_goal.orientation.w = 0.2247
# pose_goal.position.x = (Neurondata[5]-0.05)*1.48+0.053
# pose_goal.position.y = (Neurondata[3]+0.18)*1.48-0.12
# pose_goal.position.z = (Neurondata[4]-0.53)*1.48+0.47
# right_arm.set_pose_target(pose_goal)
# print "End effector pose %s" % pose_goal
# # 设置动作对象目标位置姿态
# pose_goal.orientation.x = pose_goal.orientation.x
# pose_goal.orientation.y = pose_goal.orientation.y
# pose_goal.orientation.z = pose_goal.orientation.z
# pose_goal.orientation.w = pose_goal.orientation.w
# pose_goal.position.x = pose_goal.position.x
# pose_goal.position.y = pose_goal.position.y - 0.01
# pose_goal.position.z = pose_goal.position.z
# right_arm.set_pose_target(pose_goal)
# print "End effector pose %s" % pose_goal
# 规划和输出动作
traj = right_arm.plan()
right_arm.execute(traj, wait=False)
# 动作完成后清除目标信息
right_arm.clear_pose_targets()
# 清除路径约束
right_arm.clear_path_constraints()
# 确保没有剩余未完成动作在执行
right_arm.stop()
def deliver_sample(move_arm, args):
"""
:type move_arm: class 'moveit_commander.move_group.MoveGroupCommander'
:type args: List[bool, float, float, float]
"""
move_arm.set_planner_id("RRTstar")
x_delivery = args[1]
y_delivery = args[2]
z_delivery = args[3]
# after sample collect
mypi = 3.14159
d2r = mypi / 180
r2d = 180 / mypi
goal_pose = move_arm.get_current_pose().pose
# position was found from rviz tool
goal_pose.position.x = x_delivery
goal_pose.position.y = y_delivery
goal_pose.position.z = z_delivery
r = -179
p = -20
y = -90
q = quaternion_from_euler(r * d2r, p * d2r, y * d2r)
goal_pose.orientation = Quaternion(q[0], q[1], q[2], q[3])
move_arm.set_pose_target(goal_pose)
plan = move_arm.plan()
if len(plan.joint_trajectory.points) == 0: # If no plan found, abort
return False
plan = move_arm.go(wait=True)
move_arm.stop()
# rotate scoop to deliver sample at current location...
# adding position constraint on the solution so that the tip doesnot diverge to get to the solution.
pos_constraint = PositionConstraint()
pos_constraint.header.frame_id = "base_link"
pos_constraint.link_name = "l_scoop"
pos_constraint.target_point_offset.x = 0.1
pos_constraint.target_point_offset.y = 0.1
# rotate scoop to deliver sample at current location begin
pos_constraint.target_point_offset.z = 0.1
pos_constraint.constraint_region.primitives.append(
SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01]))
pos_constraint.weight = 1
# using euler angles for own verification..
r = +180
p = 90 # 45 worked get
y = -90
q = quaternion_from_euler(r * d2r, p * d2r, y * d2r)
goal_pose = move_arm.get_current_pose().pose
rotation = (goal_pose.orientation.x, goal_pose.orientation.y,
goal_pose.orientation.z, goal_pose.orientation.w)
euler_angle = euler_from_quaternion(rotation)
goal_pose.orientation = Quaternion(q[0], q[1], q[2], q[3])
move_arm.set_pose_target(goal_pose)
plan = move_arm.plan()
if len(plan.joint_trajectory.points) == 0: # If no plan found, abort
return False
plan = move_arm.go(wait=True)
move_arm.stop()
move_arm.clear_pose_targets()
move_arm.set_planner_id("RRTconnect")
return True
def on_enter(self, userdata):
self._planning_failed = False
self._control_failed = False
self._success = False
# TODO check userdata
# if not isinstance(userdata.pose, PoseStamped):
#Logger.logwarn('userdata.pose must be geomery_msgs.msg.PoseStamped. `%s` received' % str(type(userdata.pose)))
#self._planning_failed = True
#return
check_type('pose', 'geometry_msgs/PoseStamped', userdata.pose)
# request planing and execution
action_goal = MoveGroupGoal()
# set palnning options
action_goal.planning_options.plan_only = self._plan_only
action_goal.planning_options.replan = False
# action_goal.planning_options.planning_scene_diff.is_diff = True
# action_goal.planning_options.planning_scene_diff.robot_state.is_diff = True
# setup request
action_goal.request.group_name = self._move_group
action_goal.request.num_planning_attempts = 3
action_goal.request.allowed_planning_time = 1.0
action_goal.request.max_velocity_scaling_factor = 1.0
action_goal.request.max_acceleration_scaling_factor = 1.0
# start pose
action_goal.request.start_state.is_diff = True
# pose constraint
goal_constraint = Constraints(name='')
# set target position
constraint = PositionConstraint()
constraint.header = Header(frame_id=userdata.pose.header.frame_id)
constraint.link_name = self._end_effector
constraint.constraint_region = BoundingVolume()
constraint.constraint_region.primitives = [
SolidPrimitive(type=SolidPrimitive.SPHERE,
dimensions=[self._position_tolerance])
]
constraint.constraint_region.primitive_poses = [
Pose(position=userdata.pose.pose.position)
]
constraint.weight = 1.0
goal_constraint.position_constraints.append(constraint)
# set target orientation
constraint = OrientationConstraint()
constraint.header = Header(frame_id=userdata.pose.header.frame_id)
constraint.link_name = self._end_effector
constraint.orientation = userdata.pose.pose.orientation
constraint.absolute_x_axis_tolerance = self._orientation_tolerance
constraint.absolute_y_axis_tolerance = self._orientation_tolerance
constraint.absolute_z_axis_tolerance = self._orientation_tolerance
constraint.weight = 0.1
goal_constraint.orientation_constraints.append(constraint)
# add goal_constraint
action_goal.request.goal_constraints.append(goal_constraint)
try:
self._client.send_goal('move_group', action_goal)
except Exception as e:
Logger.logwarn(
'MoveitToPose: Failed to send MoveGroupAction goal for group: %s\n%s'
% (self._move_group, str(e)))
self._planning_failed = True
def dip_incrementally(self, z_pose):
# time limited and incremental movement
group = self.group
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.3
position_constraint.weight = 0.75
position_constraint.link_name = group.get_end_effector_link()
position_constraint.header.frame_id = group.get_planning_frame()
constraint = Constraints()
constraint.position_constraints.append(position_constraint)
group.set_path_constraints(constraint)
current_pose = group.get_current_pose().pose
current_orientation = group.get_current_pose().pose.orientation
curr_q = [
current_orientation.x, current_orientation.y,
current_orientation.z, current_orientation.w
]
allow_replanning = False
planning_time = 5
print "Now performing dip"
tolerance = 0.01
time_limit = 120
start_time = time.time()
offset_x = 0.0
target_x = current_pose.position.x - offset_x
status = self.go_to_pose_goal(curr_q[0],
curr_q[1],
curr_q[2],
curr_q[3],
target_x,
current_pose.position.y,
current_pose.position.z,
allow_replanning,
planning_time,
thresh=tolerance)
while (current_pose.position.z - z_pose) > 2 * tolerance and (
time.time() - start_time) < time_limit:
status = self.go_to_pose_goal(curr_q[0],
curr_q[1],
curr_q[2],
curr_q[3],
target_x,
current_pose.position.y,
current_pose.position.z - 0.01,
allow_replanning,
planning_time,
thresh=tolerance)
current_pose = group.get_current_pose().pose
rospy.sleep(0.1)
group.clear_path_constraints()
rospy.sleep(0.05)
dip = (current_pose.position.z - z_pose) < 2 * tolerance
print("dip_incrementally: dip successful? " + str(dip))
return dip
def right_arm_go_to_pose_goal(self):
# 设置动作对象变量,此处为arm
right_arm = self.right_arm
# 获取当前末端执行器位置姿态
pose_goal = right_arm.get_current_pose().pose
# 限制末端夹具运动
right_joint_const = JointConstraint()
right_joint_const.joint_name = "gripper_r_joint_r"
right_joint_const.position = 0
right_joint_const.weight = 1.0
# 限制1轴转动
right_joint1_const = JointConstraint()
right_joint1_const.joint_name = "yumi_joint_1_r"
right_joint1_const.position = 0
right_joint1_const.tolerance_above = 120
right_joint1_const.tolerance_below = 0
right_joint1_const.weight = 1.0
# 限制2轴转动
right_joint2_const = JointConstraint()
right_joint2_const.joint_name = "yumi_joint_2_r"
right_joint2_const.position = 0
right_joint2_const.tolerance_above = 0
right_joint2_const.tolerance_below = 150
right_joint2_const.weight = 1.0
# 限制3轴转动
right_joint3_const = JointConstraint()
right_joint3_const.joint_name = "yumi_joint_3_r"
right_joint3_const.position = 0
right_joint3_const.tolerance_above = 35
right_joint3_const.tolerance_below = 55
right_joint3_const.weight = 1.0
# 限制4轴转动
right_joint4_const = JointConstraint()
right_joint4_const.joint_name = "yumi_joint_4_r"
right_joint4_const.position = 0
right_joint4_const.tolerance_above = 60
right_joint4_const.tolerance_below = 75
right_joint4_const.weight = 1.0
# 限制5轴转动
right_joint5_const = JointConstraint()
right_joint5_const.joint_name = "yumi_joint_5_r"
right_joint5_const.position = 40
right_joint5_const.tolerance_above = 50
right_joint5_const.tolerance_below = 20
right_joint5_const.weight = 1.0
# 限制6轴转动
right_joint6_const = JointConstraint()
right_joint6_const.joint_name = "yumi_joint_6_r"
right_joint6_const.position = 0
right_joint6_const.tolerance_above = 10
right_joint6_const.tolerance_below = 35
right_joint6_const.weight = 1.0
# 限制7轴转动
right_joint7_const = JointConstraint()
right_joint7_const.joint_name = "yumi_joint_7_r"
right_joint7_const.position = -10
right_joint7_const.tolerance_above = 0
right_joint7_const.tolerance_below = 160
right_joint7_const.weight = 1.0
# 限制末端位移
right_position_const = PositionConstraint()
right_position_const.header = Header()
right_position_const.link_name = "gripper_r_joint_r"
right_position_const.target_point_offset.x = 0.5
right_position_const.target_point_offset.y = -0.5
right_position_const.target_point_offset.z = 1.0
right_position_const.weight = 1.0
# 添加末端姿态约束:
right_orientation_const = OrientationConstraint()
right_orientation_const.header = Header()
right_orientation_const.orientation = pose_goal.orientation
right_orientation_const.link_name = "gripper_r_finger_r"
right_orientation_const.absolute_x_axis_tolerance = 0.50
right_orientation_const.absolute_y_axis_tolerance = 0.25
right_orientation_const.absolute_z_axis_tolerance = 0.50
right_orientation_const.weight = 100
# 施加全约束
consts = Constraints()
consts.joint_constraints = [right_joint_const]
# consts.orientation_constraints = [right_orientation_const]
# consts.position_constraints = [right_position_const]
right_arm.set_path_constraints(consts)
# # 设置动作对象目标位置姿态
# pose_goal.orientation.x = Right_Qux
# pose_goal.orientation.y = Right_Quy
# pose_goal.orientation.z = Right_Quz
# pose_goal.orientation.w = Right_Quw
# pose_goal.position.x = (Neurondata[5]-0.05)*1.48+0.053
# pose_goal.position.y = (Neurondata[3]+0.18)*1.48-0.12
# pose_goal.position.z = (Neurondata[4]-0.53)*1.48+0.47
# right_arm.set_pose_target(pose_goal)
# print "End effector pose %s" % pose_goal
global n
# 设置动作对象目标位置姿态
if n == 1:
pose_goal.orientation.x = -0.00825
pose_goal.orientation.y = 0.03556
pose_goal.orientation.z = 0.79932
pose_goal.orientation.w = 0.5998
pose_goal.position.x = 0.54425
pose_goal.position.y = -0.2208
pose_goal.position.z = 0.14822
elif n == 2:
pose_goal.orientation.x = -0.00507
pose_goal.orientation.y = -0.012593
pose_goal.orientation.z = 0.98844
pose_goal.orientation.w = 0.15101
pose_goal.position.x = 0.55198
pose_goal.position.y = -0.06859
pose_goal.position.z = 0.17909
elif n == 3:
pose_goal.orientation.x = -0.05578
pose_goal.orientation.y = 0.025377
pose_goal.orientation.z = 0.99365
pose_goal.orientation.w = -0.095267
pose_goal.position.x = 0.36478
pose_goal.position.y = -0.05781
pose_goal.position.z = 0.15907
n = 0
right_arm.set_pose_target(pose_goal)
print "End effector pose %s" % pose_goal
# 规划和输出动作
traj = right_arm.plan()
right_arm.execute(traj, wait=False)
# 动作完成后清除目标信息
right_arm.clear_pose_targets()
# 清除路径约束
right_arm.clear_path_constraints()
# 确保没有剩余未完成动作在执行
right_arm.stop()
def deliver_sample(move_arm, robot, moveit_fk, args):
"""
:type move_arm: class 'moveit_commander.move_group.MoveGroupCommander'
:type args: List[bool, float, float, float]
"""
move_arm.set_planner_id("RRTstar")
robot_state = robot.get_current_state()
move_arm.set_start_state(robot_state)
x_delivery = args.delivery.x
y_delivery = args.delivery.y
z_delivery = args.delivery.z
# after sample collect
mypi = 3.14159
d2r = mypi / 180
r2d = 180 / mypi
goal_pose = move_arm.get_current_pose().pose
# position was found from rviz tool
goal_pose.position.x = x_delivery
goal_pose.position.y = y_delivery
goal_pose.position.z = z_delivery
r = -179
p = -20
y = -90
q = quaternion_from_euler(r * d2r, p * d2r, y * d2r)
goal_pose.orientation = Quaternion(q[0], q[1], q[2], q[3])
move_arm.set_pose_target(goal_pose)
_, plan_a, _, _ = move_arm.plan()
if len(plan_a.joint_trajectory.points) == 0: # If no plan found, abort
return False
# rotate scoop to deliver sample at current location...
# adding position constraint on the solution so that the tip doesnot diverge to get to the solution.
pos_constraint = PositionConstraint()
pos_constraint.header.frame_id = "base_link"
pos_constraint.link_name = "l_scoop"
pos_constraint.target_point_offset.x = 0.1
pos_constraint.target_point_offset.y = 0.1
# rotate scoop to deliver sample at current location begin
pos_constraint.target_point_offset.z = 0.1
pos_constraint.constraint_region.primitives.append(
SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01]))
pos_constraint.weight = 1
# using euler angles for own verification..
r = +180
p = 90 # 45 worked get
y = -90
q = quaternion_from_euler(r * d2r, p * d2r, y * d2r)
cs, start_state, goal_pose = calculate_joint_state_end_pose_from_plan_arm(
robot, plan_a, move_arm, moveit_fk)
move_arm.set_start_state(cs)
goal_pose.orientation = Quaternion(q[0], q[1], q[2], q[3])
move_arm.set_pose_target(goal_pose)
_, plan_b, _, _ = move_arm.plan()
if len(plan_b.joint_trajectory.points
) == 0: # If no plan found, send the previous plan only
return plan_a
deliver_sample_traj = cascade_plans(plan_a, plan_b)
# move_arm.set_planner_id("RRTconnect")
return deliver_sample_traj
