def add_way_points(self, waypoints):
arm = self.arm
# 限制末端夹具运动
right_joint_const = JointConstraint()
right_joint_const.joint_name = "gripper_r_joint_r"
right_joint_const.position = 0
right_joint_const.weight = 1.0
# 施加全约束
consts = Constraints()
consts.joint_constraints = [right_joint_const]
arm.set_path_constraints(consts)
wpose = arm.get_current_pose().pose
wpose.orientation.x = Qux
wpose.orientation.y = Quy
wpose.orientation.z = Quz
wpose.orientation.w = Quw
wpose.position.x = a[5]
wpose.position.y = a[3]
wpose.position.z = a[4]
waypoints.append(copy.deepcopy(wpose))
# print waypoints
# print "----------------\n"
return waypoints
def right_arm_go_to_pose_goal(self):
# 设置动作对象变量,此处为arm
right_arm = self.right_arm
# Create a path constraint for the arm
# UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS
joint_const = JointConstraint()
joint_const.joint_name = "gripper_r_joint_r"
joint_const.position = 0
consts = Constraints()
consts.joint_constraints = [joint_const]
right_arm.set_path_constraints(consts)
# 获取当前末端执行器位置姿态
pose_goal = right_arm.get_current_pose().pose
# print (a)
# print (Qux, Quy, Quz, Quw)
# 设置动作对象目标位置姿态
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]
pose_goal.position.y = Neurondata[3]
pose_goal.position.z = Neurondata[4]
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.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
# 施加全约束
consts = Constraints()
consts.joint_constraints = [right_joint_const]
right_arm.set_path_constraints(consts)
# 计算夹取姿态
if obj_theta <= 0:
(Qux, Quy, Quz,
Quw) = quaternion_from_euler(90.01 / 180 * pi, 0,
(-180 - obj_theta) / 180 * pi)
else:
(Qux, Quy, Quz,
Quw) = quaternion_from_euler(90.01 / 180 * pi, 0,
(180 - obj_theta) / 180 * pi)
# (Qux, Quy, Quz, Quw) = quaternion_from_euler(90/180*pi, 0, -180/180*pi)
# 设置动作对象目标位置姿态
pose_goal.orientation.x = Qux
pose_goal.orientation.y = Quy
pose_goal.orientation.z = Quz
pose_goal.orientation.w = Quw
# pose_goal.position.y = 0.0244387395252 + (-0.595877665686-0.0244387395252)*obj_x/320
# pose_goal.position.x = 0.625989932306 + (0.197518221397-0.625989932306)*obj_y/240
pose_goal.position.x = 0.1819576873 + (160 - obj_y) * (
0.494596343128 - 0.1819576873) / 160
pose_goal.position.y = obj_x * (
-0.455644324437 + 0.0238434066464) / 220 - 0.0238434066464
pose_goal.position.z = 0.0942404372508
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 constraint_planner(start_robot_state, goal_config, group_handle, planner_name, planning_attemps, planning_time): planning_workspace = WorkspaceParameters(); planning_workspace.header.frame_id = "/base_link"; planning_workspace.header.stamp = rospy.Time.now(); planning_workspace.min_corner.x = -1; planning_workspace.min_corner.y = -1; planning_workspace.min_corner.z = -1; planning_workspace.max_corner.x = 1; planning_workspace.max_corner.y = 1; planning_workspace.max_corner.z = 1; # Set Start start_state = RobotState(); start_state = start_robot_state; print ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>"; #print start_robot_state; # Set Goal goal_state = Constraints(); Jnt_constraint = setJointConstraints(goal_config, group_handle); goal_state.joint_constraints = Jnt_constraint; #print Jnt_constraint; # Set Constraints des_w = -0.070099; des_x = 0.41382; des_y = -0.57302; des_z = 0.70391; rotation_constraints = setOrientationConstraints(des_w,des_x,des_y,des_z,group_handle, weight = 1.0); # Generating Request planningRequest = MotionPlanRequest(); planningRequest.workspace_parameters = planning_workspace; planningRequest.start_state = start_robot_state; planningRequest.goal_constraints.append(goal_state); # Setting Constraint planningRequest.path_constraints.name = 'scoop_constraint'; planningRequest.path_constraints.orientation_constraints.append(rotation_constraints); #traj_constraint = Constraints(); #traj_constraint.orientation_constraints.append(rotation_constraints); #planningRequest.trajectory_constraints.constraints.append(traj_constraint); planningRequest.planner_id = "RRTConnectkConfigDefault"; planningRequest.group_name = group_handle.get_name(); planningRequest.num_planning_attempts = planning_attemps; planningRequest.allowed_planning_time = planning_time; planningRequest.max_velocity_scaling_factor = 1.0; Planning_req.publish(planningRequest);
def right_arm_go_down_goal(self):
# 设置动作对象变量,此处为arm
right_arm = self.right_arm
waypoints = []
# 获取当前末端执行器位置姿态
pose_goal = right_arm.get_current_pose().pose
# 添加路径起始点
waypoints.append(copy.deepcopy(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.0239
right_joint_const.weight = 1.0
# 施加全约束
consts = Constraints()
consts.joint_constraints = [right_joint_const]
right_arm.set_path_constraints(consts)
# 设置动作对象目标位置姿态
pose_goal.position.z = pose_goal.position.z - 0.045
# pose_goal.position.y = pose_goal.position.y - 0.1
# 添加路径末端点
waypoints.append(copy.deepcopy(pose_goal))
# 路径规划
(plan, fraction) = right_arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
print "End effector pose %s" % waypoints
# robot = self.robot
# display_trajectory_publisher = self.display_trajectory_publisher
# display_trajectory = moveit_msgs.msg.DisplayTrajectory()
# display_trajectory.trajectory_start = robot.get_current_state()
# display_trajectory.trajectory.append(plan)
# display_trajectory_publisher.publish(display_trajectory)
# 规划和输出动作
right_arm.execute(plan, wait=False)
def get_constraints(self, move_group):
'''
Creates a Constraints structure for the specified move_group based on
current stored information.
@param move_group : string specifying a particular move group
'''
constraints = Constraints()
constraints.joint_constraints = self.get_joint_constraints(move_group)
constraints.position_constraints = self.get_position_constraints(move_group)
constraints.orientation_constraints = self.get_orientation_constraints(move_group)
constraints.visibility_constraints = self.get_visibility_constraints(move_group)
return constraints
def add_robot_constraints():
constraint = Constraints()
constraint.name = "camera constraint"
roll_constraint = OrientationConstraint()
# 'base_link' is equal to the world link
roll_constraint.header.frame_id = 'world'
# The link that must be oriented upwards
roll_constraint.link_name = "camera"
roll_constraint.orientation = Quaternion(*tf.transformations.quaternion_from_euler(0,np.pi/3,0))
# Allow rotation of 45 degrees around the x and y axis
roll_constraint.absolute_x_axis_tolerance = np.pi #Allow max rotation of x degrees
roll_constraint.absolute_y_axis_tolerance = np.pi
roll_constraint.absolute_z_axis_tolerance = np.pi/2
# The roll constraint is the only constraint
roll_constraint.weight = 1
#constraint.orientation_constraints = [roll_constraint]
joint_1_constraint = JointConstraint()
joint_1_constraint.joint_name = "joint_1"
joint_1_constraint.position = 0
joint_1_constraint.tolerance_above = np.pi/2
joint_1_constraint.tolerance_below = np.pi/2
joint_1_constraint.weight = 1
joint_4_constraint = JointConstraint()
joint_4_constraint.joint_name = "joint_4"
joint_4_constraint.position = 0
joint_4_constraint.tolerance_above = np.pi/2
joint_4_constraint.tolerance_below = np.pi/2
joint_4_constraint.weight = 1
joint_5_constraint = JointConstraint()
joint_5_constraint.joint_name = "joint_5"
joint_5_constraint.position = np.pi/2
joint_5_constraint.tolerance_above = np.pi/2
joint_5_constraint.tolerance_below = np.pi/2
joint_5_constraint.weight = 1
joint_6_constraint = JointConstraint()
joint_6_constraint.joint_name = "joint_6"
joint_6_constraint.position = np.pi-0.79
joint_6_constraint.tolerance_above = np.pi
joint_6_constraint.tolerance_below = np.pi
joint_6_constraint.weight = 1
constraint.joint_constraints = [joint_1_constraint, joint_6_constraint]
return constraint
def left_arm_go_to_pose_goal(self):
# 设置动作对象变量,此处为arm
left_arm = self.left_arm
# 限制末端夹具运动
left_joint_const = JointConstraint()
left_joint_const.joint_name = "gripper_l_joint_r"
if Leftfinger >= 0.025:
left_joint_const.position = 0.024
else:
left_joint_const.position = 0
consts = Constraints()
consts.joint_constraints = [left_joint_const]
left_arm.set_path_constraints(consts)
# 获取当前末端执行器位置姿态
pose_goal = left_arm.get_current_pose().pose
# 设置动作对象目标位置姿态
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.stop()
def right_arm_go_to_pose_goal(self):
# 设置动作对象变量,此处为arm
right_arm = self.right_arm
# 限制末端夹具运动
right_joint_const = JointConstraint()
right_joint_const.joint_name = "gripper_r_joint_r"
if Rightfinger >= 0.025:
right_joint_const.position = 0.024
else:
right_joint_const.position = 0
consts = Constraints()
consts.joint_constraints = [right_joint_const]
right_arm.set_path_constraints(consts)
# 获取当前末端执行器位置姿态
pose_goal = right_arm.get_current_pose().pose
# 设置动作对象目标位置姿态
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 = 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.stop()
def quick_motion_test_client():
rospy.wait_for_service('/lightning/lightning_get_path')
print "Working on it..."
req = GetMotionPlanRequest()
req.motion_plan_request.group_name = "torso"
req.motion_plan_request.start_state.joint_state.name = ["torso_lift_joint"]
req.motion_plan_request.start_state.joint_state.position = [float(0.2)]
tempConstraint = JointConstraint()
tempConstraint.joint_name = req.motion_plan_request.start_state.joint_state.name[0]
tempConstraint.position = float(0.15)
fullConstraint = Constraints()
fullConstraint.joint_constraints = [tempConstraint]
req.motion_plan_request.goal_constraints = [fullConstraint]
req.motion_plan_request.allowed_planning_time = 50.0
try:
client_func = rospy.ServiceProxy('/lightning/lightning_get_path', GetMotionPlan)
res = client_func(req)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
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 > -60 :
# left_joint_const.position = 0.024
# else:
left_joint_const.position = 0
left_joint_const.weight = 1.0
# 施加全约束
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 = -0.626215011187053
pose_goal.orientation.y = -0.4552380340595104
pose_goal.orientation.z = -0.48010690496799074
pose_goal.orientation.w = 0.4124444834298159
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
# 规划和输出动作
traj = left_arm.plan()
left_arm.execute(traj, wait=False)
# 动作完成后清除目标信息
left_arm.clear_pose_targets()
# 清除路径约束
left_arm.clear_path_constraints()
# 确保没有剩余未完成动作在执行
left_arm.stop()
def plan_cartesian_path(self, waypoints):
arm = self.arm
# 限制末端夹具运动
right_joint_const = JointConstraint()
right_joint_const.joint_name = "gripper_r_joint_r"
right_joint_const.position = 0
right_joint_const.weight = 1.0
# 施加全约束
consts = Constraints()
consts.joint_constraints = [right_joint_const]
arm.set_path_constraints(consts)
(plan, fraction) = arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
return plan, fraction
def quick_motion_test_client():
rospy.wait_for_service('/lightning/lightning_get_path')
print "Working on it..."
req = GetMotionPlanRequest()
req.motion_plan_request.group_name = "torso"
req.motion_plan_request.start_state.joint_state.name = ["torso_lift_joint"]
req.motion_plan_request.start_state.joint_state.position = [float(0.2)]
tempConstraint = JointConstraint()
tempConstraint.joint_name = req.motion_plan_request.start_state.joint_state.name[
0]
tempConstraint.position = float(0.15)
fullConstraint = Constraints()
fullConstraint.joint_constraints = [tempConstraint]
req.motion_plan_request.goal_constraints = [fullConstraint]
req.motion_plan_request.allowed_planning_time = 50.0
try:
client_func = rospy.ServiceProxy('/lightning/lightning_get_path',
GetMotionPlan)
res = client_func(req)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def create_basic_mp_joint_request(joint_names, joint_values, planner_id):
motion_plan_request = MotionPlanRequest()
motion_plan_request.group_name = move_group
motion_plan_request.num_planning_attempts = 1
motion_plan_request.allowed_planning_time = 5.0
motion_plan_request.workspace_parameters = WorkspaceParameters()
motion_plan_request.max_velocity_scaling_factor = 0.5
motion_plan_request.max_acceleration_scaling_factor = 0.5
motion_plan_request.planner_id = planner_id
joint_constraints = []
for i in range(len(joint_names)):
joint_con = JointConstraint()
joint_con.joint_name = joint_names[i]
joint_con.position = joint_values[i]
joint_con.tolerance_above = 0.0001
joint_con.tolerance_below = 0.0001
joint_con.weight = 0.0001
joint_constraints.append(joint_con)
constraint = Constraints()
constraint.joint_constraints = joint_constraints
constraints = [constraint]
motion_plan_request.goal_constraints = constraints
return motion_plan_request
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 on_enter(self, userdata):
# Clear return information
self.action_client = None # Clear prior connection in case we changed topics
self.request_time = None
self.robot_trajectory = None
self.return_code = None
# We require action_topic and move_group to be set to use this state
if (self.given_action_topic is None) and (not hasattr(
userdata, 'action_topic') or userdata.action_topic is None):
self.return_code = 'param_error'
Logger.logwarn(
'Userdata action topic of state %s does not exist or is currently undefined!'
% self.name)
return
if not hasattr(userdata, 'move_group') or userdata.move_group is None:
self.return_code = 'param_error'
Logger.logwarn(
'Userdata move_group of state %s does not exist or is currently undefined!'
% self.name)
return
if self.moveit_client is None:
try:
self.moveit_client = ProxyMoveItClient(None)
except Exception as e:
self.moveit_client = None
self.return_code = 'param_error'
Logger.logerr(str(e))
Logger.logwarn(
'No MoveItClient configured - state %s does not exist or is currently undefined!'
% self.name)
return
try:
if (self.given_action_topic is None):
if (self.current_action_topic != userdata.action_topic) or \
(self.move_group != userdata.move_group) :
# Make sure connection is established for change in topics
self.current_action_topic = userdata.action_topic
self.move_group = userdata.move_group
Logger.loginfo(
"%s - trying to connect to %s on moveit client ..." %
(self.name, self.current_action_topic))
self.moveit_client.setup_action_client(
self.current_action_topic, "MoveGroupAction",
self.enter_wait_duration)
else:
self.current_action_topic = self.given_action_topic
if not self.moveit_client.is_available(self.current_action_topic):
# Try to re-connect if not available
ret = False
if self.enter_wait_duration >= 0.0:
Logger.loginfo(
"%s - trying to reconnect to %s on moveit client ..."
% (self.name, self.current_action_topic))
ret = self.moveit_client.connect_action_server(
self.current_action_topic, "MoveGroupAction",
self.enter_wait_duration)
if not ret:
Logger.logwarn(
'State %s -action topic %s connection to action server is not available'
% (self.name, current_action_topic))
self.return_code = 'topics_unavailable'
return
# Retrieve reference to the relevant action client
self.action_client = self.moveit_client._action_clients[
self.current_action_topic]
except Exception as e:
Logger.logwarn('State %s - invalid action connection !\n%s' %
(self.name, str(e)))
self.return_code = 'param_error'
return
try:
joint_values = userdata.joint_values
joint_names = ProxyMoveItClient._joint_names[userdata.move_group]
if hasattr(userdata, 'joint_names'):
joint_names = userdata.joint_names
if len(joint_names) != len(joint_values):
Logger.logwarn(
'%s - Joint values mismatch %d vs. %d' %
(self.name, len(joint_names), len(joint_values)))
self.return_code = 'param_error'
return
# Grab the current data and create a deep copy for local mods
action_goal = MoveGroupGoal(
request=copy.deepcopy(ProxyMoveItClient._motion_plan_requests[
userdata.move_group]),
planning_options=copy.deepcopy(
ProxyMoveItClient._planning_options[userdata.move_group]))
action_goal.planning_options.plan_only = True
Logger.loginfo('State %s - set goal joint constraints !' %
(self.name))
constraints = Constraints()
constraints.joint_constraints = self.moveit_client.get_goal_joint_constraints(
self.move_group, joint_values, joint_names)
action_goal.request.goal_constraints.append(constraints)
self.request_time = rospy.Time.now()
print "------------ Planning Goal -----"
print str(action_goal)
self.action_client.send_goal(self.current_action_topic,
action_goal)
if self.timeout_duration > rospy.Duration(0.0):
self.timeout_target = self.request_time + self.timeout_duration + rospy.Duration(
action_goal.request.allowed_planning_time)
else:
self.timeout_target = None
except Exception as e:
Logger.logwarn('Could not request a plan!\n%s' % str(e))
self.return_code = 'param_error'
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 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()
