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 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 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 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 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 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 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 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 freeze_orientation(self):
# get current pose
cur_pose = self._group.get_current_pose()
# create pose constraint
constraints = Constraints()
constraints.name = "orientation_constraints"
orientation_constraint = OrientationConstraint()
orientation_constraint.header = cur_pose.header
orientation_constraint.header.stamp = rospy.Time.now()
orientation_constraint.link_name = self._group.get_end_effector_link()
orientation_constraint.orientation = cur_pose.pose.orientation
orientation_constraint.absolute_x_axis_tolerance = 0.5
orientation_constraint.absolute_y_axis_tolerance = 0.5
orientation_constraint.absolute_z_axis_tolerance = 0.5
orientation_constraint.weight = 1.0
constraints.orientation_constraints.append(orientation_constraint)
self._group.set_path_constraints(constraints)
self.orientation_frozen = True
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 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="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 get_constraint(self,
euler_orientation=[0, math.pi / 2, 0],
tol=[3, 3, .5]):
#method takes euler-angle inputs, this converts it to a quaternion
q_orientation = tf.transformations.quaternion_from_euler(
euler_orientation[0], euler_orientation[1], euler_orientation[2])
orientation_msg = Quaternion(q_orientation[0], q_orientation[1],
q_orientation[2], q_orientation[3])
#defines a constraint that sets the end-effector so a cup in it's hand will be upright, or straight upside-down
constraint = Constraints()
constraint.name = 'upright_wrist'
upright_orientation = OrientationConstraint()
upright_orientation.link_name = self.group.get_end_effector_link()
upright_orientation.orientation = orientation_msg
upright_orientation.absolute_x_axis_tolerance = tol[0]
upright_orientation.absolute_y_axis_tolerance = tol[1]
upright_orientation.absolute_z_axis_tolerance = tol[2]
upright_orientation.weight = 1.0
upright_orientation.header = self.start_pose.header
constraint.orientation_constraints.append(upright_orientation)
return (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 = '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 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 __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 __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 constraint 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('right_arm_zero')
# 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('right_arm_zero')
# 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 __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 __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)
# 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_tolerance(0.00001)
# 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('right')
# Plan and execute a trajectory to the goal configuration
right_arm.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.4
target_pose.pose.position.y = 0.3
target_pose.pose.position.z = 0.4
target_pose.pose.orientation.x = 0.670820393249937
target_pose.pose.orientation.y = 0.223606797749979
target_pose.pose.orientation.z = 0.223606797749979
target_pose.pose.orientation.w = 0.670820393249937
# Set the start state and target pose, then plan and execute
right_arm.set_pose_target(target_pose, end_effector_link)
right_arm.go()
rospy.sleep(2)
# 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 = end_effector_link
orientation_constraint.absolute_x_axis_tolerance = 0.00001
orientation_constraint.absolute_y_axis_tolerance = 3.14
orientation_constraint.absolute_z_axis_tolerance = 0.00001
orientation_constraint.weight = 1.0
orientation_constraint.orientation = start_pose.pose.orientation
# 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 = deepcopy(start_pose)
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.4
target_pose.pose.position.y = -0.3
target_pose.pose.position.z = 0.4
target_pose.pose.orientation.x = 0.670820393249937
target_pose.pose.orientation.y = -0.223606797749979
target_pose.pose.orientation.z = -0.223606797749979
target_pose.pose.orientation.w = 0.670820393249937
#target_pose.pose.orientation.x = 0.707107
#target_pose.pose.orientation.y = 0
#target_pose.pose.orientation.z = 0
#target_pose.pose.orientation.w = 0.707107
# 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()
# Return to the "resting" configuration stored in the SRDF file
right_arm.set_named_target('up')
# 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 getOrientation(self, xyz):
pass
def print_pointlist(self, point_list):
# Get differential of way point list
differential = self.get_way_point_differential(point_list)
differential_num = [0]
differential_num = differential_num + differential
eef_rotation_change = [0, 0]
for i in range(2, len(differential_num)):
eef_rotation_change.append(differential_num[i] -
differential_num[i - 1])
pose = self.group.get_current_pose(self.group.get_end_effector_link())
constraints = Constraints()
#### joint constraints
joint_constraint = JointConstraint()
joint_constraint.joint_name = 'arm_joint_1'
joint_constraint.position = 169 * pi / 180
joint_constraint.tolerance_above = 30 * pi / 180
joint_constraint.tolerance_below = 30 * pi / 180
joint_constraint.weight = 1
constraints.joint_constraints.append(joint_constraint)
joint_constraint = JointConstraint()
joint_constraint.joint_name = 'arm_joint_4'
joint_constraint.position = 150 * pi / 180
joint_constraint.tolerance_above = 30 * pi / 180
joint_constraint.tolerance_below = 30 * pi / 180
joint_constraint.weight = 1
constraints.joint_constraints.append(joint_constraint)
self.group.set_path_constraints(constraints)
# Orientation constrains
# constraints = Constraints()
orientation_constraint = OrientationConstraint()
orientation_constraint.header = pose.header
orientation_constraint.link_name = self.group.get_end_effector_link()
orientation_constraint.orientation = pose.pose.orientation
orientation_constraint.absolute_x_axis_tolerance = 2 * pi
orientation_constraint.absolute_y_axis_tolerance = 2 * pi
orientation_constraint.absolute_z_axis_tolerance = 2 * pi
constraints.orientation_constraints.append(orientation_constraint)
while len(point_list) > 1:
# Move the robot point to first point and find the height
initial_plan = self.move_to_initial(point_list[1])
joint_goal = self.group.get_current_joint_values()
head = initial_plan.joint_trajectory.header
robot_state = self.robot.get_current_state()
# print(robot.get_current_state().joint_state.position)
robot_state.joint_state.position = tuple(initial_plan.joint_trajectory.points[-1].positions) + \
tuple(robot_state.joint_state.position[7:])
resp = self.request_fk(head, [self.group.get_end_effector_link()],
robot_state)
current_pose = self.group.get_current_pose().pose
current_pose.orientation = resp.pose_stamped[0].pose.orientation
(current_pose.position.x, current_pose.position.y,
current_pose.position.z) = point_list[0]
self.group.set_pose_target(current_pose)
self.group.go()
# Move the robot to the center of the striaght line to make sure Way point method can be executed
# Way points
waypoints = []
wpose = self.group.get_current_pose().pose
# Add the current pose to make sure the path is smooth
waypoints.append(copy.deepcopy(wpose))
success_num = 0
for point_num in range(len(point_list)):
(wpose.position.x, wpose.position.y,
wpose.position.z) = point_list[point_num]
(roll, pitch, yaw) = euler_from_quaternion([
wpose.orientation.x, wpose.orientation.y,
wpose.orientation.z, wpose.orientation.w
])
# [wpose.orientation.x, wpose.orientation.y, wpose.orientation.z, wpose.orientation.w] = \
# quaternion_from_euler(roll, pitch, yaw + eef_rotation_change[point_num])
# print(yaw + eef_rotation_change[point_num])
[wpose.orientation.x, wpose.orientation.y, wpose.orientation.z, wpose.orientation.w] = \
quaternion_from_euler(roll, pitch, yaw)
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = self.group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.00,
path_constraints=constraints)
if fraction == 1:
success_num += 1
execute_plan = plan
if success_num == len(point_list):
self.group.execute(execute_plan)
self.print_list_visualize(point_list)
elif success_num == 0:
break
else:
# execute success plan
self.msg_prit.data = True
self.group.execute(execute_plan)
self.print_list_visualize(point_list[:success_num])
break
self.msg_prit.data = False
# Delete the points what already execute
if success_num > 0:
del (point_list[0:success_num - 1])
del (eef_rotation_change[0:success_num - 1])
# Add obstacle after printing (need to revise)
self.group.set_path_constraints(None)
print 'all finish'
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 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 print_pointlist(self, point_list):
# Save original points list
all_point_list = point_list
pose = self.group.get_current_pose(self.group.get_end_effector_link())
constraints = Constraints()
last_ee_pose = self.group.get_current_pose().pose
#### joint constraints
joint_constraint = JointConstraint()
joint_constraint.joint_name = 'arm_joint_1'
joint_constraint.position = 169 * pi / 180
joint_constraint.tolerance_above = 30 * pi / 180
joint_constraint.tolerance_below = 30 * pi / 180
joint_constraint.weight = 1.0
constraints.joint_constraints.append(joint_constraint)
joint_constraint = JointConstraint()
joint_constraint.joint_name = 'arm_joint_4'
joint_constraint.position = 150 * pi / 180
joint_constraint.tolerance_above = 30 * pi / 180
joint_constraint.tolerance_below = 30 * pi / 180
joint_constraint.weight = 1.0
constraints.joint_constraints.append(joint_constraint)
self.group.set_path_constraints(constraints)
# Orientation constrains
orientation_constraint = OrientationConstraint()
orientation_constraint.header = pose.header
orientation_constraint.link_name = self.group.get_end_effector_link()
orientation_constraint.orientation = pose.pose.orientation
orientation_constraint.absolute_x_axis_tolerance = 2 * pi
orientation_constraint.absolute_y_axis_tolerance = 2 * pi
orientation_constraint.absolute_z_axis_tolerance = 2 * pi
orientation_constraint.weight = 1.0
constraints.orientation_constraints.append(orientation_constraint)
j = 0
# Record how many points has already finished
finsih_num = 0
print_num = 0
while len(point_list) > 1:
# Move the robot point to first point and find the height
initial_plan = self.move_to_initial(point_list[1])
# joint_goal = self.group.get_current_joint_values()
head = initial_plan.joint_trajectory.header
robot_state = self.robot.get_current_state()
# print(robot_state.joint_state)
robot_state.joint_state.position = tuple(initial_plan.joint_trajectory.points[-1].positions) + \
tuple(robot_state.joint_state.position[7:]) # the joints for the wheel
resp = self.request_fk(head, [self.group.get_end_effector_link()],
robot_state)
current_pose = self.group.get_current_pose().pose
current_pose.orientation = resp.pose_stamped[0].pose.orientation
(current_pose.position.x, current_pose.position.y,
current_pose.position.z) = point_list[0]
self.group.set_pose_target(current_pose)
self.group.go()
# Move the robot to the center of the striaght line to make sure Way point method can be executed
# Way points
waypoints = []
wpose = self.group.get_current_pose().pose
# Add the current pose to make sure the path is smooth
waypoints.append(copy.deepcopy(wpose))
success_num = 0
for point_num in range(len(point_list)):
(wpose.position.x, wpose.position.y,
wpose.position.z) = point_list[point_num]
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = self.group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.00,
path_constraints=constraints)
executing_state = 0
if fraction == 1:
success_num += 1
execute_plan = plan
if success_num == len(point_list):
self.group.execute(execute_plan, wait=False)
executing_state = 1
elif success_num == 0:
break
else:
# execute success plan
self.msg_print.data = True
self.group.execute(execute_plan, wait=False)
executing_state = 1
break
## 2nd loop
## always re-plan and check for obstacle while executing waypoints
executed_waypoint_index = 0 # initial value of nothing
if executing_state == 1:
success_planned_waypoint_array = np.delete(np.array(
point_list[:success_num]),
2,
axis=1)
print 'success planned waypoint\n', success_planned_waypoint_array
print 'status', self.waypoint_execution_status
while self.waypoint_execution_status != 3:
if self.waypoint_execution_status == 4:
# aborted state
print 'stop and abort waypoint execution'
self.group.stop()
executing_state = 0
current_ee_pose = self.group.get_current_pose().pose
self.printing_visualize((last_ee_pose.position.x, last_ee_pose.position.y, last_ee_pose.position.z), \
(current_ee_pose.position.x, current_ee_pose.position.y, current_ee_pose.position.z))
last_ee_pose = current_ee_pose
break
current_ee_pose = self.group.get_current_pose().pose
current_ee_position_array = np.array([
current_ee_pose.position.x, current_ee_pose.position.y
])
self.printing_visualize((last_ee_pose.position.x, last_ee_pose.position.y, last_ee_pose.position.z), \
(current_ee_pose.position.x, current_ee_pose.position.y, current_ee_pose.position.z))
last_ee_pose = current_ee_pose
executed_waypoint_index = self.check_executed_waypoint_index(
success_planned_waypoint_array,
current_ee_position_array)
# print 'last_ee', (last_ee_pose.position.x,last_ee_pose.position.y), \
# 'current_ee', (current_ee_pose.position.x,current_ee_pose.position.y), \
# 'executed latest index', executed_waypoint_index
# print(executed_waypoint_index)
# print 'current number', finsih_num + executed_waypoint_index, 'printing number', print_num
# if finsih_num + executed_waypoint_index - print_num <= 2 and finsih_num + executed_waypoint_index - print_num >=-2:
# self.print_future_visualize(all_point_list, print_num)
# print_num = finsih_num + executed_waypoint_index + 1
# # print(success_planned_waypoint_array.size)
# print('printing')
# print(finsih_num + executed_waypoint_index)
# next_future_ob_index = finsih_num + executed_waypoint_index
# if next_future_ob_index + 9 < len(all_point_list):
# self.future_visualize(all_point_list[next_future_ob_index + 8], all_point_list[next_future_ob_index + 9])
## Replan to check for dynamic obstacle
waypoints = []
# Add the current pose to make sure the path is smooth, get latest pose
current_ee_pose_smooth = self.group.get_current_pose().pose
waypoints.append(copy.deepcopy(current_ee_pose_smooth))
# discard the executed waypoints
new_point_list = point_list[executed_waypoint_index +
1:success_num]
# Add future printing obstacle
for k in new_point_list:
(current_ee_pose_smooth.position.x,
current_ee_pose_smooth.position.y,
current_ee_pose_smooth.position.z) = k
waypoints.append(copy.deepcopy(current_ee_pose_smooth))
(plan2, fraction2) = self.group.compute_cartesian_path(
waypoints, 0.01, 0.00, path_constraints=constraints)
if fraction2 < 1.0:
## new obstacle appear
# print 'executed latest index', executed_waypoint_index
# print 'fraction value', fraction,'\n'
print 'new obstacle appeared along the planned path'
self.group.stop()
executing_state = 0
break
j += 1
if j == 2:
self.scene.addBox('boxb', 0.5, 0.5, 0.5, 2.5, 0.5,
0.15)
rospy.sleep(0.01)
if self.waypoint_execution_status == 3:
# waypoint successfully printed
# self.print_list_visualize(point_list[:success_num])
# print 'status 3', point_list[:success_num]
del (point_list[0:success_num - 1])
finsih_num += success_num
elif self.waypoint_execution_status == 2 or 4:
# state 2 = preempted, state 4 = aborted.
# only printed partial waypoint
# self.print_list_visualize(point_list[:executed_waypoint_index+1])
# print 'status 4', point_list[:executed_waypoint_index+1]
del (point_list[:executed_waypoint_index + 1]
) # delete up till whatever is executed
finsih_num += executed_waypoint_index + 1
self.msg_print.data = False
self.group.set_path_constraints(None)
print 'all finish'
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
scene = PlanningSceneInterface('world')
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)
self.colors = dict()
max_pick_attempts = 10
max_place_attempts = 10
rospy.sleep(1)
arm = MoveGroupCommander('arm')
end_effector_link = arm.get_end_effector_link()
# arm.set_goal_position_tolerance(0.02)
# arm.set_goal_orientation_tolerance(0.03)
arm.allow_replanning(True)
arm.set_planning_time(5)
table_id = 'table'
side_id = 'side'
table2_id = 'table2'
box1_id = 'box1'
box2_id = 'box2'
target_id = 'target'
wall_id = 'wall'
ground_id = 'ground'
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(table_id)
scene.remove_world_object(table2_id)
scene.remove_world_object(target_id)
scene.remove_world_object(wall_id)
scene.remove_world_object(ground_id)
scene.remove_world_object(side_id)
rospy.sleep(1)
table_ground = 0.55
table_size = [0.2, 0.7, 0.01]
table2_size = [0.25, 0.6, 0.01]
box1_size = [0.02, 0.9, 0.6]
box2_size = [0.05, 0.05, 0.15]
target_size = [0.03, 0.06, 0.10]
wall_size = [0.01, 0.9, 0.6]
ground_size = [3, 3, 0.01]
side_size = [0.01, 0.7, table_ground]
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
self.setPose(table_pose,
[0.7, 0.0, table_ground + table_size[2] / 2.0])
scene.add_box(table_id, table_pose, table_size)
side_pose = PoseStamped()
side_pose.header.frame_id = REFERENCE_FRAME
self.setPose(side_pose, [0.605, 0.0, side_size[2] / 2.0])
scene.add_box(side_id, side_pose, side_size)
table2_pose = PoseStamped()
table2_pose.header.frame_id = REFERENCE_FRAME
self.setPose(table2_pose,
[-0.325, 0.0, box1_size[2] + table2_size[2] / 2.0])
# scene.add_box(table2_id, table2_pose, table2_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = REFERENCE_FRAME
self.setPose(box1_pose, [-0.36, 0.0, box1_size[2] / 2.0])
# scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = REFERENCE_FRAME
self.setPose(
box2_pose,
[0.63, -0.12, table_ground + table_size[2] + box2_size[2] / 2.0])
# scene.add_box(box2_id, box2_pose, box2_size)
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
self.setPose(
target_pose,
[0.62, 0.0, table_ground + table_size[2] + target_size[2] / 2.0])
scene.add_box(target_id, target_pose, target_size)
wall_pose = PoseStamped()
wall_pose.header.frame_id = REFERENCE_FRAME
self.setPose(
wall_pose,
[-0.405, 0.0, box1_size[2] + table2_size[2] + wall_size[2] / 2.0])
# scene.add_box(wall_id, wall_pose, wall_size)
ground_pose = PoseStamped()
ground_pose.header.frame_id = REFERENCE_FRAME
self.setPose(ground_pose, [0.0, 0.0, -ground_size[2] / 2.0])
# scene.add_box(ground_id, ground_pose, ground_size)
self.setColor(table_id, 0.8, 0.0, 0.0, 1.0)
# self.setColor(table2_id, 0.8, 0.0, 0.0)
# self.setColor(box1_id, 0.9, 0.9, 0.9)
# self.setColor(box2_id, 0.8, 0.4, 1.0)
self.setColor(target_id, 0.5, 0.4, 1.0)
# self.setColor(wall_id, 0.9, 0.9, 0.9)
self.setColor(ground_id, 0.3, 0.3, 0.3, 1.0)
self.setColor(side_id, 0.8, 0.0, 0.0, 1.0)
self.sendColors()
constraints = Constraints()
constraints.name = 'gripper constraint'
orientation_constraint = OrientationConstraint()
arm.set_support_surface_name(table_id)
test_pose = PoseStamped()
test_pose.header.frame_id = REFERENCE_FRAME
test_orientation = Quaternion()
test_orientation = quaternion_from_euler(0.0, 1.57, -3.14)
self.setPose(test_pose, [
-0.15, -0.22, box1_size[2] + table2_size[2] + target_size[2] / 2.0
], list(test_orientation))
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
self.setPose(
place_pose,
[0.62, -0.22, table_ground + table_size[2] + target_size[2] / 2.0])
grasp_pose = target_pose
grasp_init_orientation = Quaternion()
grasp_init_orientation = quaternion_from_euler(0.0, 1.57, 0.0)
grasp_pose.pose.orientation.x = grasp_init_orientation[0]
grasp_pose.pose.orientation.y = grasp_init_orientation[1]
grasp_pose.pose.orientation.z = grasp_init_orientation[2]
grasp_pose.pose.orientation.w = grasp_init_orientation[3]
grasp_pose.pose.position.x -= 0.02
rospy.loginfo('quaterion: ' + str(grasp_pose.pose.orientation))
place_pose.pose.orientation.x = grasp_init_orientation[0]
place_pose.pose.orientation.y = grasp_init_orientation[1]
place_pose.pose.orientation.z = grasp_init_orientation[2]
place_pose.pose.orientation.w = grasp_init_orientation[3]
orientation_constraint.header = grasp_pose.header
orientation_constraint.link_name = end_effector_link
orientation_constraint.orientation.x = grasp_init_orientation[0]
orientation_constraint.orientation.y = grasp_init_orientation[1]
orientation_constraint.orientation.z = grasp_init_orientation[2]
orientation_constraint.orientation.w = grasp_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)
grasps = self.make_grasps(grasp_pose, [table_id],
[0.05, 0.07, [1.0, 0.0, 0.0]],
[0.22, 0.26, [-1.0, 0.0, 0.0]])
result = None
n_attempts = 0
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
result = arm.pick(target_id, grasps)
n_attempts += 1
rospy.loginfo('Pick attempt:' + str(n_attempts))
rospy.sleep(0.2)
arm.set_path_constraints(constraints)
arm.set_pose_target(place_pose)
arm.go()
'''
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
places = self.make_places(place_pose, [table_id], [0.8, 0.13, [1.0, 0.0, 0.0]], [0.1, 0.12, [-1.0, 0.0, 0.0]])
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
for place in places:
result = arm.place(target_id, place)
if result == MoveItErrorCodes.SUCCESS:
break
n_attempts += 1
rospy.loginfo('Place attempt:' + str(n_attempts))
rospy.sleep(0.2)
'''
'''
grasp_pose = place_pose
grasp_pose.pose.orientation.x = grasp_init_orientation[0]
grasp_pose.pose.orientation.y = grasp_init_orientation[1]
grasp_pose.pose.orientation.z = grasp_init_orientation[2]
grasp_pose.pose.orientation.w = grasp_init_orientation[3]
grasp_pose.pose.position.x -= 0.02
grasps = self.make_grasps(grasp_pose, [table_id], [0.05, 0.07, [1.0, 0.0, 0.0]], [0.15, 0.20, [-1.0, 0.0, 0.0]])
result = None
n_attempts = 0
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
result = arm.pick(target_id, grasps)
n_attempts += 1
rospy.loginfo('Pick attempt:' + str(n_attempts))
rospy.sleep(0.2)
'''
'''
arm.set_support_surface_name(table2_id)
self.setPose(place_pose, [-0.26, -0.22, box1_size[2] + table2_size[2] + target_size[2]/2.0])
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
places = self.make_places(place_pose, [table2_id], [0.03, 0.06, [-1.0, 0.0, 0.0]], [0.1, 0.15, [1.0, 0.0, 0.0]])
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
for place in places:
result = arm.place(target_id, place)
if result == MoveItErrorCodes.SUCCESS:
break
n_attempts += 1
rospy.loginfo('Place attempt:' + str(n_attempts))
rospy.sleep(0.2)
'''
rospy.sleep(2)
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def both_arms_go_to_pose_goal(self):
# 设置动作对象变量,此处为both_arms
both_arms = self.both_arms
# 获取当前各轴转角
axis_angle = both_arms.get_current_joint_values()
# print axis_angle
# 获取当前末端执行器位置姿态
right_pose_goal = both_arms.get_current_pose(
end_effector_link="gripper_r_finger_r")
left_pose_goal = both_arms.get_current_pose(
end_effector_link="gripper_l_finger_r")
print right_pose_goal
# 限制末端夹具运动
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
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
# 添加右臂末端姿态约束:
right_orientation_const = OrientationConstraint()
right_orientation_const.header = Header()
right_orientation_const.orientation = right_pose_goal.pose.orientation
right_orientation_const.link_name = "gripper_r_joint_r"
right_orientation_const.absolute_x_axis_tolerance = 0.6
right_orientation_const.absolute_y_axis_tolerance = 0.6
right_orientation_const.absolute_z_axis_tolerance = 0.6
right_orientation_const.weight = 1
# 添加左臂末端姿态约束:
left_orientation_const = OrientationConstraint()
left_orientation_const.header = Header()
left_orientation_const.orientation = left_pose_goal.pose.orientation
left_orientation_const.link_name = "gripper_l_joint_r"
left_orientation_const.absolute_x_axis_tolerance = 0.6
left_orientation_const.absolute_y_axis_tolerance = 0.6
left_orientation_const.absolute_z_axis_tolerance = 0.6
left_orientation_const.weight = 1
# 施加全约束
consts = Constraints()
consts.joint_constraints = [right_joint_const, left_joint_const]
# consts.orientation_constraints = [right_orientation_const, left_orientation_const]
both_arms.set_path_constraints(consts)
# 右臂目标位姿设置
right_pose_goal.pose.orientation.x = Right_Qux
right_pose_goal.pose.orientation.y = Right_Quy
right_pose_goal.pose.orientation.z = Right_Quz
right_pose_goal.pose.orientation.w = Right_Quw
right_pose_goal.pose.position.x = (Neurondata[5] - 0.05) * 1.48 + 0.053
right_pose_goal.pose.position.y = (Neurondata[3] + 0.18) * 1.48 - 0.12
right_pose_goal.pose.position.z = (Neurondata[4] - 0.53) * 1.48 + 0.47
# 左臂目标位姿设置
left_pose_goal.pose.orientation.x = Left_Qux
left_pose_goal.pose.orientation.y = Left_Quy
left_pose_goal.pose.orientation.z = Left_Quz
left_pose_goal.pose.orientation.w = Left_Quw
left_pose_goal.pose.position.x = (Neurondata[11] - 0.05) * 1.48 + 0.053
left_pose_goal.pose.position.y = (Neurondata[9] - 0.18) * 1.48 + 0.12
left_pose_goal.pose.position.z = (Neurondata[10] - 0.53) * 1.48 + 0.47
# 设置动作组的两个目标点
both_arms.set_pose_target(right_pose_goal,
end_effector_link="gripper_r_finger_r")
both_arms.set_pose_target(left_pose_goal,
end_effector_link="gripper_l_finger_r")
# 规划和输出动作
traj = both_arms.plan()
both_arms.execute(traj, wait=False)
# # 清除路径约束
both_arms.clear_path_constraints()
# 确保输出停止
both_arms.stop()
def print_pointlist(self, point_list, future_print_status=False):
# Save original points list
full_point_list = copy.deepcopy(point_list)
full_point_array = np.delete(np.array(full_point_list), 2, axis=1)
if future_print_status:
self.print_future_visualize(full_point_list,
0,
status=self.future_printing_status)
self.future_printing_status = 1
# Initial the previous printing point
last_ee_pose = point_list[0]
# Constraints
pose = self.group.get_current_pose(self.group.get_end_effector_link())
constraints = Constraints()
# joint constraints
joint_constraint = JointConstraint()
joint_constraint.joint_name = 'arm_joint_1'
joint_constraint.position = 169 * pi / 180
joint_constraint.tolerance_above = 30 * pi / 180
joint_constraint.tolerance_below = 30 * pi / 180
joint_constraint.weight = 1.0
constraints.joint_constraints.append(joint_constraint)
joint_constraint = JointConstraint()
joint_constraint.joint_name = 'arm_joint_4'
joint_constraint.position = 150 * pi / 180
joint_constraint.tolerance_above = 30 * pi / 180
joint_constraint.tolerance_below = 30 * pi / 180
joint_constraint.weight = 1.0
constraints.joint_constraints.append(joint_constraint)
self.group.set_path_constraints(constraints)
# Orientation constrains
orientation_constraint = OrientationConstraint()
orientation_constraint.header = pose.header
orientation_constraint.link_name = self.group.get_end_effector_link()
orientation_constraint.orientation = pose.pose.orientation
orientation_constraint.absolute_x_axis_tolerance = 2 * pi
orientation_constraint.absolute_y_axis_tolerance = 2 * pi
orientation_constraint.absolute_z_axis_tolerance = 2 * pi
orientation_constraint.weight = 1.0
constraints.orientation_constraints.append(orientation_constraint)
# Record how many points has already finished
finsih_num = 0
print_num = 0
index_check = 0
while len(point_list) > 0:
print('New Plan, points left:', len(point_list), point_list)
# Move the robot point to first point and find the height
if len(point_list) > 1:
initial_plan = self.move_to_initial(point_list[1])
else:
initial_plan = self.move_to_initial(point_list[0])
# joint_goal = self.group.get_current_joint_values()
head = initial_plan.joint_trajectory.header
robot_state = self.robot.get_current_state()
# print(robot_state.joint_state)
robot_state.joint_state.position = tuple(initial_plan.joint_trajectory.points[-1].positions) + \
tuple(robot_state.joint_state.position[7:]) # the joints for the wheel
resp = self.request_fk(head, [self.group.get_end_effector_link()],
robot_state)
current_pose = self.group.get_current_pose().pose
current_pose.orientation = resp.pose_stamped[0].pose.orientation
(current_pose.position.x, current_pose.position.y,
current_pose.position.z) = point_list[0]
self.group.set_pose_target(current_pose)
self.group.go()
# Move the robot to the center of the striaght line to make sure Way point method can be executed
# Way points
waypoints = []
wpose = self.group.get_current_pose().pose
# Add the current pose to make sure the path is smooth
waypoints.append(copy.deepcopy(wpose))
success_num = 0
for point_num in range(len(point_list)):
(wpose.position.x, wpose.position.y,
wpose.position.z) = point_list[point_num]
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = self.group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.00,
path_constraints=constraints)
print('plan', fraction)
executing_state = 0
if fraction == 1:
success_num += 1
execute_plan = plan
if success_num == len(point_list):
self.group.execute(execute_plan, wait=True)
executing_state = 1
success_num += 1
elif success_num == 0:
break
else:
# execute success plan
self.msg_print.data = True
self.group.execute(execute_plan, wait=True)
executing_state = 1
break
# Delete the points what already execute
if success_num > 0:
del (point_list[0:success_num - 1])
## 2nd loop
## always re-plan and check for obstacle while executing waypoints
executed_waypoint_index = 0 # initial value of nothing
success_point_list = point_list[:success_num]
print('when plan success, move_group_status:',
self.move_group_execution_status)
if executing_state == 1:
# if len(full_point_list) != len(point_list): self.future_printing_status = 1
# print 'success planned waypoint\n', success_planned_waypoint_array
# print 'status', self.waypoint_execution_status
while self.waypoint_execution_status != 3 and len(
success_point_list) > 0:
print(len(success_point_list))
success_planned_waypoint_array = np.delete(
np.array(success_point_list), 2, axis=1)
if self.waypoint_execution_status == 4 or self.move_group_execution_status == 4:
# aborted state
print 'stop and abort waypoint execution'
self.group.stop()
executing_state = 0
self.group.clear_pose_targets()
break
# if self.waypoint_execution_status == 1:
# # Check for enclosure
# self.base_group.set_position_target([0, 0, 0.05], self.base_group.get_end_effector_link())
# result = self.base_group.plan()
# self.base_group.clear_pose_targets()
# if len(result.joint_trajectory.points) == 0:
# print('Check enclosure failed')
# self.group.stop()
# executing_state = 0
# break
# else: print('Check enclosure successful')
current_ee_pose = self.group.get_current_pose().pose
current_ee_position_array = np.array([
current_ee_pose.position.x, current_ee_pose.position.y
])
# Add current printing obstacle
# self.printing_visualize(last_ee_pose, (current_ee_pose.position.x, current_ee_pose.position.y, current_ee_pose.position.z),
# name = 'printing point')
# Update previous printing point
last_ee_pose = (current_ee_pose.position.x,
current_ee_pose.position.y,
current_ee_pose.position.z)
executed_waypoint_index = self.check_executed_waypoint_index(
success_planned_waypoint_array,
current_ee_position_array)
del (point_list[:executed_waypoint_index + 1])
del (success_point_list[:executed_waypoint_index + 1])
index_check = self.check_executed_waypoint_index(
full_point_array, current_ee_position_array)
print 'index check:', index_check, executed_waypoint_index
# print 'status:', self.waypoint_execution_status
if future_print_status:
if index_check >= self.further_printing_number:
self.print_future_visualize(
full_point_list,
index_check,
status=self.future_printing_status,
point_num=index_check -
self.further_printing_number)
self.further_printing_number = index_check
## Replan to check for dynamic obstacle
waypoints = []
# Add the current pose to make sure the path is smooth, get latest pose
current_ee_pose_smooth = self.group.get_current_pose().pose
waypoints.append(copy.deepcopy(current_ee_pose_smooth))
# discard the executed waypoints
new_point_list = copy.deepcopy(
success_point_list[executed_waypoint_index + 1:])
# Add future printing obstacle
for k in new_point_list:
(current_ee_pose_smooth.position.x,
current_ee_pose_smooth.position.y,
current_ee_pose_smooth.position.z) = k
waypoints.append(copy.deepcopy(current_ee_pose_smooth))
(plan2, fraction2) = self.group.compute_cartesian_path(
waypoints, 0.01, 0.00, path_constraints=constraints)
if fraction2 < 0.95:
## new obstacle appear
# print 'executed latest index', executed_waypoint_index
# print 'fraction value', fraction,'\n'
print 'new obstacle appeared along the planned path', 'fraction2:', fraction2
self.group.stop()
executing_state = 0
break
if self.waypoint_execution_status == 3:
# waypoint successfully printed
print 'status 3', executed_waypoint_index, point_list[:
success_num]
# del(point_list[0:executed_waypoint_index+1])
finsih_num += executed_waypoint_index + 1
elif self.waypoint_execution_status == 2 or 4:
# state 2 = preempted, state 4 = aborted.
print 'status 4', executed_waypoint_index, point_list[:
executed_waypoint_index
+ 1]
# del(point_list[:executed_waypoint_index+1]) # delete up till whatever is executed
finsih_num += executed_waypoint_index + 1
self.group.clear_pose_targets()
self.group.stop()
executing_state = 0
print('point list left:', len(point_list))
self.msg_print.data = False
self.group.set_path_constraints(None)
print 'all finish'
def both_arms_go_to_pose_goal(self):
# 设置动作对象变量,此处为both_arms
both_arms = self.both_arms
# 获取当前各轴转角
axis_angle = both_arms.get_current_joint_values()
# print axis_angle
# 获取当前末端执行器位置姿态
right_pose_goal = both_arms.get_current_pose(
end_effector_link="gripper_r_finger_r")
left_pose_goal = both_arms.get_current_pose(
end_effector_link="gripper_l_finger_r")
print right_pose_goal
# 限制末端夹具运动
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
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
# 添加末端姿态约束:
right_orientation_const = OrientationConstraint()
right_orientation_const.header = Header()
right_orientation_const.orientation = right_pose_goal.pose.orientation
right_orientation_const.link_name = "gripper_r_joint_r"
right_orientation_const.absolute_x_axis_tolerance = 0.6
right_orientation_const.absolute_y_axis_tolerance = 0.6
right_orientation_const.absolute_z_axis_tolerance = 0.6
right_orientation_const.weight = 1
left_orientation_const = OrientationConstraint()
left_orientation_const.header = Header()
left_orientation_const.orientation = left_pose_goal.pose.orientation
left_orientation_const.link_name = "gripper_l_joint_r"
left_orientation_const.absolute_x_axis_tolerance = 0.6
left_orientation_const.absolute_y_axis_tolerance = 0.6
left_orientation_const.absolute_z_axis_tolerance = 0.6
left_orientation_const.weight = 1
# 施加全约束
consts = Constraints()
consts.joint_constraints = [right_joint_const, left_joint_const]
# consts.orientation_constraints = [right_orientation_const, left_orientation_const]
both_arms.set_path_constraints(consts)
# # 设置动作对象目标位置姿态
# # 右臂
# right_pose_goal.pose.orientation.x = Right_Qux
# right_pose_goal.pose.orientation.y = Right_Quy
# right_pose_goal.pose.orientation.z = Right_Quz
# right_pose_goal.pose.orientation.w = Right_Quw
# right_pose_goal.pose.position.x = Neurondata[5]
# right_pose_goal.pose.position.y = Neurondata[3]
# right_pose_goal.pose.position.z = Neurondata[4]
# # 左臂
# left_pose_goal.pose.orientation.x = Left_Qux
# left_pose_goal.pose.orientation.y = Left_Quy
# left_pose_goal.pose.orientation.z = Left_Quz
# left_pose_goal.pose.orientation.w = Left_Quw
# left_pose_goal.pose.position.x = Neurondata[11]
# left_pose_goal.pose.position.y = Neurondata[9]
# left_pose_goal.pose.position.z = Neurondata[10]
# # 右臂
# right_pose_goal.pose.orientation.x = Right_Qux
# right_pose_goal.pose.orientation.y = Right_Quy
# right_pose_goal.pose.orientation.z = Right_Quz
# right_pose_goal.pose.orientation.w = Right_Quw
# right_pose_goal.pose.position.x = (1266/740)*(Neurondata[5]+0.28)-0.495
# right_pose_goal.pose.position.y = (1295/780)*(Neurondata[3]+0.56)-0.754
# right_pose_goal.pose.position.z = (1355/776)*(Neurondata[4]-0.054)-0.184
# # 左臂
# left_pose_goal.pose.orientation.x = Left_Qux
# left_pose_goal.pose.orientation.y = Left_Quy
# left_pose_goal.pose.orientation.z = Left_Quz
# left_pose_goal.pose.orientation.w = Left_Quw
# left_pose_goal.pose.position.x = (1266/850)*(Neurondata[11]+0.33)-0.495
# left_pose_goal.pose.position.y = (1295/720)*(Neurondata[9]+0.19)-0.541
# left_pose_goal.pose.position.z = (1355/745)*(Neurondata[10]-0.055)-0.184
# 右臂
right_pose_goal.pose.orientation.x = Right_Qux
right_pose_goal.pose.orientation.y = Right_Quy
right_pose_goal.pose.orientation.z = Right_Quz
right_pose_goal.pose.orientation.w = Right_Quw
right_pose_goal.pose.position.x = (Neurondata[5] - 0.05) * 1.48 + 0.053
right_pose_goal.pose.position.y = (Neurondata[3] + 0.18) * 1.48 - 0.12
right_pose_goal.pose.position.z = (Neurondata[4] - 0.53) * 1.48 + 0.47
# 左臂
left_pose_goal.pose.orientation.x = Left_Qux
left_pose_goal.pose.orientation.y = Left_Quy
left_pose_goal.pose.orientation.z = Left_Quz
left_pose_goal.pose.orientation.w = Left_Quw
left_pose_goal.pose.position.x = (Neurondata[11] - 0.05) * 1.48 + 0.053
left_pose_goal.pose.position.y = (Neurondata[9] - 0.18) * 1.48 + 0.12
left_pose_goal.pose.position.z = (Neurondata[10] - 0.53) * 1.48 + 0.47
# # 右臂
# right_pose_goal.pose.orientation.x = right_pose_goal.pose.orientation.x
# right_pose_goal.pose.orientation.y = right_pose_goal.pose.orientation.y
# right_pose_goal.pose.orientation.z = right_pose_goal.pose.orientation.z
# right_pose_goal.pose.orientation.w = right_pose_goal.pose.orientation.w
# right_pose_goal.pose.position.x = right_pose_goal.pose.position.x
# right_pose_goal.pose.position.y = right_pose_goal.pose.position.y
# right_pose_goal.pose.position.z = right_pose_goal.pose.position.z
# # 左臂
# left_pose_goal.pose.orientation.x = left_pose_goal.pose.orientation.x
# left_pose_goal.pose.orientation.y = left_pose_goal.pose.orientation.y
# left_pose_goal.pose.orientation.z = left_pose_goal.pose.orientation.z
# left_pose_goal.pose.orientation.w = left_pose_goal.pose.orientation.w
# left_pose_goal.pose.position.x = left_pose_goal.pose.position.x
# left_pose_goal.pose.position.y = left_pose_goal.pose.position.y
# left_pose_goal.pose.position.z = left_pose_goal.pose.position.z
# 设置动作组的两个目标点
both_arms.set_pose_target(right_pose_goal,
end_effector_link="gripper_r_finger_r")
both_arms.set_pose_target(left_pose_goal,
end_effector_link="gripper_l_finger_r")
# 规划和输出动作
traj = both_arms.plan()
both_arms.execute(traj, wait=False)
# # 清除路径约束
both_arms.clear_path_constraints()
# 确保输出停止
both_arms.stop()
def print_pointlist(self, point_list, future_print_status=False):
startime = datetime.datetime.now()
# Record number of print part
number_printing_part = 0
# Save original points list
full_point_list = copy.deepcopy(point_list)
full_point_array = np.delete(np.array(full_point_list), 2, axis=1)
self.target_list = full_point_list
if future_print_status: self.future_printing_status = True
# Constraints
pose = self.group.get_current_pose(self.group.get_end_effector_link())
constraints = Constraints()
# joint constraints
joint_constraint = JointConstraint()
joint_constraint.joint_name = 'arm_joint_1'
joint_constraint.position = 169 * pi / 180
joint_constraint.tolerance_above = 30 * pi / 180
joint_constraint.tolerance_below = 30 * pi / 180
joint_constraint.weight = 1.0
constraints.joint_constraints.append(joint_constraint)
joint_constraint = JointConstraint()
joint_constraint.joint_name = 'arm_joint_4'
joint_constraint.position = 150 * pi / 180
joint_constraint.tolerance_above = 30 * pi / 180
joint_constraint.tolerance_below = 30 * pi / 180
joint_constraint.weight = 1.0
constraints.joint_constraints.append(joint_constraint)
self.group.set_path_constraints(constraints)
# Orientation constrains
orientation_constraint = OrientationConstraint()
orientation_constraint.header = pose.header
orientation_constraint.link_name = self.group.get_end_effector_link()
orientation_constraint.orientation = pose.pose.orientation
orientation_constraint.absolute_x_axis_tolerance = 2 * pi
orientation_constraint.absolute_y_axis_tolerance = 2 * pi
orientation_constraint.absolute_z_axis_tolerance = 2 * pi
orientation_constraint.weight = 1.0
constraints.orientation_constraints.append(orientation_constraint)
# Record how many points has already finished
finsih_num = 0
print_num = 0
index_check = 0
while len(point_list) > 0:
print('New Plan, points left:', len(point_list))
# Move the robot point to first point and find the height
if len(point_list) > 1:
initial_plan = self.move_to_initial(point_list[1])
else:
initial_plan = self.move_to_initial(point_list[0])
# joint_goal = self.group.get_current_joint_values()
head = initial_plan.joint_trajectory.header
robot_state = self.robot.get_current_state()
# print(robot_state.joint_state)
robot_state.joint_state.position = tuple(initial_plan.joint_trajectory.points[-1].positions) + \
tuple(robot_state.joint_state.position[7:]) # the joints for the wheel
resp = self.request_fk(head, [self.group.get_end_effector_link()],
robot_state)
current_pose = self.group.get_current_pose().pose
current_pose.orientation = resp.pose_stamped[0].pose.orientation
(current_pose.position.x, current_pose.position.y,
current_pose.position.z) = point_list[0]
self.group.set_pose_target(current_pose)
self.group.go()
# Way points
plan_start_time = datetime.datetime.now()
waypoints = []
wpose = self.group.get_current_pose().pose
# Add the current pose to make sure the path is smooth
waypoints.append(copy.deepcopy(wpose))
success_num = 0
for point_num in range(len(point_list)):
(wpose.position.x, wpose.position.y,
wpose.position.z) = point_list[point_num]
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = self.group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.00,
path_constraints=constraints)
print 'Adding the first planing point, and fraction is', fraction
executing_state = 0
if fraction == 1:
success_num += 1
execute_plan = plan
if success_num == len(point_list):
self.group.execute(execute_plan, wait=False)
self.msg_print.data = True
executing_state = 1
success_num += 1
elif success_num == 0:
break
else:
# execute success plan
self.msg_print.data = True
self.group.execute(execute_plan, wait=False)
executing_state = 1
break
plan_end_time = datetime.datetime.now()
print 'first planing point time is', (plan_end_time -
plan_start_time).seconds
self.planning_time += max(
(plan_end_time - plan_start_time).seconds, 1)
## 2nd loop
## always re-plan and check for obstacle while executing waypoints
if future_print_status == True:
# Check for enclosure
self.base_group.set_position_target(
[0, 0, 0.05], self.base_group.get_end_effector_link())
result = self.base_group.plan()
self.base_group.clear_pose_targets()
if len(result.joint_trajectory.points) == 0:
print('Check enclosure failed')
self.group.stop()
print('Removing future obstacle')
self.future_printing_status = False
self.enclosure(point_list[0])
break
else:
print('Check enclosure successful')
executed_waypoint_index = 0 # initial value of nothing
success_point_list = point_list[:success_num]
print('when plan success, move_group_status:',
self.move_group_execution_status, 'success_plan_number:',
success_num)
if executing_state == 1:
printing_start_time = datetime.datetime.now()
success_planned_waypoint_array = np.delete(np.array(
point_list[:success_num]),
2,
axis=1)
# print 'success planned waypoint\n', success_planned_waypoint_array
print 'status', self.waypoint_execution_status
while self.waypoint_execution_status != 3:
if point_list == []: break
if self.waypoint_execution_status == 4:
# aborted state
print 'stop and abort waypoint execution'
self.msg_print.data = False
self.group.stop()
executing_state = 0
break
if self.waypoint_execution_status == 3: break
current_ee_pose = self.group.get_current_pose().pose
current_ee_position_array = np.array([
current_ee_pose.position.x, current_ee_pose.position.y
])
executed_waypoint_index = max(
self.check_executed_waypoint_index(
success_planned_waypoint_array,
current_ee_position_array),
executed_waypoint_index)
# print 'executed latest index', executed_waypoint_index
index_check = max(
self.check_executed_waypoint_index(
full_point_array, current_ee_position_array),
index_check)
self.further_printing_number = index_check
print 'index:', index_check, 'way_point index', executed_waypoint_index
if future_print_status == True:
# Check for enclosure
self.base_group.set_position_target(
[0, 0, 0.05],
self.base_group.get_end_effector_link())
result = self.base_group.plan()
self.base_group.clear_pose_targets()
if len(result.joint_trajectory.points) == 0:
print('Check enclosure failed')
self.group.stop()
print('Removing future obstacle')
self.future_printing_status = False
self.enclosure(full_point_list[index_check])
break
else:
print('Check enclosure successful')
## Replan to check for dynamic obstacle
waypoints = []
# Add the current pose to make sure the path is smooth, get latest pose
current_ee_pose = self.group.get_current_pose().pose
waypoints.append(copy.deepcopy(current_ee_pose))
# discard the executed waypoints
new_point_list = point_list[
executed_waypoint_index:success_num]
for k in new_point_list:
(current_ee_pose.position.x,
current_ee_pose.position.y,
current_ee_pose.position.z) = k
waypoints.append(copy.deepcopy(current_ee_pose))
(plan2, fraction2) = self.group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.00,
path_constraints=constraints)
print 'Dynamic check fraction:', fraction2
if fraction2 < 0.95:
## new obstacle appear
# print 'executed latest index', executed_waypoint_index
# print 'fraction value', fraction,'\n'
print 'new obstacle appeared to be in the path'
self.group.stop()
self.msg_print.data = False
executing_state = 0
break
rospy.sleep(2)
printing_end_time = datetime.datetime.now()
self.printing_time += (printing_end_time -
printing_start_time).seconds
number_printing_part += 1
print 'status:', self.waypoint_execution_status, 'executed_index:', executed_waypoint_index, 'success_num:', success_num
if self.waypoint_execution_status == 3:
# waypoint successfully printed
# self.print_list_visualize(point_list[:success_num])
del (point_list[:success_num - 1])
elif self.waypoint_execution_status == 2 or 4:
# state 2 = preempted, state 4 = aborted.
# only printed partial waypoint
# self.print_list_visualize(point_list[:executed_waypoint_index+1])
if executed_waypoint_index > 0: # at index 0, it might have not print the point 0-1 edge successfully.
del (point_list[:executed_waypoint_index]
) # delete up till whatever is executed
self.msg_print.data = False
if point_list == []: rospy.sleep(2)
self.group.stop()
# Delete the points what already execute
# if success_num > 0:
# Add obstacle after printing (need to revise)
self.group.set_path_constraints(None)
finshtime = datetime.datetime.now()
print('All time:', (finshtime - startime).seconds)
print('Printing time:', self.printing_time)
print('planning time:', self.planning_time)
print('Travel time:', (finshtime - startime).seconds -
self.printing_time - self.planning_time)
print('number of printing:', number_printing_part)
full_point_list_x = [base[0] for base in full_point_list]
full_point_list_y = [base[1] for base in full_point_list]
plt3 = plt.figure("Printing result", figsize=(6, 6))
plt.plot(self.re_position_x, self.re_position_y, 'b', linewidth=1.2)
plt.plot(full_point_list_x, full_point_list_y, 'ro')
plt.xlim(min(full_point_list_x) - 0.1, max(full_point_list_x) + 0.1)
plt.ylim(min(full_point_list_y) - 0.1, max(full_point_list_y) + 0.1)
plt.legend(['Printing result', 'ground truth'],
fontsize=8,
bbox_to_anchor=(1.0, 1))
plt.xlabel("X axis (m)")
plt.ylabel("y axis (m)")
plt.title('Printing result')
plt3.savefig('experiment_data/' + str(self.name) + '/' + self.name +
'_result.png',
dpi=plt3.dpi)
self.save_date()
print('All finish')
def print_pointlist(self, point_list, future_print_status=True):
# Save original points list
full_point_list = copy.deepcopy(point_list)
full_point_array = np.delete(np.array(full_point_list), 2, axis=1)
self.target_list = full_point_list
if future_print_status: self.future_printing_status = True
# Constraints
pose = self.group.get_current_pose(self.group.get_end_effector_link())
constraints = Constraints()
# joint constraints
joint_constraint = JointConstraint()
joint_constraint.joint_name = 'arm_joint_1'
joint_constraint.position = 169 * pi / 180
joint_constraint.tolerance_above = 30 * pi / 180
joint_constraint.tolerance_below = 30 * pi / 180
joint_constraint.weight = 1.0
constraints.joint_constraints.append(joint_constraint)
joint_constraint = JointConstraint()
joint_constraint.joint_name = 'arm_joint_4'
joint_constraint.position = 150 * pi / 180
joint_constraint.tolerance_above = 30 * pi / 180
joint_constraint.tolerance_below = 30 * pi / 180
joint_constraint.weight = 1.0
constraints.joint_constraints.append(joint_constraint)
self.group.set_path_constraints(constraints)
# Orientation constrains
orientation_constraint = OrientationConstraint()
orientation_constraint.header = pose.header
orientation_constraint.link_name = self.group.get_end_effector_link()
orientation_constraint.orientation = pose.pose.orientation
orientation_constraint.absolute_x_axis_tolerance = 2 * pi
orientation_constraint.absolute_y_axis_tolerance = 2 * pi
orientation_constraint.absolute_z_axis_tolerance = 2 * pi
orientation_constraint.weight = 1.0
constraints.orientation_constraints.append(orientation_constraint)
# Record how many points has already finished
finsih_num = 0
print_num = 0
index_check = 0
while len(point_list) > 0:
print('New Plan, points left:', len(point_list), point_list)
# Move the robot point to first point and find the height
if len(point_list) > 1:
initial_plan = self.move_to_initial(point_list[1])
else:
initial_plan = self.move_to_initial(point_list[0])
# joint_goal = self.group.get_current_joint_values()
head = initial_plan.joint_trajectory.header
robot_state = self.robot.get_current_state()
# print(robot_state.joint_state)
robot_state.joint_state.position = tuple(initial_plan.joint_trajectory.points[-1].positions) + \
tuple(robot_state.joint_state.position[7:]) # the joints for the wheel
resp = self.request_fk(head, [self.group.get_end_effector_link()],
robot_state)
current_pose = self.group.get_current_pose().pose
current_pose.orientation = resp.pose_stamped[0].pose.orientation
(current_pose.position.x, current_pose.position.y,
current_pose.position.z) = point_list[0]
self.group.set_pose_target(current_pose)
self.group.go()
# Move the robot to the center of the striaght line to make sure Way point method can be executed
# Way points
waypoints = []
wpose = self.group.get_current_pose().pose
# Add the current pose to make sure the path is smooth
waypoints.append(copy.deepcopy(wpose))
success_num = 0
for point_num in range(len(point_list)):
(wpose.position.x, wpose.position.y,
wpose.position.z) = point_list[point_num]
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = self.group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.00,
path_constraints=constraints)
print 'Adding the first planing point, and fraction is', fraction
executing_state = 0
if fraction == 1:
success_num += 1
execute_plan = plan
if success_num == len(point_list):
self.group.execute(execute_plan, wait=False)
self.msg_extrude = 3.0
self.extruder_publisher.publish(self.msg_extrude)
executing_state = 1
success_num += 1
elif success_num == 0:
break
else:
# execute success plan
self.msg_print.data = True
self.group.execute(execute_plan, wait=False)
self.msg_extrude = 3.0
self.extruder_publisher.publish(self.msg_extrude)
executing_state = 1
break
## 2nd loop
## always re-plan and check for obstacle while executing waypoints
executed_waypoint_index = 0 # initial value of nothing
success_point_list = point_list[:success_num]
print('when plan success, move_group_status:',
self.move_group_execution_status, 'success_plan_number:',
success_num)
if executing_state == 1:
success_planned_waypoint_array = np.delete(np.array(
point_list[:success_num]),
2,
axis=1)
# print 'success planned waypoint\n', success_planned_waypoint_array
print 'status', self.waypoint_execution_status
while self.waypoint_execution_status != 3:
if self.waypoint_execution_status == 4:
# aborted state
print 'stop and abort waypoint execution'
self.msg_print.data = False
self.group.stop()
executing_state = 0
break
current_ee_pose = self.group.get_current_pose().pose
current_ee_position_array = np.array([
current_ee_pose.position.x, current_ee_pose.position.y
])
executed_waypoint_index = self.check_executed_waypoint_index(
success_planned_waypoint_array,
current_ee_position_array)
# print 'executed latest index', executed_waypoint_index
index_check = self.check_executed_waypoint_index(
full_point_array, current_ee_position_array)
self.further_printing_number = index_check
print 'index:', index_check, 'way_point index', executed_waypoint_index
if future_print_status == True:
# Check for enclosure
self.base_group.set_position_target(
[0, 0, 0.05],
self.base_group.get_end_effector_link())
result = self.base_group.plan()
self.base_group.clear_pose_targets()
if len(result.joint_trajectory.points) == 0:
print('Check enclosure failed')
else:
print('Check enclosure successful')
## Replan to check for dynamic obstacle
waypoints = []
# Add the current pose to make sure the path is smooth, get latest pose
current_ee_pose = self.group.get_current_pose().pose
waypoints.append(copy.deepcopy(current_ee_pose))
# discard the executed waypoints
new_point_list = point_list[
executed_waypoint_index:success_num]
for k in new_point_list:
(current_ee_pose.position.x,
current_ee_pose.position.y,
current_ee_pose.position.z) = k
waypoints.append(copy.deepcopy(current_ee_pose))
(plan2, fraction2) = self.group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.00,
path_constraints=constraints)
print 'Dynamic check fraction:', fraction2
if fraction2 < 1.0:
## new obstacle appear
# print 'executed latest index', executed_waypoint_index
# print 'fraction value', fraction,'\n'
print 'new obstacle appeared to be in the path'
self.group.stop()
self.msg_print.data = False
executing_state = 0
break
rospy.sleep(2)
print 'status:', self.waypoint_execution_status, 'executed_index:', executed_waypoint_index, 'success_num:', success_num
if self.waypoint_execution_status == 3:
# waypoint successfully printed
# self.print_list_visualize(point_list[:success_num])
self.rviz_visualise_marker(point_list[:success_num])
del (point_list[:success_num - 1])
elif self.waypoint_execution_status == 2 or 4:
# state 2 = preempted, state 4 = aborted.
# only printed partial waypoint
# self.print_list_visualize(point_list[:executed_waypoint_index+1])
if executed_waypoint_index > 0: # at index 0, it might have not print the point 0-1 edge successfully.
self.rviz_visualise_marker(
point_list[:executed_waypoint_index + 1])
del (point_list[:executed_waypoint_index]
) # delete up till whatever is executed
self.msg_extrude = 0.0
self.extruder_publisher.publish(self.msg_extrude)
self.group.stop()
# Delete the points what already execute
# if success_num > 0:
# Add obstacle after printing (need to revise)
self.group.set_path_constraints(None)
print 'all finish'
def main():
roscpp_initialize(sys.argv)
rospy.init_node('grasp_ur5', anonymous=True)
robot = RobotCommander()
scene = PlanningSceneInterface()
arm_group = MoveGroupCommander("manipulator")
q = quaternion_from_euler(1.5701, 0.0, -1.5701)
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.x = q[0]
pose_target.orientation.y = q[1]
pose_target.orientation.z = q[2]
pose_target.orientation.w = q[3]
pose_target.position.z = 0.23 #0.23
pose_target.position.y = 0.11 #0.11
pose_target.position.x = -0.45 #-0.45
arm_group.set_pose_target(pose_target)
plan = arm_group.plan(pose_target)
while plan[0]!= True:
plan = arm_group.plan(pose_target)
if plan[0]:
traj = plan[1]
arm_group.execute(traj, wait = True)
arm_group.stop()
arm_group.clear_pose_targets()
# rospy.sleep(1)
#FAKE PLAN WITHOUT RESTRICTIONS #
# state = RobotState()
# arm_group.set_start_state(state)
pose_target.position.z = 0.77
arm_group.set_pose_target(pose_target)
plan_fake = arm_group.plan(pose_target)
while plan_fake[0] != True:
plan_fake = arm_group.plan(pose_target)
pose = arm_group.get_current_pose()
constraint = Constraints()
constraint.name = "restricao"
orientation_constraint = OrientationConstraint()
orientation_constraint.header = pose.header
orientation_constraint.link_name = arm_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 = 3.14
orientation_constraint.absolute_z_axis_tolerance = 3.14
orientation_constraint.weight = 1
constraint.orientation_constraints.append(orientation_constraint)
arm_group.set_path_constraints(constraint)
# state = RobotState()
# arm_group.set_start_state(state)
pose_target.position.z = 0.77 # 0.77
# pose_target.position.y = -0.11 # -0.11
# pose_target.position.x = 0.31 # 0.31
arm_group.set_pose_target(pose_target)
plan = arm_group.plan(pose_target)
while plan[0]!= True:
plan = arm_group.plan(pose_target)
if plan[0]:
traj = plan[1]
arm_group.execute(traj, wait = True)
arm_group.stop()
arm_group.clear_pose_targets()
