constraints.name = "down" orientation_constraint = OrientationConstraint() orientation_constraint.header.frame_id = group.get_planning_frame() orientation_constraint.link_name = group.get_end_effector_link() orientation_constraint.orientation = pose_target_1.orientation orientation_constraint.absolute_x_axis_tolerance = 3.1415 orientation_constraint.absolute_y_axis_tolerance = 0.05 orientation_constraint.absolute_z_axis_tolerance = 0.05 orientation_constraint.weight = 1.0 constraints.orientation_constraints.append(orientation_constraint) group.set_path_constraints(constraints) rospy.loginfo("Get Path Constraints:\n{}".format( group.get_path_constraints())) # Pose Target 2 rospy.loginfo("Start Pose Target 2") pose_target_2 = Pose() pose_target_2.position.x = 0.3 pose_target_2.position.y = -0.5 pose_target_2.position.z = 0.15 pose_target_2.orientation.x = 0.0 pose_target_2.orientation.y = -0.707 pose_target_2.orientation.z = 0.0 pose_target_2.orientation.w = 0.707 group.set_planner_id("RRTConnectkConfigDefault") group.allow_replanning(True)
print plan print fraction right_arm.execute(plan, wait=True) rospy.sleep(0.5) raw_input("please input") # [00000000] print right_arm.get_joint_value_target() print both_arms.get_joint_value_target() # no this functions # print right_arm.get_named_targets() print right_arm.get_remembered_joint_values() print both_arms.get_remembered_joint_values() print right_arm.get_path_constraints() print both_arms.get_path_constraints() print right_arm.get_active_joints() print both_arms.get_active_joints() print right_arm.get_current_joint_values() print right_arm.get_current_pose() print right_arm.get_current_rpy() print both_arms.get_current_joint_values() print both_arms.get_current_pose() print both_arms.get_current_rpy() right_arm.clear_pose_targets()