def setUp(self):
# create a action client of move group
self._move_client = SimpleActionClient('move_group', MoveGroupAction)
self._move_client.wait_for_server()
moveit_commander.roscpp_initialize(sys.argv)
group_name = moveit_commander.RobotCommander().get_group_names()[0]
group = moveit_commander.MoveGroupCommander(group_name)
# prepare a joint goal
self._goal = MoveGroupGoal()
self._goal.request.group_name = group_name
self._goal.request.max_velocity_scaling_factor = 0.1
self._goal.request.max_acceleration_scaling_factor = 0.1
self._goal.request.start_state.is_diff = True
goal_constraint = Constraints()
joint_values = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]
joint_names = group.get_active_joints()
for i in range(len(joint_names)):
joint_constraint = JointConstraint()
joint_constraint.joint_name = joint_names[i]
joint_constraint.position = joint_values[i]
joint_constraint.weight = 1.0
goal_constraint.joint_constraints.append(joint_constraint)
self._goal.request.goal_constraints.append(goal_constraint)
self._goal.planning_options.planning_scene_diff.robot_state.is_diff = True
def execute(self, userdata):
header = Header()
header.frame_id = "base_link"
header.stamp = rospy.Time.now()
userdata.move_it_joint_goal = MoveGroupGoal()
goal_c = Constraints()
for name, value in zip(userdata.manip_joint_names, userdata.manip_goal_joint_pose):
joint_c = JointConstraint()
joint_c.joint_name = name
joint_c.position = value
joint_c.tolerance_above = 0.01
joint_c.tolerance_below = 0.01
joint_c.weight = 1.0
goal_c.joint_constraints.append(joint_c)
userdata.move_it_joint_goal.request.goal_constraints.append(goal_c)
userdata.move_it_joint_goal.request.num_planning_attempts = 5
userdata.move_it_joint_goal.request.allowed_planning_time = 5.0
userdata.move_it_joint_goal.planning_options.plan_only = False # False = Plan + Execute ; True = Plan only
userdata.move_it_joint_goal.planning_options.planning_scene_diff.is_diff = True
userdata.move_it_joint_goal.request.group_name = userdata.manip_joint_group
rospy.loginfo("Group Name: " + userdata.manip_joint_group)
rospy.loginfo("Joints name: " + str(userdata.manip_joint_names))
rospy.loginfo("Joints Values: " + str(userdata.manip_goal_joint_pose))
rospy.loginfo("GOAL TO SEND IS:... " + str(userdata.move_it_joint_goal))
return "succeeded"
def create_move_group_joints_goal(joint_names, joint_values, group="right_arm_torso", plan_only=True):
""" Creates a move_group goal based on pose.
@arg joint_names list of strings of the joint names
@arg joint_values list of digits with the joint values
@arg group string representing the move_group group to use
@arg plan_only bool to for only planning or planning and executing
@return MoveGroupGoal with the desired contents"""
header = Header()
header.frame_id = 'base_link'
header.stamp = rospy.Time.now()
moveit_goal = MoveGroupGoal()
goal_c = Constraints()
for name, value in zip(joint_names, joint_values):
joint_c = JointConstraint()
joint_c.joint_name = name
joint_c.position = value
joint_c.tolerance_above = 0.01
joint_c.tolerance_below = 0.01
joint_c.weight = 1.0
goal_c.joint_constraints.append(joint_c)
moveit_goal.request.goal_constraints.append(goal_c)
moveit_goal.request.num_planning_attempts = 1
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
moveit_goal.request.group_name = group
return moveit_goal
def plan_trajectory(self, start_point, goal_point, planner_number, joint_names, group_name, planning_time, planner_config_name):
"""
Given a start and goal point, returns the planned path.
Args:
start_point (list of float): A starting joint configuration.
goal_point (list of float): A goal joint configuration.
planner_number (int): The index of the planner to be used as
returned by acquire_planner().
joint_names (list of str): The name of the joints corresponding to
start_point and goal_point.
group_name (str): The name of the group to which the joint names
correspond.
planning_time (float): Maximum allowed time for planning, in seconds.
planner_config_name (str): Type of planner to use.
Return:
list of list of float: A sequence of points representing the joint
configurations at each point on the path.
"""
planner_client = rospy.ServiceProxy(self.planners[planner_number], GetMotionPlan)
rospy.loginfo("Plan Trajectory Wrapper: got a plan_trajectory request for %s with start = %s and goal = %s" % (self.planners[planner_number], start_point, goal_point))
# Put together the service request.
req = GetMotionPlanRequest()
req.motion_plan_request.workspace_parameters.header.stamp = rospy.get_rostime()
req.motion_plan_request.group_name = group_name
req.motion_plan_request.num_planning_attempts = 1
req.motion_plan_request.allowed_planning_time = planning_time
req.motion_plan_request.planner_id = planner_config_name #using RRT planner by default
req.motion_plan_request.start_state.joint_state.header.stamp = rospy.get_rostime()
req.motion_plan_request.start_state.joint_state.name = joint_names
req.motion_plan_request.start_state.joint_state.position = start_point
req.motion_plan_request.goal_constraints.append(Constraints())
req.motion_plan_request.goal_constraints[0].joint_constraints = []
for i in xrange(len(joint_names)):
temp_constraint = JointConstraint()
temp_constraint.joint_name = joint_names[i]
temp_constraint.position = goal_point[i]
temp_constraint.tolerance_above = 0.05;
temp_constraint.tolerance_below = 0.05;
req.motion_plan_request.goal_constraints[0].joint_constraints.append(temp_constraint)
#call the planner
rospy.wait_for_service(self.planners[planner_number])
rospy.loginfo("Plan Trajectory Wrapper: sent request to service %s" % planner_client.resolved_name)
try:
response = planner_client(req)
except rospy.ServiceException, e:
rospy.loginfo("Plan Trajectory Wrapper: service call failed: %s"%e)
return None
def createJointConstraints(pose_from_params, tolerances=0.1):
"""Create a JointConstraints message with its joint names and positions with some default tolerances
@param pose_from_params dictionary with names of the joints and it's values
@param tolerances the tolerance in radians for the joint positions, defaults to 0.1
@return moveit_msgs/JointConstraints[] message with the joint names and positions"""
joint_constraints = []
for joint in pose_from_params:
joint_constraint = JointConstraint()
joint_constraint.joint_name = joint
joint_constraint.position = pose_from_params[joint]
joint_constraint.tolerance_above = tolerances
joint_constraint.tolerance_below = tolerances
joint_constraint.weight = 1.0
joint_constraints.append(joint_constraint)
return joint_constraints
def createJointConstraintsZero():
joint_positions = [1.7567944054, 1.68571255762, -0.35731008621,
1.06480870567, 1.36986326531, -0.662985424101,
-1.31376998814]
joint_constraints = []
for joint_num in range(1,8):
joint_constraint = JointConstraint()
joint_constraint.joint_name = 'arm_right_' + str(joint_num) + '_joint'
joint_constraint.tolerance_above = 0.1
joint_constraint.tolerance_below = 0.1
joint_constraint.weight = 1.0
joint_constraint.position = 0.0
joint_constraints.append(joint_constraint)
print str(joint_constraint)
return joint_constraints
def createJointConstraints(joint_names, joint_positions, tolerances=0.1):
"""Create a JointConstraints message with its joint names and positions with some default tolerances
@param joint_names names of the joints which will reference to values in param joint_positions
@param joint_positions values for the joints specified in joint_names
@param tolerances the tolerance in radians for the joint positions, defaults to 0.1
@return moveit_msgs/JointConstraints[] message with the joint names and positions"""
joint_constraints = []
for joint, pos in zip(joint_names, joint_positions):
joint_constraint = JointConstraint()
joint_constraint.joint_name = joint
joint_constraint.position = pos
joint_constraint.tolerance_above = tolerances
joint_constraint.tolerance_below = tolerances
joint_constraint.weight = 1.0
joint_constraints.append(joint_constraint)
return joint_constraints
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 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 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 update(self):
self.logger.debug("{0}.update()".format(self.__class__.__name__))
if not self.action_client:
self.feedback_message = "no action client, did you call setup() on your tree?"
return py_trees.Status.INVALID
# pity there is no 'is_connected' api like there is for c++
if not self.sent_goal:
if self.blackboard.frame_id != self.blackboard.target:
print("Detected fail!")
return py_trees.Status.FAILURE
#update goal data from blackboard
self.action_goal.target_pose.header.frame_id = "base_footprint"
# self.action_goal.target_pose.pose.position.x = self.blackboard.object_pose.pose.position.x
# self.action_goal.target_pose.pose.position.y = self.blackboard.object_pose.pose.position.y
# self.action_goal.target_pose.pose.position.z = self.blackboard.object_pose.pose.position.z
self.action_goal.target_pose.pose.position.x = self.blackboard.object_pose.pose.position.x + self.x_offset
self.action_goal.target_pose.pose.position.y = self.blackboard.object_pose.pose.position.y + self.y_offset
self.action_goal.target_pose.pose.position.z = self.blackboard.object_pose.pose.position.z + self.z_offset
# self.action_goal.target_pose.pose.position.x = 0.6
# self.action_goal.target_pose.pose.position.y = 0
# self.action_goal.target_pose.pose.position.z = 0.8
# if self.x_offset != 0:
# self.action_goal.target_pose.pose.position.x = self.blackboard.object_pose.pose.position.x - (self.blackboard.object_pose.pose.position.x/8.0)
# self.action_goal.target_pose.pose.position.y = self.blackboard.object_pose.pose.position.y - (self.blackboard.object_pose.pose.position.y/8.0)
# self.action_goal.target_pose.pose.position.x = self.blackboard.object_pose.pose.position.x + 0.04
# self.action_goal.target_pose.pose.position.y = self.blackboard.object_pose.pose.position.y
# self.action_goal.target_pose.pose.position.z = self.blackboard.object_pose.pose.position.z - 0.02
if self.mode == "put":
self.action_goal.target_pose.pose.position.x = self.blackboard.object_pose.pose.position.x
self.action_goal.target_pose.pose.position.z = self.blackboard.object_pose.pose.position.z + 0.1
self.action_goal.target_pose.pose.position.y = -self.blackboard.object_pose.pose.position.y
theta = math.atan2(self.action_goal.target_pose.pose.position.y,
self.action_goal.target_pose.pose.position.x)
quat = quaternion_from_euler(0.0, 0.0, theta)
self.action_goal.target_pose.pose.orientation.x = quat[0]
self.action_goal.target_pose.pose.orientation.y = quat[1]
self.action_goal.target_pose.pose.orientation.z = quat[2]
self.action_goal.target_pose.pose.orientation.w = quat[3]
# self.action_goal.target_pose.pose.orientation.x = 0
# self.action_goal.target_pose.pose.orientation.y = 0
# self.action_goal.target_pose.pose.orientation.z = 0
# self.action_goal.target_pose.pose.orientation.w = 1.0
if self.constraint:
if len(self.joint) == 0:
print("Constraint joint not defined.")
return py_trees.Status.FAILURE
for joint in self.joint:
joint_constraint = JointConstraint()
joint_constraint.joint_name = joint
joint_constraint.position = self.joint[
joint_constraint.joint_name][0]
joint_constraint.tolerance_above = self.joint[
joint_constraint.joint_name][1]
joint_constraint.tolerance_below = self.joint[
joint_constraint.joint_name][2]
joint_constraint.weight = 1.0
self.action_goal.joint_constraints.append(joint_constraint)
print("Joint constrained : " + str(joint_constraint))
print("Goal position : (" +
str(self.action_goal.target_pose.pose.position.x) + " , " +
str(self.action_goal.target_pose.pose.position.y) + " , " +
str(self.action_goal.target_pose.pose.position.z) + ")")
print("Goal orientation : ", quat)
self.action_client.send_goal(self.action_goal)
self.sent_goal = True
self.feedback_message = "sent goal to the action server"
return py_trees.Status.RUNNING
# print("self.action_client.get_state() : " + str(self.action_client.get_state()))
# print("self.feedback_message : " + str(self.feedback_message))
self.feedback_message = self.action_client.get_goal_status_text()
# Failure case
if self.action_client.get_state() in [
actionlib_msgs.GoalStatus.ABORTED,
actionlib_msgs.GoalStatus.PREEMPTED
]:
return py_trees.Status.FAILURE
result = self.action_client.get_result()
if result:
return py_trees.Status.SUCCESS
else:
self.feedback_message = self.override_feedback_message_on_running
return py_trees.Status.RUNNING
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 main():
#Initialize the node
rospy.init_node('moveit_client')
# Create the SimpleActionClient, passing the type of the action
# (MoveGroupAction) to the constructor.
client = actionlib.SimpleActionClient('move_group', MoveGroupAction)
# Wait until the action server has started up and started
# listening for goals.
client.wait_for_server()
# Creates a goal to send to the action server.
goal = MoveGroupGoal()
#----------------Construct the goal message (start)----------------
joint_names = ['head_pan', 'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1', 'left_w2', 'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2']
#Set parameters for the planner
goal.request.group_name = 'both_arms'
goal.request.num_planning_attempts = 1
goal.request.allowed_planning_time = 5.0
#Define the workspace in which the planner will search for solutions
goal.request.workspace_parameters.min_corner.x = -1
goal.request.workspace_parameters.min_corner.y = -1
goal.request.workspace_parameters.min_corner.z = -1
goal.request.workspace_parameters.max_corner.x = 1
goal.request.workspace_parameters.max_corner.y = 1
goal.request.workspace_parameters.max_corner.z = 1
goal.request.start_state.joint_state.header.frame_id = "base"
#Set the start state for the trajectory
goal.request.start_state.joint_state.name = joint_names
goal.request.start_state.joint_state.position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
goal.request.start_state.joint_state.velocity = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
print 'Enter start state:'
try:
for i in range(len(joint_names)):
val = float(raw_input(joint_names[i]+':'))
goal.request.start_state.joint_state.position[i] = val
except:
print 'Using defaults for remaining joints'
print goal.request.start_state.joint_state.position
#Tell MoveIt whether to execute the trajectory after planning it
goal.planning_options.plan_only = True
#Set the goal position of the robot
#Note that the goal is specified with a collection of individual
#joint constraints, rather than a vector of joint angles
arm_joint_names = joint_names[1:]
target_joint_angles = [0.5, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
print 'Enter goal state:'
try:
for i in range(len(target_joint_angles)):
val = float(raw_input(joint_names[i]+':'))
target_joint_angles[i] = val
except:
print 'Using defaults for remaining joints'
print target_joint_angles
tolerance = 0.0001
consts = []
for i in range(len(arm_joint_names)):
const = JointConstraint()
const.joint_name = arm_joint_names[i]
const.position = target_joint_angles[i]
const.tolerance_above = tolerance
const.tolerance_below = tolerance
const.weight = 1.0
consts.append(const)
goal.request.goal_constraints.append(Constraints(name='', joint_constraints=consts))
#---------------Construct the goal message (end)-----------------
# Send the goal to the action server.
client.send_goal(goal)
# Wait for the server to finish performing the action.
client.wait_for_result()
# Print out the result of executing the action
print(client.get_result())
def main():
#Initialize the node
rospy.init_node('moveit_client')
# Create the SimpleActionClient, passing the type of the action
# (MoveGroupAction) to the constructor.
client = actionlib.SimpleActionClient('move_group', MoveGroupAction)
# Wait until the action server has started up and started
# listening for goals.
client.wait_for_server()
# Creates a goal to send to the action server.
goal = MoveGroupGoal()
#----------------Construct the goal message (start)----------------
joint_names = [
'head_pan', 'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0',
'left_w1', 'left_w2', 'right_s0', 'right_s1', 'right_e0', 'right_e1',
'right_w0', 'right_w1', 'right_w2'
]
#Set parameters for the planner
goal.request.group_name = 'both_arms'
goal.request.num_planning_attempts = 1
goal.request.allowed_planning_time = 5.0
#Define the workspace in which the planner will search for solutions
goal.request.workspace_parameters.min_corner.x = -1
goal.request.workspace_parameters.min_corner.y = -1
goal.request.workspace_parameters.min_corner.z = -1
goal.request.workspace_parameters.max_corner.x = 1
goal.request.workspace_parameters.max_corner.y = 1
goal.request.workspace_parameters.max_corner.z = 1
goal.request.start_state.joint_state.header.frame_id = "base"
#Set the start state for the trajectory
goal.request.start_state.joint_state.name = joint_names
#YOUR CODE HERE
#Have a user input a specified start position for the first four angles
#raw_input() might be useful to look at here
goal.request.start_state.joint_state.position = [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0
]
goal.request.start_state.joint_state.velocity = [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0
]
#Tell MoveIt whether to execute the trajectory after planning it
goal.planning_options.plan_only = True
#Set the goal position of the robot
#Note that the goal is specified with a collection of individual
#joint constraints, rather than a vector of joint angles
arm_joint_names = joint_names[1:]
#YOUR CODE HERE
#Have a user input a specified target position for the first four angles
#raw_input() might be useful to look at here
TargetAnglesIn = raw_input("Please enter the first 4 target angles")
temp = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
userTargetJointAngles = TargetAnglesIn.split(" ")
for i in range(len(userTargetJointAngles)):
userTargetJointAngles[i] = float(userTargetJointAngles[i])
userTargetJointAngles.extend(temp)
tolerance = 0.0001
consts = []
print(userTargetJointAngles)
for i in range(len(arm_joint_names)):
const = JointConstraint()
const.joint_name = arm_joint_names[i]
const.position = userTargetJointAngles[i]
const.tolerance_above = tolerance
const.tolerance_below = tolerance
const.weight = 1.0
consts.append(const)
goal.request.goal_constraints.append(
Constraints(name='', joint_constraints=consts))
#---------------Construct the goal message (end)-----------------
# Send the goal to the action server.
client.send_goal(goal)
# Wait for the server to finish performing the action.
client.wait_for_result()
# Print out the result of executing the action
print(client.get_result())
# Get instance from moveit_commander
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
# Get group_commander from MoveGroupCommander
group_name = "arm"
move_group = moveit_commander.MoveGroupCommander(group_name)
# Move using target_pose with constraint
contraints = Constraints()
contraints.name = "constraints1"
jc1 = JointConstraint()
jc1.joint_name = "joint2"
jc1.position = 0.0
jc1.tolerance_above = 0.01
jc1.tolerance_below = 0.01
contraints.joint_constraints.append(jc1)
move_group.set_path_constraints(contraints)
# Move using target_pose
x = 0.185
y = 0
z = 0.365
test = [x, y, z]
move_group.set_position_target(test)
plan = move_group.go(wait=True)
def inverse_kinematics():
# Construct the request
request = GetPositionIKRequest()
request.ik_request.group_name = "right_arm"
request.ik_request.ik_link_name = "right_gripper"
request.ik_request.attempts = 50
request.ik_request.pose_stamped.header.frame_id = "base"
# Create joint constraints
#This is joint constraint will need to be set at the group
constraints = Constraints()
joint_constr = JointConstraint()
joint_constr.joint_name = "right_j0"
joint_constr.position = TORSO_POSITION
joint_constr.tolerance_above = TOLERANCE
joint_constr.tolerance_below = TOLERANCE
joint_constr.weight = 0.5
constraints.joint_constraints.append(joint_constr)
# Get the transformed AR Tag (x,y,z) coordinates
# Only care about the x coordinate of AR tag; tells use
# how far away wall is
x_coord = board_x
y_coord = board_y
z_coord = board_z
y_bias = raw_input("Input y coordinate: ")
z_bias = raw_input("Input z coordinate: ")
y_coord += float(y_bias)
z_coord += float(z_bias)
#Creating Path Planning
waypoints = []
target_pose = Pose()
target_pose.position.x = float(x_coord - PLANNING_BIAS)
target_pose.position.y = float(y_coord)
target_pose.position.z = float(z_coord)
target_pose.orientation.y = 1.0/2**(1/2.0)
target_pose.orientation.w = 1.0/2**(1/2.0)
waypoints.append(target_pose)
#Set the desired orientation for the end effector HERE
request.ik_request.pose_stamped.pose = target_pose
try:
#Send the request to the service
response = compute_ik(request)
group = MoveGroupCommander("right_arm")
#Creating a Robot Trajectory for the Path Planning
jump_thres = 0.0
eef_step = 0.01
path,fraction = group.compute_cartesian_path(waypoints, eef_step, jump_thres)
print("Path fraction: {}".format(fraction))
#Setting position and orientation target
group.set_pose_target(request.ik_request.pose_stamped)
#Setting the Joint constraint
group.set_path_constraints(constraints)
if fraction < 0.5:
group.go()
else:
group.execute(path)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
workspace_parameters = WorkspaceParameters()
workspace_parameters.header.stamp = rospy.Time.now()
workspace_parameters.header.frame_id = "/BASE"
workspace_parameters.min_corner = Vector3(-1.0, -1.0, -1.0)
workspace_parameters.max_corner = Vector3(1.0, 1.0, 1.0)
start_state = RobotState()
# start_state.joint_state.header.stamp = rospy.Time.now()
start_state.joint_state.header.frame_id = "/BASE"
start_state.joint_state.name = ["j1", "j2", "j3", "j4", "j5", "flange"]
start_state.joint_state.position = [-0.2569046038066598, -0.8442722962923348, 1.849082034218144, 0.26825374068443164, -0.04090683809444329, 5.745512865657193]
start_state.joint_state.velocity = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
jc2 = JointConstraint()
jc2.joint_name = "j1"
jc2.position = -1.37353344093
jc2.tolerance_above = 0.0001
jc2.tolerance_below = 0.0001
jc2.weight = 1.0
jc3 = JointConstraint()
jc3.joint_name = "j2"
jc3.position = -1.45013378543
jc3.tolerance_above = 0.0001
jc3.tolerance_below = 0.0001
jc3.weight = 1.0
jc4 = JointConstraint()
jc4.joint_name = "j3"
jc4.position = 2.18173030842
jc4.tolerance_above = 0.0001
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 main(argv):
# tf_buffer = tf2_ros.Buffer()
# tf_listener = tf2_ros.TransformListener(tf_buffer)
# target_detect_pose = tf_buffer.lookup_transform("end_effector", "base_footprint", rospy.Time())
listener = tf.TransformListener()
listener.waitForTransform('/base_footprint', '/end_effector', rospy.Time(),
rospy.Duration(4.0))
(trans, quat) = listener.lookupTransform('/base_footprint',
'/end_effector', rospy.Time(0))
client = actionlib.SimpleActionClient(
'/plan_and_execute_pose_w_joint_constraints',
PlanExecutePoseConstraintsAction)
client.wait_for_server()
goal = PlanExecutePoseConstraintsGoal()
# exit(0)
try:
goal.target_pose.header.frame_id = argv[0]
goal.target_pose.pose.position.x = trans[0]
goal.target_pose.pose.position.y = trans[1]
goal.target_pose.pose.position.z = trans[2] - 0.13
# theta = math.atan2(goal.target_pose.pose.position.y, goal.target_pose.pose.position.x)
# quat = quaternion_from_euler(0.0, 0.0, theta)
# print quat
goal.target_pose.pose.orientation.x = quat[0]
goal.target_pose.pose.orientation.y = quat[1]
goal.target_pose.pose.orientation.z = quat[2]
goal.target_pose.pose.orientation.w = quat[3]
joint_constraint_body_rotate_joint = JointConstraint()
joint_constraint_body_rotate_joint.joint_name = "body_rotate_joint"
joint_constraint_body_rotate_joint.position = float(0.0)
joint_constraint_body_rotate_joint.tolerance_above = float(
5) * math.pi / 180.0
joint_constraint_body_rotate_joint.tolerance_below = float(
5) * math.pi / 180.0
joint_constraint_body_rotate_joint.weight = 1.0
# joint_constraint_arm4_joint = JointConstraint()
# joint_constraint_arm4_joint.joint_name = "arm4_joint"
# joint_constraint_arm4_joint.position = float(0.0)
# joint_constraint_arm4_joint.tolerance_above = float(10) * math.pi / 180.0
# joint_constraint_arm4_joint.tolerance_below = float(10) * math.pi / 180.0
# joint_constraint_arm4_joint.weight = 1.0
# joint_constraint_arm6_joint = JointConstraint()
# joint_constraint_arm6_joint.joint_name = "arm6_joint"
# joint_constraint_arm6_joint.position = float(0.0)
# joint_constraint_arm6_joint.tolerance_above = float(10) * math.pi / 180.0
# joint_constraint_arm6_joint.tolerance_below = float(10) * math.pi / 180.0
# joint_constraint_arm6_joint.weight = 1.0
goal.joint_constraints.append(joint_constraint_body_rotate_joint)
# goal.joint_constraints.append(joint_constraint_arm6_joint)
except ValueError:
quit()
client.send_goal(goal)
client.wait_for_result()
print client.get_result()
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 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()
result = fk.getFK('arm_right_7_link', current_joint_states.name, correct_js.position, 'base_link')
rospy.loginfo("Result of current robot pose FK is: " + str(result))
rs = RobotState()
rs.joint_state = correct_js
drs = DisplayRobotState()
drs.state = rs
rs_pub.publish(drs)
rospy.loginfo("Published current robot state")
rospy.sleep(2.0)
c = Constraints()
jc = JointConstraint()
jc.joint_name = 'arm_right_1_link'
jc.position = 0.0
jc.tolerance_above = 0.00001
jc.tolerance_below = 0.00001
jc.weight = 1.0
c.joint_constraints.append(jc)
rospy.loginfo("Result without constraints:")
resultik = ik.getIK("right_arm", "right_arm_7_link", result.pose_stamped[0], False, 0, rs)
rospy.loginfo(str(resultik))
rs = RobotState()
rs.joint_state = resultik.solution.joint_state
drs = DisplayRobotState()
drs.state = rs
rs_pub.publish(drs)
rospy.loginfo("Published solution with False collision avoidance robot state")
def update(self):
self.logger.debug("{0}.update()".format(self.__class__.__name__))
if not self.action_client:
self.feedback_message = "no action client, did you call setup() on your tree?"
return py_trees.Status.INVALID
# pity there is no 'is_connected' api like there is for c++
if not self.sent_goal:
# if self.blackboard.frame_id != self.blackboard.target:
# print("Detected fail!")
# return py_trees.Status.SUCCESS
#update goal data from blackboard
self.action_goal.target_pose.header.frame_id = "base_footprint"
self.action_goal.target_pose.pose.position.x = self.blackboard.qrcode_pose.pose.position.x # + self.x_offset
self.action_goal.target_pose.pose.position.y = self.blackboard.qrcode_pose.pose.position.y # + self.y_offset
self.action_goal.target_pose.pose.position.z = self.blackboard.qrcode_pose.pose.position.z # + self.z_offset
#self.action_goal.target_pose.pose.position.x = self.x_offset
#self.action_goal.target_pose.pose.position.y = self.y_offset
#self.action_goal.target_pose.pose.position.z = self.z_offset
# rospy.loginfo(self.action_goal.target_pose.pose.position.x)
theta = math.atan2(self.action_goal.target_pose.pose.position.y,
self.action_goal.target_pose.pose.position.x)
quat = quaternion_from_euler(0.0, 0.0, theta)
self.action_goal.target_pose.pose.orientation.x = quat[0]
self.action_goal.target_pose.pose.orientation.y = quat[1]
self.action_goal.target_pose.pose.orientation.z = quat[2]
self.action_goal.target_pose.pose.orientation.w = quat[3]
# self.action_goal.target_pose.pose.orientation.x = 0
# self.action_goal.target_pose.pose.orientation.y = 0
# self.action_goal.target_pose.pose.orientation.z = 0
# self.action_goal.target_pose.pose.orientation.w = 1.0
if self.constraint:
if self.joint == "":
print("Constraint joint not defined.")
return py_trees.Status.FAILURE
joint_constraint = JointConstraint()
joint_constraint.joint_name = self.joint
joint_constraint.position = 0.0
joint_constraint.tolerance_above = 30 * math.pi / 180.0
joint_constraint.tolerance_below = 30 * math.pi / 180.0
joint_constraint.weight = 1.0
# arm6_joint makes singularity, so always constraint the arm6_joint.
joint_constraint_arm6_joint = JointConstraint()
joint_constraint_arm6_joint.joint_name = "arm6_joint"
joint_constraint_arm6_joint.position = float(0.0)
joint_constraint_arm6_joint.tolerance_above = float(
10) * math.pi / 180.0
joint_constraint_arm6_joint.tolerance_below = float(
10) * math.pi / 180.0
joint_constraint_arm6_joint.weight = 1.0
self.action_goal.joint_constraints.append(joint_constraint)
self.action_goal.joint_constraints.append(
joint_constraint_arm6_joint)
print("Joint constrained.")
# print("<== action goal ==>")
# print("Find : " + self.blackboard.frame_id)
# print("Goal position : (" + str(self.action_goal.target_pose.pose.position.x)
# + " , " + str(self.action_goal.target_pose.pose.position.y)
# + " , " + str(self.action_goal.target_pose.pose.position.z) + ") away from /base_footprint")
print("Goal position : (" +
str(self.action_goal.target_pose.pose.position.x) + " , " +
str(self.action_goal.target_pose.pose.position.y) + " , " +
str(self.action_goal.target_pose.pose.position.z) + ")")
print("Goal orientation : ", quat)
self.action_client.send_goal(self.action_goal)
self.sent_goal = True
self.feedback_message = "sent goal to the action server"
return py_trees.Status.RUNNING
self.feedback_message = self.action_client.get_goal_status_text()
if self.action_client.get_state() in [
actionlib_msgs.GoalStatus.ABORTED,
actionlib_msgs.GoalStatus.PREEMPTED
]:
return py_trees.Status.FAILURE
result = self.action_client.get_result()
if result:
return py_trees.Status.SUCCESS
else:
self.feedback_message = self.override_feedback_message_on_running
return py_trees.Status.RUNNING
workspace_parameters.min_corner = Vector3(-1.0, -1.0, -1.0)
workspace_parameters.max_corner = Vector3(1.0, 1.0, 1.0)
start_state = RobotState()
# start_state.joint_state.header.stamp = rospy.Time.now()
start_state.joint_state.header.frame_id = "/BASE"
start_state.joint_state.name = ["j1", "j2", "j3", "j4", "j5", "flange"]
start_state.joint_state.position = [
-0.2569046038066598, -0.8442722962923348, 1.849082034218144,
0.26825374068443164, -0.04090683809444329, 5.745512865657193
]
start_state.joint_state.velocity = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
jc2 = JointConstraint()
jc2.joint_name = "j1"
jc2.position = -1.37353344093
jc2.tolerance_above = 0.0001
jc2.tolerance_below = 0.0001
jc2.weight = 1.0
jc3 = JointConstraint()
jc3.joint_name = "j2"
jc3.position = -1.45013378543
jc3.tolerance_above = 0.0001
jc3.tolerance_below = 0.0001
jc3.weight = 1.0
jc4 = JointConstraint()
jc4.joint_name = "j3"
jc4.position = 2.18173030842
jc4.tolerance_above = 0.0001
def joint_position_callback(joints):
global plan_only
fixed_frame = rospy.get_param("/fixed_frame")
client = actionlib.SimpleActionClient('move_group', MoveGroupAction)
client.wait_for_server()
move_group_goal = MoveGroupGoal()
try:
msg = MotionPlanRequest()
workspace_parameters = WorkspaceParameters()
workspace_parameters.header.stamp = rospy.Time.now()
workspace_parameters.header.frame_id = fixed_frame
workspace_parameters.min_corner = Vector3(-1.0, -1.0, -1.0)
workspace_parameters.max_corner = Vector3(1.0, 1.0, 1.0)
start_state = RobotState()
# start_state.joint_state.header.stamp = rospy.Time.now()
start_state.joint_state.header.frame_id = fixed_frame
start_state.joint_state.name = []
start_state.joint_state.position = []
cons = Constraints()
cons.name = ""
i = 0
for dim in joints.start_joint.layout.dim:
start_state.joint_state.name.append(dim.label)
start_state.joint_state.position.append(joints.start_joint.data[i])
jc = JointConstraint()
jc.joint_name = dim.label
jc.position = joints.goal_joint.data[i]
jc.tolerance_above = 0.0001
jc.tolerance_below = 0.0001
jc.weight = 1.0
i = i + 1
cons.joint_constraints.append(jc)
msg.workspace_parameters = workspace_parameters
msg.start_state = start_state
msg.goal_constraints.append(cons)
msg.num_planning_attempts = 1
msg.allowed_planning_time = 5.0
msg.group_name = joints.group_name
move_group_goal.request = msg
if joints.plan_only:
plan_only = True
move_group_goal.planning_options.plan_only = True
else:
plan_only = False
client.send_goal(move_group_goal)
client.wait_for_result(rospy.Duration.from_sec(5.0))
except rospy.ROSInterruptException, e:
print "failed: %s"%e
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 print_pointlist(self, point_list):
##Add constrain
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)
self.group.set_path_constraints(constraints)
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]
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = self.group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
if fraction == 1:
success_num += 1
execute_plan = plan
if success_num == len(point_list):
self.group.execute(execute_plan)
elif success_num == 0:
break
else:
# execute success plan
self.msg_prit.data = True
self.group.execute(execute_plan)
break
self.msg_prit.data = False
# Delete the points what already execute
if success_num > 0:
del (point_list[0:success_num - 1])
# Add obstacle after printing (need to revise)
self.group.set_path_constraints(None)
print 'all finish'
# Notes on getting jacobian etc.
# Install pykdl_utils by checking out git into catkin_ws and catkin_make_isolated
if __name__ == '__main__':
rospy.init_node('touch_table', anonymous=True)
moveit_commander.roscpp_initialize(sys.argv)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group_name = "panda_arm"
move_group = moveit_commander.MoveGroupCommander(group_name)
path_constr = Constraints()
path_constr.name = "arm_constr"
joint_constraint = JointConstraint()
joint_constraint.position = 0.8
joint_constraint.tolerance_above = 3.14
joint_constraint.tolerance_below = 0.1
joint_constraint.weight = 1
joint_constraint.joint_name = "panda_joint2"
path_constr.joint_constraints.append(joint_constraint)
move_group.set_path_constraints(path_constr)
try:
touch_and_refine_table(robot, scene, move_group)
except KeyboardInterrupt:
print("Shutting down")
def print_pointlist(self,
point_list,
future_print_status=False,
name=None):
msg_experiment = String()
msg_experiment = name
self.experiment.publish(msg_experiment)
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
repeat_check = 0
previous_index = 0
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)
if executed_waypoint_index == previous_index:
repeat_check += 1
previous_index = executed_waypoint_index
if repeat_check >= 20: break
# 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)
# Tell plot node save data
msg_experiment = String()
msg_experiment = 'Finish'
self.experiment.publish(msg_experiment)
# Save result to csv
list_result = [[(finshtime - startime).seconds,
self.printing_time, self.planning_time,
(finshtime - startime).seconds - self.printing_time -
self.planning_time, number_printing_part]]
result_name = [
'All time', 'Printing time', 'planning time', 'Travel time',
'number of printing'
]
result_data = pd.DataFrame(columns=result_name, data=list_result)
# Plot Result
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')
plt.show()
# Save printing path
printing_result_list = [
full_point_list_x, full_point_list_y, self.re_position_x,
self.re_position_y
]
printing_data = pd.DataFrame(data=printing_result_list)
# Save result plot
if name:
plt3.savefig('experiment_data/' + str(name) + '/' + str(name) +
'_result.png',
dpi=plt3.dpi)
result_data.to_csv('experiment_data/' + str(name) + '/' +
str(name) + '_result.csv',
encoding='gbk')
printing_data.to_csv('experiment_data/' + str(name) + '/' +
str(name) + '_printpath.csv',
encoding='gbk')
print('All finish')
def main():
#Initialize the node
rospy.init_node('moveit_client')
# Create the SimpleActionClient, passing the type of the action
# (MoveGroupAction) to the constructor.
client = actionlib.SimpleActionClient('move_group', MoveGroupAction)
# Wait until the action server has started up and started
# listening for goals.
client.wait_for_server()
# Creates a goal to send to the action server.
goal = MoveGroupGoal()
#----------------Construct the goal message (start)----------------
joint_names = [
'head_pan', 'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0',
'left_w1', 'left_w2', 'right_s0', 'right_s1', 'right_e0', 'right_e1',
'right_w0', 'right_w1', 'right_w2'
]
#Set parameters for the planner
goal.request.group_name = 'both_arms'
goal.request.num_planning_attempts = 1
goal.request.allowed_planning_time = 5.0
#Define the workspace in which the planner will search for solutions
goal.request.workspace_parameters.min_corner.x = -1
goal.request.workspace_parameters.min_corner.y = -1
goal.request.workspace_parameters.min_corner.z = -1
goal.request.workspace_parameters.max_corner.x = 1
goal.request.workspace_parameters.max_corner.y = 1
goal.request.workspace_parameters.max_corner.z = 1
goal.request.start_state.joint_state.header.frame_id = "base"
#Set the start state for the trajectory
goal.request.start_state.joint_state.name = joint_names
goal.request.start_state.joint_state.position = [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0
]
goal.request.start_state.joint_state.velocity = [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0
]
print 'Enter start state:'
try:
for i in range(len(joint_names)):
val = float(raw_input(joint_names[i] + ':'))
goal.request.start_state.joint_state.position[i] = val
except:
print 'Using defaults for remaining joints'
print goal.request.start_state.joint_state.position
#Tell MoveIt whether to execute the trajectory after planning it
goal.planning_options.plan_only = True
#Set the goal position of the robot
#Note that the goal is specified with a collection of individual
#joint constraints, rather than a vector of joint angles
arm_joint_names = joint_names[1:]
target_joint_angles = [
0.5, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0
]
print 'Enter goal state:'
try:
for i in range(len(target_joint_angles)):
val = float(raw_input(joint_names[i] + ':'))
target_joint_angles[i] = val
except:
print 'Using defaults for remaining joints'
print target_joint_angles
tolerance = 0.0001
consts = []
for i in range(len(arm_joint_names)):
const = JointConstraint()
const.joint_name = arm_joint_names[i]
const.position = target_joint_angles[i]
const.tolerance_above = tolerance
const.tolerance_below = tolerance
const.weight = 1.0
consts.append(const)
goal.request.goal_constraints.append(
Constraints(name='', joint_constraints=consts))
#---------------Construct the goal message (end)-----------------
# Send the goal to the action server.
client.send_goal(goal)
# Wait for the server to finish performing the action.
client.wait_for_result()
# Print out the result of executing the action
print(client.get_result())
def joint_position_callback(joints):
global plan_only
fixed_frame = rospy.get_param("/fixed_frame")
client = actionlib.SimpleActionClient('move_group', MoveGroupAction)
client.wait_for_server()
move_group_goal = MoveGroupGoal()
try:
msg = MotionPlanRequest()
workspace_parameters = WorkspaceParameters()
workspace_parameters.header.stamp = rospy.Time.now()
workspace_parameters.header.frame_id = fixed_frame
workspace_parameters.min_corner = Vector3(-1.0, -1.0, -1.0)
workspace_parameters.max_corner = Vector3(1.0, 1.0, 1.0)
start_state = RobotState()
# start_state.joint_state.header.stamp = rospy.Time.now()
start_state.joint_state.header.frame_id = fixed_frame
start_state.joint_state.name = []
start_state.joint_state.position = []
cons = Constraints()
cons.name = ""
i = 0
for dim in joints.start_joint.layout.dim:
start_state.joint_state.name.append(dim.label)
start_state.joint_state.position.append(joints.start_joint.data[i])
jc = JointConstraint()
jc.joint_name = dim.label
jc.position = joints.goal_joint.data[i]
jc.tolerance_above = 0.0001
jc.tolerance_below = 0.0001
jc.weight = 1.0
i = i + 1
cons.joint_constraints.append(jc)
msg.workspace_parameters = workspace_parameters
msg.start_state = start_state
msg.goal_constraints.append(cons)
msg.num_planning_attempts = 1
msg.allowed_planning_time = 5.0
msg.group_name = joints.group_name
move_group_goal.request = msg
if joints.plan_only:
plan_only = True
move_group_goal.planning_options.plan_only = True
else:
plan_only = False
client.send_goal(move_group_goal)
client.wait_for_result(rospy.Duration.from_sec(5.0))
except rospy.ROSInterruptException, e:
print "failed: %s" % e
def plan_trajectory(self, start_point, goal_point, planner_number,
joint_names, group_name, planning_time,
planner_config_name):
"""
Given a start and goal point, returns the planned path.
Args:
start_point (list of float): A starting joint configuration.
goal_point (list of float): A goal joint configuration.
planner_number (int): The index of the planner to be used as
returned by acquire_planner().
joint_names (list of str): The name of the joints corresponding to
start_point and goal_point.
group_name (str): The name of the group to which the joint names
correspond.
planning_time (float): Maximum allowed time for planning, in seconds.
planner_config_name (str): Type of planner to use.
Return:
list of list of float: A sequence of points representing the joint
configurations at each point on the path.
"""
planner_client = rospy.ServiceProxy(self.planners[planner_number],
GetMotionPlan)
rospy.loginfo(
"Plan Trajectory Wrapper: got a plan_trajectory request for %s with start = %s and goal = %s"
% (self.planners[planner_number], start_point, goal_point))
# Put together the service request.
req = GetMotionPlanRequest()
req.motion_plan_request.workspace_parameters.header.stamp = rospy.get_rostime(
)
req.motion_plan_request.group_name = group_name
req.motion_plan_request.num_planning_attempts = 1
req.motion_plan_request.allowed_planning_time = planning_time
req.motion_plan_request.planner_id = planner_config_name #using RRT planner by default
req.motion_plan_request.start_state.joint_state.header.stamp = rospy.get_rostime(
)
req.motion_plan_request.start_state.joint_state.name = joint_names
req.motion_plan_request.start_state.joint_state.position = start_point
req.motion_plan_request.goal_constraints.append(Constraints())
req.motion_plan_request.goal_constraints[0].joint_constraints = []
for i in xrange(len(joint_names)):
temp_constraint = JointConstraint()
temp_constraint.joint_name = joint_names[i]
temp_constraint.position = goal_point[i]
temp_constraint.tolerance_above = 0.05
temp_constraint.tolerance_below = 0.05
req.motion_plan_request.goal_constraints[
0].joint_constraints.append(temp_constraint)
#call the planner
rospy.wait_for_service(self.planners[planner_number])
rospy.loginfo("Plan Trajectory Wrapper: sent request to service %s" %
planner_client.resolved_name)
try:
response = planner_client(req)
except rospy.ServiceException, e:
rospy.loginfo("Plan Trajectory Wrapper: service call failed: %s" %
e)
return None
def cw3_example_script():
"""
This script will go through the main aspects of moveit and the components you will need to complete the coursework.
You can find more information on
"""
# Initialize moveit_commander and rospy node
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface')
# Initialize moveit scene interface (woeld surrounding the robot)
scene = moveit_commander.PlanningSceneInterface()
# Robot contains the entire state of the robot (iiw a and shadow hand)
global group, base_group, arm_group
# request_prin = rospy.ServiceProxy('set_extruder_printing', SetBool)
msg_prit = SetBoolRequest()
request_fk = rospy.ServiceProxy('/compute_fk', GetPositionFK)
robot = moveit_commander.RobotCommander()
group = moveit_commander.MoveGroupCommander('robot')
arm_group = moveit_commander.MoveGroupCommander('arm')
# gripeer_group = moveit_commander.MoveGroupCommander('gripper')
base_group = moveit_commander.MoveGroupCommander('base')
# Create a DisplayTrajectory publisher which is used later to publish trajectories for RViz to visualize:
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
extruder_publisher = rospy.Publisher('set_extruder_rate',
Float32,
queue_size=20)
msg_extrude = Float32()
msg_extrude = 5.0
extruder_publisher.publish(msg_extrude)
group.allow_replanning(True)
group.allow_looking(True)
# Set the number of planning attempts to find better solution
group.set_num_planning_attempts(10)
################################################
###Add obstacle
rospy.sleep(0.2)
box_pose = geometry_msgs.msg.PoseStamped()
box_pose.header.frame_id = "odom"
box_pose.pose.position.x = 0
box_pose.pose.position.y = 2
box_pose.pose.position.z = 0.01
box_name = "wall"
scene.add_box(box_name, box_pose, size=(2, 2, 0.01))
rospy.sleep(0.2)
### Add cylinder
# p = PlanningSceneInterface("base_link")
# p.addCylinder("Cylinder", 0.01, 0.5, -1, 1, 0, wait=False)
# p.addMesh("iron_man", 0.01, 0.5, , wait=False)
p = PoseStamped()
p.header.frame_id = "odom"
p.pose.position.x = 0
p.pose.position.y = 0
p.pose.position.z = -0.05
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 1
scene.add_mesh("cylinder",p,'model/cylinder.stl')
rospy.sleep(0.2)
return
#############################################################
point_list = []
for i in range(10):
point_list.append((i * 0.2 -1, 1, 0.1))
for i in range(10):
point_list.append((1, i * 0.2 + 1, 0.1))
for i in range(10):
point_list.append((i * (-0.2) + 1, 3, 0.1))
for i in range(10):
point_list.append((-1, i * -0.2 + 3, 0.1))
point_list.append(point_list[0])
print(point_list)
#############################################################
########################################################################
##Add constrain
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)
#### 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)
group.set_path_constraints(constraints)
#######################################################################
while len(point_list) > 1:
# Move the robot point to first point and find the height
initial_plan = move_to_initial(point_list[1])
joint_goal = group.get_current_joint_values()
head = initial_plan.joint_trajectory.header
robot_state = 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 = request_fk(head, [group.get_end_effector_link()], robot_state)
current_pose = 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]
group.set_pose_target(current_pose)
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 = 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) = group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
if fraction == 1:
success_num += 1
execute_plan = plan
if success_num == len(point_list): group.execute(execute_plan)
else:
# execute success plan
msg_prit.data = True
group.execute(execute_plan)
break
msg_prit.data = False
# Delete the points what already execute
if success_num > 0: del(point_list[0:success_num-1])
#############################################################
print 'all finish'
# When finished shut down moveit_commander.
moveit_commander.roscpp_shutdown()
def update(self):
self.logger.debug("{0}.update()".format(self.__class__.__name__))
if not self.action_client:
self.feedback_message = "no action client, did you call setup() on your tree?"
return py_trees.Status.INVALID
# pity there is no 'is_connected' api like there is for c++
if not self.sent_goal:
self.listener.waitForTransform('/base_footprint', '/end_effector',
rospy.Time(), rospy.Duration(4.0))
(trans,
quat) = self.listener.lookupTransform('/base_footprint',
'/end_effector',
rospy.Time(0))
#update goal data from blackboard
self.action_goal.target_pose.header.frame_id = "base_footprint"
self.action_goal.target_pose.pose.position.x = trans[
0] + self.x_offset
self.action_goal.target_pose.pose.position.y = trans[
1] + self.y_offset
self.action_goal.target_pose.pose.position.z = trans[
2] + self.z_offset
self.action_goal.target_pose.pose.orientation.x = quat[0]
self.action_goal.target_pose.pose.orientation.y = quat[1]
self.action_goal.target_pose.pose.orientation.z = quat[2]
self.action_goal.target_pose.pose.orientation.w = quat[3]
if self.constraint:
if len(self.joint) == 0:
print("Constraint joint not defined.")
return py_trees.Status.FAILURE
for joint in self.joint:
joint_constraint = JointConstraint()
joint_constraint.joint_name = joint
joint_constraint.position = self.joint[
joint_constraint.joint_name][0]
joint_constraint.tolerance_above = self.joint[
joint_constraint.joint_name][1]
joint_constraint.tolerance_below = self.joint[
joint_constraint.joint_name][2]
joint_constraint.weight = 1.0
self.action_goal.joint_constraints.append(joint_constraint)
print("Joint constrained : " + str(joint_constraint))
print("Goal position : (" +
str(self.action_goal.target_pose.pose.position.x) + " , " +
str(self.action_goal.target_pose.pose.position.y) + " , " +
str(self.action_goal.target_pose.pose.position.z) + ")")
print("Goal orientation : ", quat)
self.action_client.send_goal(self.action_goal)
self.sent_goal = True
self.feedback_message = "sent goal to the action server"
return py_trees.Status.RUNNING
self.feedback_message = self.action_client.get_goal_status_text()
# Failure case
if self.action_client.get_state() in [
actionlib_msgs.GoalStatus.ABORTED,
actionlib_msgs.GoalStatus.PREEMPTED
]:
return py_trees.Status.FAILURE
result = self.action_client.get_result()
if result:
return py_trees.Status.SUCCESS
else:
self.feedback_message = self.override_feedback_message_on_running
return py_trees.Status.RUNNING
def main(p, a):
#Initialize the node
rospy.init_node('moveit_client')
# Create the SimpleActionClient, passing the type of the action
# (MoveGroupAction) to the constructor.
client = actionlib.SimpleActionClient('move_group', MoveGroupAction)
# Wait until the action server has started up and started
# listening for goals.
client.wait_for_server()
# Creates a goal to send to the action server.
goal = MoveGroupGoal()
#----------------Construct the goal message (start)----------------
joint_names = [
'head_pan', 'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0',
'left_w1', 'left_w2', 'right_s0', 'right_s1', 'right_e0', 'right_e1',
'right_w0', 'right_w1', 'right_w2'
]
#Set parameters for the planner
goal.request.group_name = 'both_arms'
goal.request.num_planning_attempts = 1
goal.request.allowed_planning_time = 5.0
#Define the workspace in which the planner will search for solutions
goal.request.workspace_parameters.min_corner.x = -1
goal.request.workspace_parameters.min_corner.y = -1
goal.request.workspace_parameters.min_corner.z = -1
goal.request.workspace_parameters.max_corner.x = 1
goal.request.workspace_parameters.max_corner.y = 1
goal.request.workspace_parameters.max_corner.z = 1
goal.request.start_state.joint_state.header.frame_id = "base"
#Set the start state for the trajectory
goal.request.start_state.joint_state.name = joint_names
if (len(p) == 15):
goal.request.start_state.joint_state.position = [
p[0], p[1], p[2], p[3], p[4], p[5], p[6], p[7], p[8], p[9], p[10],
p[11], p[12], p[13], p[14]
]
else:
print('Not enough positions: Need 15')
goal.request.start_state.joint_state.velocity = [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0
]
#Tell MoveIt whether to execute the trajectory after planning it
goal.planning_options.plan_only = True
#Set the goal position of the robot
#Note that the goal is specified with a collection of individual
#joint constraints, rather than a vector of joint angles
arm_joint_names = joint_names[1:]
if (len(a) == 15):
target_joint_angles = [
a[0], a[1], a[2], a[3], a[4], a[5], a[6], a[7], a[8], a[9], a[10],
a[11], a[12], a[13], a[14]
]
else:
print('Not enough angles: Need 15')
tolerance = 0.0001
consts = []
for i in range(len(arm_joint_names)):
const = JointConstraint()
const.joint_name = arm_joint_names[i]
const.position = target_joint_angles[i]
const.tolerance_above = tolerance
const.tolerance_below = tolerance
const.weight = 1.0
consts.append(const)
goal.request.goal_constraints.append(
Constraints(name='', joint_constraints=consts))
#---------------Construct the goal message (end)-----------------
# Send the goal to the action server.
client.send_goal(goal)
# Wait for the server to finish performing the action.
client.wait_for_result()
# Print out the result of executing the action
print(client.get_result())
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 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'
sv = StateValidity()
init_time = time.time() #rospy.Time.now()
result_sv = sv.getStateValidity(rs, "right_arm")
fin_time = time.time() #rospy.Time.now()
#rospy.loginfo("Call took: " + str((fin_time - init_time).secs) + "s " + str((fin_time - init_time).nsecs))
rospy.loginfo("Call took: " + str(fin_time - init_time))
rospy.loginfo("Result of state validity of current pose (must be valid):" +
str(result_sv))
exit(0)
c = Constraints()
jc = JointConstraint()
jc.joint_name = 'arm_right_1_link'
jc.position = 0.0
jc.tolerance_above = 0.00001
jc.tolerance_below = 0.00001
jc.weight = 1.0
c.joint_constraints.append(jc)
rospy.loginfo("Result without constraints:")
resultik = ik.getIK("right_arm", "right_arm_7_link",
result.pose_stamped[0], False, 0, rs)
rospy.loginfo(str(resultik))
rs = RobotState()
rs.joint_state = resultik.solution.joint_state
drs = DisplayRobotState()
drs.state = rs
rs_pub.publish(drs)
def main():
# Initialize the node
rospy.init_node("moveit_client")
# Create the SimpleActionClient, passing the type of the action
# (MoveGroupAction) to the constructor.
client = actionlib.SimpleActionClient("move_group", MoveGroupAction)
# Wait until the action server has started up and started
# listening for goals.
client.wait_for_server()
# Creates a goal to send to the action server.
goal = MoveGroupGoal()
# ----------------Construct the goal message (start)----------------
joint_names = [
"head_pan",
"left_s0",
"left_s1",
"left_e0",
"left_e1",
"left_w0",
"left_w1",
"left_w2",
"right_s0",
"right_s1",
"right_e0",
"right_e1",
"right_w0",
"right_w1",
"right_w2",
]
# Set parameters for the planner
goal.request.group_name = "both_arms"
goal.request.num_planning_attempts = 1
goal.request.allowed_planning_time = 5.0
# Define the workspace in which the planner will search for solutions
goal.request.workspace_parameters.min_corner.x = -1
goal.request.workspace_parameters.min_corner.y = -1
goal.request.workspace_parameters.min_corner.z = -1
goal.request.workspace_parameters.max_corner.x = 1
goal.request.workspace_parameters.max_corner.y = 1
goal.request.workspace_parameters.max_corner.z = 1
goal.request.start_state.joint_state.header.frame_id = "base"
# Set the start state for the trajectory
goal.request.start_state.joint_state.name = joint_names
goal.request.start_state.joint_state.position = [
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
]
goal.request.start_state.joint_state.velocity = [
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
]
# Tell MoveIt whether to execute the trajectory after planning it
goal.planning_options.plan_only = True
# Set the goal position of the robot
# Note that the goal is specified with a collection of individual
# joint constraints, rather than a vector of joint angles
arm_joint_names = joint_names[1:]
print("Joint angles...? Are coming one at a time. Give them to me in radians")
target_joint_angles = []
for joint in joint_names:
str = raw_input(joint + "\n-->")
val = float(str)
target_joint_angles.append(val)
print("I think that you want me to go to...")
print(target_joint_angles)
raw_input("Press any key to continue")
# target_joint_angles = [0.5, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
tolerance = 0.0001
consts = []
for i in range(len(arm_joint_names)):
const = JointConstraint()
const.joint_name = arm_joint_names[i]
const.position = target_joint_angles[i]
const.tolerance_above = tolerance
const.tolerance_below = tolerance
const.weight = 1.0
consts.append(const)
goal.request.goal_constraints.append(Constraints(name="", joint_constraints=consts))
# ---------------Construct the goal message (end)-----------------
# Send the goal to the action server.
client.send_goal(goal)
# Wait for the server to finish performing the action.
client.wait_for_result()
# Print out the result of executing the action
print(client.get_result())
def kinect_planner():
global fresh_data
global joint_target, pose_target, goal
global joints_req, pose_req
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('kinect_trajectory_planner', anonymous=True)
rate = rospy.Rate(0.5)
#rospy.sleep(3)
# Instantiate a RobotCommander object.
robot = moveit_commander.RobotCommander()
# Instantiate a PlanningSceneInterface object (interface to the world surrounding the robot).
scene = moveit_commander.PlanningSceneInterface()
print "Remember to disable firewall if it's not working"
# Instantiate MoveGroupCommander objects for arms and Kinect2.
group = moveit_commander.MoveGroupCommander("Kinect2_Target")
group_left_arm = moveit_commander.MoveGroupCommander("left_arm")
group_right_arm = moveit_commander.MoveGroupCommander("right_arm")
# We create this DisplayTrajectory publisher to publish
# trajectories for RVIZ to visualize.
display_trajectory_publisher = rospy.Publisher('planned_path',
moveit_msgs.msg.DisplayTrajectory, queue_size=5)
# Set the planner for Moveit
group.set_planner_id("RRTConnectkConfigDefault")
group.set_planning_time(8)
group.set_pose_reference_frame('base_footprint')
# Setting tolerance
group.set_goal_tolerance(0.08)
group.set_num_planning_attempts(10)
# Suscribing to the desired pose topic
target = rospy.Subscriber("desired_pose", PoseStamped, callback)
target_joint = rospy.Subscriber("desired_joints", pose_w_joints, callback_joints)
# Locating the arms and the Kinect2 sensor
group_left_arm_values = group_left_arm.get_current_joint_values()
group_right_arm_values = group_right_arm.get_current_joint_values()
#group_kinect_values = group_kinect.get_current_joint_values()
neck_init_joints = group.get_current_joint_values()
neck_init_joints[0] = -1.346
neck_init_joints[1] = -1.116
neck_init_joints[2] = -2.121
neck_init_joints[3] = 0.830
neck_init_joints[4] = 1.490
neck_init_joints[5] = 0.050
neck_init_joints[6] = 0
neck_init_joints[7] = 0
neck_init_joints[8] = 0
neck_init_joints[9] = 0
# Creating a box to limit the arm position
box_pose = PoseStamped()
box_pose.pose.orientation.w = 1
box_pose.pose.position.x = 0.6
box_pose.pose.position.y = 0.03
box_pose.pose.position.z = 1.5
box_pose.header.frame_id = 'base_footprint'
scene.add_box('box1', box_pose, (0.4, 0.4, 0.1))
rospy.sleep(2)
# Defining position constraints for the trajectory of the kinect
neck_const = Constraints()
neck_const.name = 'neck_constraints'
target_const = JointConstraint()
target_const.joint_name = "neck_joint_end"
target_const.position = 0.95
target_const.tolerance_above = 0.45
target_const.tolerance_below = 0.05
target_const.weight = 0.9 # Importance of this constraint
neck_const.joint_constraints.append(target_const)
# Talking to the robot
#client = actionlib.SimpleActionClient('/Kinect2_Target_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
client = actionlib.SimpleActionClient('/neck/follow_joint_trajectory', FollowJointTrajectoryAction)
print "====== Waiting for server..."
client.wait_for_server()
print "====== Connected to server"
while not rospy.is_shutdown():
print('.')
skip_iter = False
if fresh_data == True: #If a new pose is received, plan.
# Update arms position
group_left_arm_values = group_left_arm.get_current_joint_values()
group_right_arm_values = group_right_arm.get_current_joint_values()
# Set the target pose (or joint values) and generate a plan
if pose_req:
group.set_pose_target(pose_target)
if joints_req:
print('about to set joint target')
print(joint_target)
try:
if len(joint_target) == 6:
#Setting also values for the four virtual joints (one prismatic, and three rotational)
new_joint_target = joint_target + [0.1]*4
else:
new_joint_target = joint_target
print('setting: '),
print new_joint_target
group.set_joint_value_target(new_joint_target)
except:
print('cannot set joint target')
skip_iter = True
print "=========== Calculating trajectory... \n"
# Generate several plans and compare them to get the shorter one
plan_opt = dict()
differ = dict()
if not skip_iter:
try:
num = 1
rep = 0
# Generate several plans and compare them to get the shortest one
#for num in range(1,8):
while num < 7:
num += 1
plan_temp = group.plan()
move(plan_temp)
plan_opt[num] = goal.trajectory
diff=0
for point in goal.trajectory.points:
# calculate the distance between initial pose and planned one
for i in range(0,6):
diff = abs(neck_init_joints[i] - point.positions[i])+abs(diff)
differ[num] = diff
# If the current plan is good, take it
if diff < 110:
break
# If plan is too bad, don't consider it
if diff > 400:
num = num - 1
print "Plan is too long. Replanning."
rep = rep + 1
if rep > 4:
num = num +1
# If no plan was found...
if differ == {}:
print "---- Fail: No motion plan found. No execution attempted. Probably robot joints are out of range."
break
else:
# Select the shortest plan
min_plan = min(differ.itervalues())
select = [k for k, value in differ.iteritems() if value == min_plan]
goal.trajectory = plan_opt[select[0]]
#print " Plan difference:======= ", differ
#print " Selected plan:========= ", select[0]
# Remove the last 4 names and data from each point (dummy joints) before sending the goal
goal.trajectory.joint_names = goal.trajectory.joint_names[:6]
for point in goal.trajectory.points:
point.positions = point.positions[:6]
point.velocities = point.velocities[:6]
point.accelerations = point.accelerations[:6]
print "Sending goal"
client.send_goal(goal)
print "Waiting for result"
client.wait_for_result()
# Change the position of the virtual joint to avoid collision
neck_joints = group.get_current_joint_values()
print('neck joints:')
print neck_joints
#neck_joints[6] = 0.7
#group.set_joint_value_target(neck_joints)
#group.go(wait=True)
except (KeyboardInterrupt, SystemExit):
client.cancel_goal()
raise
rate.sleep()
fresh_data = False
rate.sleep()
def main(argv):
# root_dir = rospy.get_param("root_dir", default="")
root_dir = "/home/user/catkin_ws/src/zipsa_robot_simulation/living_lab_robot_perception"
plan_client = actionlib.SimpleActionClient('/plan_and_execute_pose',
PlanExecutePoseAction)
plan_client.wait_for_server()
constrained_plan_client = actionlib.SimpleActionClient(
'/plan_and_execute_pose_w_joint_constraints',
PlanExecutePoseConstraintsAction)
constrained_plan_client.wait_for_server()
pose_client = actionlib.SimpleActionClient('/pose_estimation',
PoseEstimationAction)
pose_client.wait_for_server()
rospy.wait_for_service(
'/clear_octomap'
) #this will stop your code until the clear octomap service starts running
clear_octomap = rospy.ServiceProxy('/clear_octomap', Empty)
# clear_octomap()
# exit(0)
remove_switching_pub = rospy.Publisher('/remove_points_switch',
String,
queue_size=1)
goal = PlanExecutePoseGoal()
sub_goal = PlanExecutePoseGoal()
tf_buffer = tf2_ros.Buffer()
br = tf2_ros.TransformBroadcaster()
listener = tf.TransformListener()
r = rospy.Rate(10)
"""
"004_sugar_box", # 1
"006_mustard_bottle", # 2
"005_tomato_soup_can", # 3
"024_bowl", # 4
"water_bottle", # 5
"coca_cola" # 6
"""
target_id = 2
initial_poses = np.load("{0}/predefined_poses/{1}.npy".format(
root_dir, target_id))
pose_goal = PoseEstimationGoal()
pose_goal.target_id = target_id
pose_client.send_goal(pose_goal)
pose_client.wait_for_result()
estimated_pose = pose_client.get_result().estimated_pose
estimated_pose = np.reshape(estimated_pose, (4, 4))
print(estimated_pose)
for initial_pose in initial_poses:
remove_switching_pub.publish("off")
print("initial_pose")
print(initial_pose)
# if initial_pose[2,3] < 0.12:
# continue
# if initial_pose[2,3] > 0.9:
# continue
# if initial_pose[0,3] < 0:
# continue
if np.sum(initial_pose) == 0:
continue
M = np.matmul(estimated_pose, initial_pose)
quat = quaternion_from_matrix(M)
try:
detect_pose = geometry_msgs.msg.TransformStamped()
detect_pose.header.frame_id = "camera_depth_optical_frame"
detect_pose.header.stamp = rospy.Time.now()
detect_pose.child_frame_id = "object_coordinate"
detect_pose.transform.translation.x = M[0, 3]
detect_pose.transform.translation.y = M[1, 3]
detect_pose.transform.translation.z = M[2, 3]
detect_pose.transform.rotation.x = quat[0]
detect_pose.transform.rotation.y = quat[1]
detect_pose.transform.rotation.z = quat[2]
detect_pose.transform.rotation.w = quat[3]
count = 0
while 1:
count += 1
detect_pose.header.stamp = rospy.Time.now()
br.sendTransform(detect_pose)
r.sleep()
try:
(trans,
rot) = listener.lookupTransform('/base_footprint',
'/object_coordinate',
rospy.Time(0))
break
except:
continue
# print("trans: ", trans)
# print("rot: ", rot)
# convert detect_pose with reference base_footprint
goal.target_pose.header.frame_id = "base_footprint"
goal.target_pose.pose.position.x = trans[0]
goal.target_pose.pose.position.y = trans[1]
goal.target_pose.pose.position.z = trans[2]
goal.target_pose.pose.orientation.x = rot[0]
goal.target_pose.pose.orientation.y = rot[1]
goal.target_pose.pose.orientation.z = rot[2]
goal.target_pose.pose.orientation.w = rot[3]
roll, pitch, yaw = euler_from_quaternion([
goal.target_pose.pose.orientation.x,
goal.target_pose.pose.orientation.y,
goal.target_pose.pose.orientation.z,
goal.target_pose.pose.orientation.w
])
# print(roll, pitch, yaw)
if yaw < -math.pi / 2.0:
yaw += math.pi
elif yaw > math.pi / 2.0:
yaw -= math.pi
quat = quaternion_from_euler(roll, pitch, yaw)
goal.target_pose.pose.orientation.x = quat[0]
goal.target_pose.pose.orientation.y = quat[1]
goal.target_pose.pose.orientation.z = quat[2]
goal.target_pose.pose.orientation.w = quat[3]
R = quaternion_matrix(quat)[:3, :3]
XL2 = np.matmul(R.T, trans)
XL2[0] -= 0.1
sub_trans = np.matmul(R, XL2)
sub_goal.target_pose.header.frame_id = "base_footprint"
sub_goal.target_pose.pose.position.x = sub_trans[0]
sub_goal.target_pose.pose.position.y = sub_trans[1]
sub_goal.target_pose.pose.position.z = sub_trans[2]
sub_goal.target_pose.pose.orientation.x = quat[0]
sub_goal.target_pose.pose.orientation.y = quat[1]
sub_goal.target_pose.pose.orientation.z = quat[2]
sub_goal.target_pose.pose.orientation.w = quat[3]
except ValueError:
quit()
print(sub_goal)
plan_client.send_goal(sub_goal)
plan_client.wait_for_result()
if plan_client.get_result().result == True:
remove_switching_pub.publish("on")
rospy.loginfo("Clearing Octomap")
clear_octomap()
rospy.wait_for_service('/arm_controller/query_state')
try:
query_state = rospy.ServiceProxy('/arm_controller/query_state',
QueryTrajectoryState)
resp = query_state(rospy.Time.now())
except rospy.ServiceException, e:
print "Service call failed: %s" % e
constrained_plan_goal = PlanExecutePoseConstraintsGoal()
constrained_plan_goal.target_pose = goal.target_pose
joint_names = resp.name
joint_positions = resp.position
joint_constraint = JointConstraint()
joint_constraint.joint_name = 'arm1_joint' # joint_names[2]
joint_constraint.position = joint_positions[2]
joint_constraint.tolerance_above = (np.pi / 6.0)
joint_constraint.tolerance_below = (np.pi / 6.0)
joint_constraint.weight = 1.0
constrained_plan_goal.joint_constraints.append(joint_constraint)
joint_constraint = JointConstraint()
joint_constraint.joint_name = 'arm4_joint' # joint_names[5]
joint_constraint.position = joint_positions[5]
joint_constraint.tolerance_above = (np.pi / 6.0)
joint_constraint.tolerance_below = (np.pi / 6.0)
joint_constraint.weight = 1.0
constrained_plan_goal.joint_constraints.append(joint_constraint)
joint_constraint = JointConstraint()
joint_constraint.joint_name = 'arm6_joint' # joint_names[7]
joint_constraint.position = joint_positions[7]
joint_constraint.tolerance_above = (np.pi / 6.0)
joint_constraint.tolerance_below = (np.pi / 6.0)
joint_constraint.weight = 1.0
constrained_plan_goal.joint_constraints.append(joint_constraint)
constrained_plan_client.send_goal(constrained_plan_goal)
constrained_plan_client.wait_for_result()
print constrained_plan_client.get_result()
if constrained_plan_client.get_result().result == True:
break
else: # Control the manipulator back to the home position.
client = actionlib.SimpleActionClient(
'/plan_and_execute_named_pose', PlanExecuteNamedPoseAction)
client.wait_for_server()
named_goal = PlanExecuteNamedPoseGoal()
named_goal.target_name = "home"
client.send_goal(named_goal)
client.wait_for_result()
print("=" * 50)
