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 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 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 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 create_move_group_pose_goal(goal_pose=Pose(), group="right_arm_torso", end_link_name=None, plan_only=True):
""" Creates a move_group goal based on pose.
@arg group string representing the move_group group to use
@arg end_link_name string representing the ending link to use
@arg goal_pose Pose() representing the goal pose
@arg plan_only bool to for only planning or planning and executing
@returns MoveGroupGoal with the data given on the arguments"""
header = Header()
header.frame_id = 'base_link'
header.stamp = rospy.Time.now()
# We are filling in the MoveGroupGoal a MotionPlanRequest and a PlanningOptions message
# http://docs.ros.org/hydro/api/moveit_msgs/html/msg/MotionPlanRequest.html
# http://docs.ros.org/hydro/api/moveit_msgs/html/msg/PlanningOptions.html
moveit_goal = MoveGroupGoal()
goal_c = Constraints()
position_c = PositionConstraint()
position_c.header = header
if end_link_name != None: # For some groups the end_link_name can be deduced, but better add it manually
position_c.link_name = end_link_name
position_c.constraint_region.primitives.append(SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01])) # how big is the area where the end effector can be
position_c.constraint_region.primitive_poses.append(goal_pose)
position_c.weight = 1.0
goal_c.position_constraints.append(position_c)
orientation_c = OrientationConstraint()
orientation_c.header = header
if end_link_name != None:
orientation_c.link_name = end_link_name
orientation_c.orientation = goal_pose.orientation
orientation_c.absolute_x_axis_tolerance = 0.01 # Tolerances, MoveIt! by default uses 0.001 which may be too low sometimes
orientation_c.absolute_y_axis_tolerance = 0.01
orientation_c.absolute_z_axis_tolerance = 0.01
orientation_c.weight = 1.0
goal_c.orientation_constraints.append(orientation_c)
moveit_goal.request.goal_constraints.append(goal_c)
moveit_goal.request.num_planning_attempts = 5 # The number of times this plan is to be computed. Shortest solution will be reported.
moveit_goal.request.allowed_planning_time = 5.0
moveit_goal.planning_options.plan_only = plan_only
moveit_goal.planning_options.planning_scene_diff.is_diff = True # Necessary
moveit_goal.request.group_name = group
return moveit_goal
def create_move_group_pose_goal(
self, goal_pose=Pose(), group="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 liftgripper(self, target_z=0.10):
# approx centers of onions at 0.82, width of onion is 0.038 m. table is at 0.78
# length of gripper is 0.163 m The gripper should not go lower than
# (height_z of table w.r.t base+gripper-height/2+tolerance) = 0.78-0.93+0.08+0.01=-0.24
# pnp._limb.endpoint_pose returns {'position': (x, y, z), 'orientation': (x, y, z, w)}
# moving from z=-.02 to z=-0.1
group = self.group
while self.target_location_x == -100:
rospy.sleep(0.05)
current_pose = group.get_current_pose().pose
current_orientation = group.get_current_pose().pose.orientation
curr_q = [
current_orientation.x, current_orientation.y,
current_orientation.z, current_orientation.w
]
position_constraint = PositionConstraint()
position_constraint.target_point_offset.x = 0.1
position_constraint.target_point_offset.y = 0.2
position_constraint.target_point_offset.z = 0.3
position_constraint.weight = 0.8
position_constraint.link_name = group.get_end_effector_link()
position_constraint.header.frame_id = group.get_planning_frame()
orientation_constraint = OrientationConstraint()
orientation_constraint.orientation = Quaternion(x=curr_q[0],
y=curr_q[1],
z=curr_q[2],
w=curr_q[3])
orientation_constraint.absolute_x_axis_tolerance = 0.1
orientation_constraint.absolute_y_axis_tolerance = 0.1
orientation_constraint.absolute_z_axis_tolerance = 0.1
orientation_constraint.weight = 0.2
orientation_constraint.link_name = group.get_end_effector_link()
orientation_constraint.header.frame_id = group.get_planning_frame()
constraint = Constraints()
constraint.orientation_constraints.append(orientation_constraint)
constraint.position_constraints.append(position_constraint)
group.set_path_constraints(constraint)
allow_replanning = True
planning_time = 5
threshold = 0.02
time_limit = 120
start_time = time.time()
target_x = current_pose.position.x
target_y = current_pose.position.y
delta_z = (target_z - current_pose.position.z) / 4
print "Attempting to lift gripper"
i = 1
# if (delta_z < threshold):
# return True
# while (current_pose.position.z < target_z) and ((target_z - current_pose.position.z) > 3*threshold) and ((time.time()-start_time) < time_limit):
# status = self.go_to_pose_goal(curr_q[0], curr_q[1], curr_q[2], curr_q[3],
# target_x, target_y, current_pose.position.z + i*delta_z,
# allow_replanning, planning_time, thresh=threshold)
# rospy.sleep(0.1)
# current_pose = group.get_current_pose().pose
# i += 1
status = self.go_to_pose_goal(curr_q[0],
curr_q[1],
curr_q[2],
curr_q[3],
target_x,
target_y,
target_z,
allow_replanning,
planning_time,
thresh=threshold)
rospy.sleep(0.25)
print "Successfully lifted gripper to z: ", current_pose.position.z
group.clear_path_constraints()
# return ((target_z-current_pose.position.z) < 3*threshold or (current_pose.position.z > target_z))
return True
box_pose.pose.position.z = -0.25
box_pose.pose.orientation.w = 1.0
scene.add_box('box_object', box_pose, (0.4, 0.1, 0.5))
rospy.sleep(2)
rospy.loginfo("Scene Objects : {}".format(scene.get_known_object_names()))
# Set Path Constraint
constraints = Constraints()
constraints.name = "down"
orientation_constraint = OrientationConstraint()
orientation_constraint.header.frame_id = group.get_planning_frame()
orientation_constraint.link_name = group.get_end_effector_link()
orientation_constraint.orientation = pose_target_1.orientation
orientation_constraint.absolute_x_axis_tolerance = 3.1415
orientation_constraint.absolute_y_axis_tolerance = 0.05
orientation_constraint.absolute_z_axis_tolerance = 0.05
orientation_constraint.weight = 1.0
constraints.orientation_constraints.append(orientation_constraint)
group.set_path_constraints(constraints)
rospy.loginfo("Get Path Constraints:\n{}".format(
group.get_path_constraints()))
# Pose Target 2
rospy.loginfo("Start Pose Target 2")
pose_target_2 = Pose()
pose_target_2.position.x = 0.3
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 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 print_pointlist(self, point_list, future_print_status=False):
startime = datetime.datetime.now()
# 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_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
## 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 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
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:
## 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_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(finshtime - startime).seconds
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')
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')
plt.show()
plt3.savefig('result.png', dpi=plt3.dpi)
print('All finish, time:', (finshtime - startime).seconds)
def __init__(self):
self.bridge = CvBridge()
joint_state_topic = ['joint_states:=/iiwa/joint_states']
moveit_commander.roscpp_initialize(joint_state_topic)
rospy.Subscriber("/iiwa/joint_states", JointState, self.State_callback)
# Instantiate a RobotCommander object. This object is
# an interface to the robot as a whole.
self.robot = moveit_commander.RobotCommander()
self.group = moveit_commander.MoveGroupCommander("manipulator")
# rospy.sleep(2)
# self.scene = moveit_commander.PlanningSceneInterface('/iiwa/move_group/monitored_planning_scene')
# box_pose = PoseStamped()
# box_pose.header.frame_id = "world"
# box_pose.pose.position.x = 1.0
# box_pose.pose.orientation.w = 1.0
# self.scene.add_box("test", box_pose, size=(0.1, 0.2, 0.3))
# while not rospy.is_shutdown():
# rospy.sleep(1.0)
# for k in self.scene.__dict__.keys():
# print(k, self.scene.__dict__[k])
# # print(self.scene)
# print(self.scene.get_known_object_names())
# print(self.scene.get_attached_objects())
# exit()
self.group.set_max_velocity_scaling_factor(0.05)
self.group.set_max_acceleration_scaling_factor(0.05)
current_pose = self.group.get_current_pose(end_effector_link='iiwa_link_ee').pose
self._joint_efforts = 0
self._joint_vel = 0
self._joint_name = 0
self._header = None
pose = PoseStamped()
self.upright_constraints = Constraints()
self.upright_constraints.name = "upright"
orientation_constraint = OrientationConstraint()
orientation_constraint.header.frame_id = self.group.get_planning_frame()
orientation_constraint.link_name = self.group.get_end_effector_link()
pose.pose.orientation.x = 1.0
pose.pose.orientation.y = 0.0
pose.pose.orientation.z = 0.0
pose.pose.orientation.w = 0.0
orientation_constraint.orientation = pose.pose.orientation
orientation_constraint.absolute_x_axis_tolerance = .7#3.0
orientation_constraint.absolute_y_axis_tolerance = .7#3.0
orientation_constraint.absolute_z_axis_tolerance = 3.1415
#orientation_constraint.absolute_z_axis_tolerance = 3.14 #ignore this axis
orientation_constraint.weight = 1
self.upright_constraints.orientation_constraints.append(orientation_constraint)
self.group.allow_replanning(True)
self.group.allow_looking(True)
workspace = [0.5,-0.3,0.15,0.7,0.2,0.25]
# self.group.set_workspace(workspace)
# self.group.set_path_constraints(self.upright_constraints)
self.traj_num = -1
self.im_num = 0
self.MAX_PATH_LENGTH = 15
def place(self, target_name, place_position, pre_place_distance,
post_place_retreat):
limit = {'dist': 0.01, 'r': 0.10, 'p': 0.10, 'y': 0.10}
constraints = Constraints()
oc = OrientationConstraint()
oc.header.frame_id = REFERENCE_FRAME
oc.link_name = 'j2s7s300_end_effector'
oc.absolute_x_axis_tolerance = limit['r'] # radian
oc.absolute_y_axis_tolerance = limit['p']
oc.absolute_z_axis_tolerance = limit['y']
oc.weight = 0.85
oc.orientation = place_position.pose.orientation
constraints.orientation_constraints.append(deepcopy(oc))
result = False
replan_times = 1
replan_state = True
while replan_state and replan_times <= 5:
(place_path, fraction) = self.get_path(place_position.pose,
0.01,
constraints=constraints)
if fraction >= 0.95:
print "Pre_place_approach..."
self.arm.execute(place_path)
result = self.check(place_position.pose, limit)
replan_state = False
replan_times += 1
if result:
result = False
print "Placing..."
self.gripper.set_named_target("Open")
self.gripper.go()
self.arm.detach_object(target_name)
rospy.sleep(1)
post_place_position = self.get_retreat_point(
place_position.pose, post_place_retreat)
replan_state = True
replan_times = 1
while replan_state and replan_times <= 5:
(retreat_path,
fraction) = self.get_path(post_place_position,
0.01,
constraints=constraints)
if fraction > 0.8:
self.arm.execute(retreat_path)
result = self.check(post_place_position, limit)
replan_state = False
replan_times += 1
if not result:
self.state = STATE.PLACE_RETREAT_ERR
else:
self.state = STATE.PRE_PLACE_ERR
if self._server.current_goal.get_goal_status(
).status == GoalStatus.PREEMPTING:
rospy.logwarn("Can't cancel task after place action start!")
if result:
self.state = STATE.PLACE_FINISH
self.arm.clear_path_constraints()
def pick(self, target_name, grasp_position, pre_grasp_distance,
post_grasp_retreat):
pre_grasp_posture = JointTrajectory()
grasp_posture = JointTrajectory()
pre_grasp_posture = self.make_gripper_posture(GRIPPER_OPEN)
grasp_posture = self.make_gripper_posture(GRIPPER_CLOSED)
limit = {'dist': 0.01, 'r': 0.15, 'p': 0.15, 'y': 0.15}
constraints = Constraints()
oc = OrientationConstraint()
oc.header.frame_id = REFERENCE_FRAME
oc.link_name = 'j2s7s300_end_effector'
oc.absolute_x_axis_tolerance = limit['r'] # radian
oc.absolute_y_axis_tolerance = limit['p']
oc.absolute_z_axis_tolerance = limit['y']
oc.weight = 1.0
oc.orientation = grasp_position.pose.orientation
constraints.orientation_constraints.append(deepcopy(oc))
result = False
replan_times = 1
replan_state = True
while replan_state and replan_times <= 5:
(pre_grasp_path, fraction) = self.get_path(grasp_position.pose,
0.01,
constraints=constraints)
if self._server.current_goal.get_goal_status(
).status == GoalStatus.PREEMPTING:
self.state = STATE.STOP
return
if fraction >= 0.92:
print "Pre_grasp_approach..."
self.arm.execute(pre_grasp_path)
result = self.check(grasp_position.pose, limit)
replan_state = False
replan_times += 1
if result:
result = False
if self.arm.attach_object(
target_name, self.arm.get_end_effector_link(),
[
"j2s7s300_end_effector", "j2s7s300_link_finger_1",
"j2s7s300_link_finger_tip_1", "j2s7s300_link_finger_2",
"j2s7s300_link_finger_tip_2", "j2s7s300_link_finger_3",
"j2s7s300_link_finger_tip_3", "j2s7s300_joint_finger_1",
"j2s7s300_joint_finger_2", "j2s7s300_joint_finger_3"
]):
rospy.logwarn("Attach request sent successfully!")
else:
raise Exception("Can't send attach request!")
print "Grasping..."
self.gripper.set_named_target("Close")
if self._server.current_goal.get_goal_status(
).status == GoalStatus.PREEMPTING:
self.arm.detach_object(target_name)
self.back_to_init_pose()
self.state = STATE.STOP
return
self.gripper.go()
rospy.sleep(1)
post_grasp_position = self.get_retreat_point(
grasp_position.pose, post_grasp_retreat)
print "post_grasp_position: %s\n" % post_grasp_position
replan_times = 1
replan_state = True
while replan_state and replan_times <= 5:
(retreat_path,
fraction) = self.get_path(post_grasp_position,
0.005,
constraints=constraints)
if self._server.current_goal.get_goal_status(
).status == GoalStatus.PREEMPTING:
self.arm.detach_object(target_name)
self.back_to_init_pose(state=1)
self.state = STATE.STOP
return
if fraction > 0.8:
print "Retreating..."
self.arm.execute(retreat_path)
result = self.check(post_grasp_position, limit)
replan_state = False
replan_times += 1
if not result:
self.state = STATE.GRASP_RETREAT_ERR
else:
self.state = STATE.PRE_GRASP_ERR # pre_grasp planning failed
if result:
self.state = STATE.PICK_FINISH
self.arm.clear_path_constraints()
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'
