Esempio n. 1
0
    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)
Esempio n. 2
0
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()