コード例 #1
0
    def plan_to_pose(self, target, orientation_constraints=None):
        """
        Generates a plan given an end effector pose subject to orientation constraints

        Inputs:
        target: A geometry_msgs/PoseStamped message containing the end effector pose goal
        orientation_constraints: A list of moveit_msgs/OrientationConstraint messages

        Outputs:
        path: A moveit_msgs/RobotTrajectory path
        """

        print "!!!!!!!IN PLAN_TO_POSE!!!!!!"
        self._group.set_pose_target(target)
        print "!!!!!!!SECOND!!!!!!"
        self._group.set_start_state_to_current_state()

        if orientation_constraints is not None:
            constraints = Constraints()
            constraints.orientation_constraints = orientation_constraints
            self._group.set_path_constraints(constraints)

        plan = self._group.plan()

        return plan
コード例 #2
0
def init_orient_constraints(x_tol, y_tol, z_tol):

    if x_tol < 0.1:
        x_tol = 0.1
    if y_tol < 0.1:
        y_tol = 0.1
    if z_tol < 0.1:
        z_tol = 0.1

    print x_tol, y_tol, z_tol
    cur_pose = manipulator.get_current_pose().pose
    upright_constraints = Constraints()

    upright_constraints.name = "upright"
    orientation_constraint = OrientationConstraint()
    # orientation_constraint.header = pose.header
    orientation_constraint.header.frame_id = "world"
    orientation_constraint.link_name = manipulator.get_end_effector_link()
    print "end link: ", manipulator.get_end_effector_link()
    orientation_constraint.orientation = cur_pose.orientation
    #IIWA
    orientation_constraint.absolute_x_axis_tolerance = x_tol
    orientation_constraint.absolute_y_axis_tolerance = y_tol
    orientation_constraint.absolute_z_axis_tolerance = z_tol

    orientation_constraint.weight = 1.0

    upright_constraints.orientation_constraints.append(orientation_constraint)
    return upright_constraints
コード例 #3
0
    def set_pose_constraints(self, tol_joint1, tol_joint2, tol_joint3):
        angle_constraints = Constraints()
        joint_constraint = JointConstraint()
        angle_constraints.name = "angle"
        joint_constraint.position = self.joint1_con
        joint_constraint.tolerance_above = tol_joint1
        joint_constraint.tolerance_below = tol_joint1
        joint_constraint.weight = 0.1
        joint_constraint.joint_name = "arm_shoulder_pan_joint"
        angle_constraints.joint_constraints.append(joint_constraint)

        joint_constraint2 = JointConstraint()
        joint_constraint2.position = self.joint2_con
        joint_constraint2.tolerance_above = tol_joint2
        joint_constraint2.tolerance_below = tol_joint2
        joint_constraint2.weight = 0.2
        joint_constraint2.joint_name = "arm_wrist_2_joint"
        angle_constraints.joint_constraints.append(joint_constraint2)

        joint_constraint3 = JointConstraint()
        joint_constraint3.position = self.joint3_con
        joint_constraint3.tolerance_above = tol_joint3
        joint_constraint3.tolerance_below = tol_joint3
        joint_constraint3.weight = 0.3
        joint_constraint3.joint_name = "arm_wrist_1_joint"
        angle_constraints.joint_constraints.append(joint_constraint3)
        group.set_path_constraints(angle_constraints)
コード例 #4
0
def init_upright_path_constraints():

	cur_pose = manipulator.get_current_pose().pose
	upright_constraints = Constraints()

	upright_constraints.name = "upright"
	orientation_constraint = OrientationConstraint()
	# orientation_constraint.header = pose.header
	orientation_constraint.header.frame_id = "world"
	orientation_constraint.link_name = manipulator.get_end_effector_link()
	print "end link: ", manipulator.get_end_effector_link()
	orientation_constraint.orientation = cur_pose.orientation
	#IIWA
	orientation_constraint.absolute_x_axis_tolerance = 0.1
	orientation_constraint.absolute_y_axis_tolerance = 0.1
	orientation_constraint.absolute_z_axis_tolerance = 3.14

	#UR5
	# orientation_constraint.absolute_x_axis_tolerance = 3.14
	# orientation_constraint.absolute_y_axis_tolerance = 0.1
	# orientation_constraint.absolute_z_axis_tolerance = 0.1

	orientation_constraint.weight = 1.0

	upright_constraints.orientation_constraints.append(orientation_constraint)

	return upright_constraints
コード例 #5
0
    def add_way_points(self, waypoints):

        arm = self.arm

        # 限制末端夹具运动
        right_joint_const = JointConstraint()
        right_joint_const.joint_name = "gripper_r_joint_r"
        right_joint_const.position = 0
        right_joint_const.weight = 1.0
        # 施加全约束
        consts = Constraints()
        consts.joint_constraints = [right_joint_const]
        arm.set_path_constraints(consts)

        wpose = arm.get_current_pose().pose
        wpose.orientation.x = Qux
        wpose.orientation.y = Quy
        wpose.orientation.z = Quz
        wpose.orientation.w = Quw
        wpose.position.x = a[5]
        wpose.position.y = a[3]
        wpose.position.z = a[4]

        waypoints.append(copy.deepcopy(wpose))

        # print waypoints
        # print "----------------\n"
        return waypoints
コード例 #6
0
    def set_pose_quat_target(self, pose):
        ############################
        rospy.logwarn("set_pose_quat_target")

        # Create a contraints list and give it a name
        constraints = Constraints()
        constraints.name = "Keep gripper horizontal"
        start_pose = self.arm.get_current_pose(self.end_effector_link)
        rospy.logwarn(self.end_effector_link)
        rospy.logwarn("start_pose:")
        rospy.logwarn(start_pose)
        rospy.logwarn("pose:")
        rospy.logwarn(pose)
        # Create an orientation constraint for the right gripper
        orientation_constraint = OrientationConstraint()
        orientation_constraint.header = start_pose.header
        orientation_constraint.link_name = self.end_effector_link
        orientation_constraint.orientation.x = start_pose.pose.orientation.x
        orientation_constraint.orientation.y = start_pose.pose.orientation.y
        orientation_constraint.orientation.z = start_pose.pose.orientation.z
        orientation_constraint.orientation.w = start_pose.pose.orientation.w
        orientation_constraint.absolute_x_axis_tolerance = 0.1
        orientation_constraint.absolute_y_axis_tolerance = 0.1
        orientation_constraint.absolute_z_axis_tolerance = 3.14
        orientation_constraint.weight = 1.0

        # Append the constraint to the list of contraints
        constraints.orientation_constraints.append(orientation_constraint)
        rospy.logwarn("constraints:")
        rospy.logwarn(constraints)

        # Set the path constraints on the right_arm
        self.arm.set_path_constraints(constraints)
        ############################
        self.arm.set_pose_target(pose, self.end_effector_link)
コード例 #7
0
ファイル: controller_x.py プロジェクト: Ehrensverd/contrutle
    def move_ik(self, xyz):
        move_group = self.arm_group

        fixed_end_effector = OrientationConstraint()
        fixed_end_effector.link_name = "end_effector_link"
        fixed_end_effector.header.frame_id = "link1"
        fixed_end_effector.orientation.w = 1.0

        fixed_end_effector.absolute_x_axis_tolerance = 0.1
        fixed_end_effector.absolute_y_axis_tolerance = 0.1
        fixed_end_effector.absolute_z_axis_tolerance = 3.14

        fixed_end_effector.weight = 1.0

        constraint = Constraints()
        constraint.name = "tilt constraint"
        constraint.orientation_constraints = [fixed_end_effector]

        move_group.set_path_constraints(constraint)

        move_group.set_position_target(xyz)

        try:
            move_group.go(wait=True)
        except MoveItCommanderException:
            print "sorry cant move here!"
        # move_group.stop()
        self.moving = False
コード例 #8
0
ファイル: path_planner.py プロジェクト: gkimball1/maze-bot
    def plan_straight(self, waypoint, orientation_constraints):
        """
        Generates a plan given an end effector pose subject to orientation constraints

        Inputs:
        target: A geometry_msgs/PoseStamped message containing the end effector pose goal
        orientation_constraints: A list of moveit_msgs/OrientationConstraint messages

        Outputs:
        path: A moveit_msgs/RobotTrajectory path
        """
        # waypoint is a list of poses
        # self._group.set_pose_target(target)
        # self._group.set_start_state_to_current_state()

        constraints = Constraints()
        constraints.orientation_constraints = orientation_constraints
        self._group.set_path_constraints(constraints)
        #self._scene.setMaxVelocityScalingFactor(0.5)

        (plan, fraction) = self._group.compute_cartesian_path(
            waypoint,  # waypoints to follow
            0.01,  # eef_step
            0.0)

        return plan, fraction
コード例 #9
0
    def right_arm_go_to_pose_goal(self):
        # 设置动作对象变量,此处为arm
        right_arm = self.right_arm

        # Create a path constraint for the arm
        # UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS
        joint_const = JointConstraint()
        joint_const.joint_name = "gripper_r_joint_r"
        joint_const.position = 0
        consts = Constraints()
        consts.joint_constraints = [joint_const]
        right_arm.set_path_constraints(consts)

        # 获取当前末端执行器位置姿态
        pose_goal = right_arm.get_current_pose().pose
        # print (a)
        # print (Qux, Quy, Quz, Quw)
        # 设置动作对象目标位置姿态
        pose_goal.orientation.x = Right_Qux
        pose_goal.orientation.y = Right_Quy
        pose_goal.orientation.z = Right_Quz
        pose_goal.orientation.w = Right_Quw
        pose_goal.position.x = Neurondata[5]
        pose_goal.position.y = Neurondata[3]
        pose_goal.position.z = Neurondata[4]
        right_arm.set_pose_target(pose_goal)
        print "End effector pose %s" % pose_goal

        # 规划和输出动作
        traj = right_arm.plan()
        right_arm.execute(traj, wait=False)
        # 动作完成后清除目标信息
        right_arm.clear_pose_targets()
        # 确保没有剩余未完成动作在执行
        right_arm.stop()
コード例 #10
0
def plan_motion_constrained(commander, goal_pose):
    #Define Pose
    goal = PoseStamped()
    goal.header.frame_id = "base"
    goal.pose.position.x = goal_pose[0]
    goal.pose.position.y = goal_pose[1]
    goal.pose.position.z = goal_pose[2]
    goal.pose.orientation.x = goal_pose[3]
    goal.pose.orientation.y = goal_pose[4]
    goal.pose.orientation.z = goal_pose[5]
    goal.pose.orientation.w = goal_pose[6]
    commander.set_pose_target(goal)
    commander.set_start_state_to_current_state()
    orien_const = OrientationConstraint()
    orien_const.link_name = "right_gripper";
    orien_const.header.frame_id = "base";
    orien_const.orientation.y = -1.0;
    orien_const.absolute_x_axis_tolerance = 0.1;
    orien_const.absolute_y_axis_tolerance = 0.1;
    orien_const.absolute_z_axis_tolerance = 0.1;
    orien_const.weight = 1.0;
    consts = Constraints()
    consts.orientation_constraints = [orien_const]
    commander.set_path_constraints(consts)
    #Plan a path
    plan = commander.plan()
    #Return plan
    return plan
コード例 #11
0
def append_traj_to_move_group_goal(goal_to_append=None, goal_pose=Pose(), link_name=None):
    """ Appends a trajectory_point to the given move_group goal, returns it appended"""
    if goal_to_append == None:
        rospy.logerr("append_trajectory_point_to_move_group_goal needs a goal!")
        return
    #goal_to_append = MoveGroupGoal()
    #rospy.logwarn("goal_to_append is: \n" + str(goal_to_append))
    traj_c = TrajectoryConstraints()
    goal_c = Constraints()
    goal_c.name = "traj_constraint"
    # Position constraint
    position_c = PositionConstraint()
    position_c.header = goal_to_append.request.goal_constraints[0].position_constraints[0].header
    position_c.link_name = goal_to_append.request.goal_constraints[0].position_constraints[0].link_name if link_name == None else link_name
    position_c.constraint_region.primitives.append(SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01]))
    position_c.constraint_region.primitive_poses.append(goal_pose)
    position_c.weight = 2.0
    goal_c.position_constraints.append(position_c)
    # Orientation constraint
    orientation_c = OrientationConstraint()
    orientation_c.header = goal_to_append.request.goal_constraints[0].position_constraints[0].header
    orientation_c.link_name = goal_to_append.request.goal_constraints[0].position_constraints[0].link_name if link_name == None else link_name
    orientation_c.orientation = goal_pose.orientation
    orientation_c.absolute_x_axis_tolerance = 0.01
    orientation_c.absolute_y_axis_tolerance = 0.01
    orientation_c.absolute_z_axis_tolerance = 0.01
    orientation_c.weight = 1.0
    goal_c.orientation_constraints.append(orientation_c)
    
    traj_c.constraints.append(goal_c)
    goal_to_append.request.trajectory_constraints = traj_c
    
    return goal_to_append
コード例 #12
0
ファイル: path_planner.py プロジェクト: ucb-ee106/proj1_pkg
    def plan_to_pose(self, target, orientation_constraints=None):
        """
        Generates a plan given an end effector pose subject to orientation constraints

        Inputs:
        target: A geometry_msgs/PoseStamped message containing the end effector pose goal
        orientation_constraints: A list of moveit_msgs/OrientationConstraint messages

        Outputs:
        success: boolean success flag.
        plan: A moveit_msgs/RobotTrajectory path.
        time_taken: planning time.
        error_code: MoveitErrorCodes.
        """

        self._group.set_pose_target(target)
        self._group.set_start_state_to_current_state()

        if orientation_constraints is not None:
            constraints = Constraints()
            constraints.orientation_constraints = orientation_constraints
            self._group.set_path_constraints(constraints)

        success, plan, time_taken, error_code = self._group.plan()
        return success, plan, time_taken, error_code
コード例 #13
0
def create_basic_mp_position_request(
        planning_frame,
        link_name,
        target_point_offset,
        planner_id):
    motion_plan_request = MotionPlanRequest()
    motion_plan_request.group_name = move_group
    motion_plan_request.num_planning_attempts = 1
    motion_plan_request.allowed_planning_time = 5.0
    motion_plan_request.workspace_parameters = WorkspaceParameters()
    motion_plan_request.max_velocity_scaling_factor = 0.5
    motion_plan_request.max_acceleration_scaling_factor = 0.5
    motion_plan_request.planner_id = planner_id

    position_constraints = []
    position_constraint = PositionConstraint()
    header = std_msgs.msg.Header()
    header.frame_id = planning_frame
    position_constraint.header = header
    position_constraint.link_name = link_name
    position_constraint.target_point_offset = target_point_offset
    position_constraints = [position_constraint]
    constraint = Constraints()
    constraint.position_constraints = position_constraints
    constraints = [constraint]
    motion_plan_request.goal_constraints = constraints
    return motion_plan_request
コード例 #14
0
ファイル: arm_kinematics.py プロジェクト: henryleou/blackjack
    def setConstr(self, orien_const, right):
        constraints = Constraints()
        constraints.orientation_constraints = orien_const

        if right > 0:
            self._groupR.set_path_constraints(constraints)
        else:
            self._groupL.set_path_constraints(constraints)
コード例 #15
0
    def move(self, num):

        moveit_commander.roscpp_initialize(sys.argv)
        # rospy.init_node("moveit_node")

        robot = moveit_commander.RobotCommander()
        scene = moveit_commander.PlanningSceneInterface()
        group = moveit_commander.MoveGroupCommander('right_arm')
        right_arm = baxter_interface.Limb("right")
        right_arm_pose = right_arm.endpoint_pose()
        # print(right_arm_pose)
        initial_orient = [
            right_arm_pose['orientation'].x, right_arm_pose['orientation'].y,
            right_arm_pose['orientation'].z, right_arm_pose['orientation'].w
        ]

        dig_class = dg.Digits()
        digit_shapes = dig_class.digit
        num_shape = digit_shapes[str(num)]
        i = 0
        # print(num_shape, num, len(num_shape))
        while i < len(num_shape):
            (x, y) = num_shape[i]
            pose_target = PoseStamped()
            pose_target.header.frame_id = "base"
            # print(pose_target.pose.position)
            right_arm_pose = right_arm.endpoint_pose()
            x = right_arm_pose['position'].x
            y = right_arm_pose['position'].y
            z = right_arm_pose['position'].z
            print(right_arm_pose)
            pose_target.pose.position.x = x + num_shape[i][0] * 0.1
            pose_target.pose.position.y = y + num_shape[i][1] * 0.1
            pose_target.pose.position.z = z
            pose_target.pose.orientation.x = initial_orient[0]
            pose_target.pose.orientation.y = initial_orient[1]
            pose_target.pose.orientation.z = initial_orient[2]
            pose_target.pose.orientation.w = initial_orient[3]
            group.set_pose_target(pose_target)
            group.set_start_state_to_current_state()
            orien_const = OrientationConstraint()
            orien_const.link_name = "right_gripper"
            orien_const.header.frame_id = "base"
            orien_const.orientation.x = initial_orient[0]
            orien_const.orientation.y = initial_orient[1]
            orien_const.orientation.z = initial_orient[2]
            orien_const.orientation.w = initial_orient[3]
            orien_const.absolute_x_axis_tolerance = 0.1
            orien_const.absolute_y_axis_tolerance = 0.1
            orien_const.absolute_z_axis_tolerance = 0.1
            orien_const.weight = 1.0
            consts = Constraints()
            consts.orientation_constraints = [orien_const]
            group.set_path_constraints(consts)
            plan = group.plan()
            group.execute(plan)
            i += 1
コード例 #16
0
    def right_arm_go_to_pose_goal(self):
        # 设置动作对象变量,此处为arm
        right_arm = self.right_arm

        # 获取当前末端执行器位置姿态
        pose_goal = right_arm.get_current_pose().pose

        # 限制末端夹具运动
        right_joint_const = JointConstraint()
        right_joint_const.joint_name = "gripper_r_joint_r"
        # if Rightfinger > -55 :
        #     right_joint_const.position = 0.024
        # else:
        right_joint_const.position = 0
        right_joint_const.weight = 1.0
        # 施加全约束
        consts = Constraints()
        consts.joint_constraints = [right_joint_const]
        right_arm.set_path_constraints(consts)

        # 计算夹取姿态
        if obj_theta <= 0:
            (Qux, Quy, Quz,
             Quw) = quaternion_from_euler(90.01 / 180 * pi, 0,
                                          (-180 - obj_theta) / 180 * pi)
        else:
            (Qux, Quy, Quz,
             Quw) = quaternion_from_euler(90.01 / 180 * pi, 0,
                                          (180 - obj_theta) / 180 * pi)

        # (Qux, Quy, Quz, Quw) = quaternion_from_euler(90/180*pi, 0, -180/180*pi)

        # 设置动作对象目标位置姿态
        pose_goal.orientation.x = Qux
        pose_goal.orientation.y = Quy
        pose_goal.orientation.z = Quz
        pose_goal.orientation.w = Quw
        # pose_goal.position.y = 0.0244387395252 + (-0.595877665686-0.0244387395252)*obj_x/320
        # pose_goal.position.x = 0.625989932306 + (0.197518221397-0.625989932306)*obj_y/240
        pose_goal.position.x = 0.1819576873 + (160 - obj_y) * (
            0.494596343128 - 0.1819576873) / 160
        pose_goal.position.y = obj_x * (
            -0.455644324437 + 0.0238434066464) / 220 - 0.0238434066464
        pose_goal.position.z = 0.0942404372508
        right_arm.set_pose_target(pose_goal)
        print "End effector pose %s" % pose_goal

        # 规划和输出动作
        traj = right_arm.plan()
        right_arm.execute(traj, wait=False)
        # 动作完成后清除目标信息
        right_arm.clear_pose_targets()
        # 清除路径约束
        right_arm.clear_path_constraints()
        # 确保没有剩余未完成动作在执行
        right_arm.stop()
コード例 #17
0
ファイル: path_planner.py プロジェクト: gkimball1/maze-bot
    def plan_to_pose(self, target, orientation_constraints):
        self._group.set_pose_target(target)
        self._group.set_start_state_to_current_state()

        constraints = Constraints()
        constraints.orientation_constraints = orientation_constraints
        self._group.set_path_constraints(constraints)

        plan = self._group.plan()

        return plan
コード例 #18
0
ファイル: twist_ik_control.py プロジェクト: dHutchings/ee106a
def move_to_coord(trans,
                  rot,
                  arm,
                  which_arm='left',
                  keep_oreint=False,
                  base="base"):
    #coordinates are in baxter's torso!
    goal = PoseStamped()
    goal.header.frame_id = base

    # x, y, and z position
    goal.pose.position.x = trans[0]
    goal.pose.position.y = trans[1]
    goal.pose.position.z = trans[2]

    # Orientation as a quaternion
    goal.pose.orientation.x = rot[0]
    goal.pose.orientation.y = rot[1]
    goal.pose.orientation.z = rot[2]
    goal.pose.orientation.w = rot[3]

    # Set the goal state to the pose you just defined
    arm.set_pose_target(goal)

    # Set the start state for the arm
    arm.set_start_state_to_current_state()

    if keep_oreint:
        # Create a path constraint for the arm
        orien_const = OrientationConstraint()
        orien_const.link_name = which_arm + "_gripper"
        orien_const.header.frame_id = "base"

        #constrain it to be the same as my goal state.  Seems reasonable.

        orien_const.orientation.x = rot[0]
        orien_const.orientation.y = rot[1]
        orien_const.orientation.z = rot[2]
        orien_const.orientation.w = rot[3]
        orien_const.absolute_x_axis_tolerance = 0.1
        orien_const.absolute_y_axis_tolerance = 0.1
        orien_const.absolute_z_axis_tolerance = 0.1
        orien_const.weight = 1.0
        consts = Constraints()
        consts.orientation_constraints = [orien_const]
        print(consts)
        arm.set_path_constraints(consts)

    # Plan a path
    arm_plan = arm.plan()

    # Execute the plan
    arm.execute(arm_plan)
コード例 #19
0
def constraint_planner(start_robot_state, goal_config, group_handle, planner_name, planning_attemps, planning_time):
	
	planning_workspace = WorkspaceParameters();
	planning_workspace.header.frame_id = "/base_link";
	planning_workspace.header.stamp = rospy.Time.now();
	planning_workspace.min_corner.x = -1;
	planning_workspace.min_corner.y = -1;
	planning_workspace.min_corner.z = -1;	
	planning_workspace.max_corner.x = 1;
	planning_workspace.max_corner.y = 1;
	planning_workspace.max_corner.z = 1;

	# Set Start
	start_state = RobotState();
	start_state = start_robot_state;
	print ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>";
	#print start_robot_state;
	
	# Set Goal
	goal_state = Constraints();
	Jnt_constraint = setJointConstraints(goal_config, group_handle);
	goal_state.joint_constraints = Jnt_constraint;
	#print Jnt_constraint;
	
	# Set Constraints
	des_w = -0.070099;
	des_x = 0.41382;
	des_y = -0.57302;
	des_z = 0.70391;
	rotation_constraints = setOrientationConstraints(des_w,des_x,des_y,des_z,group_handle, weight = 1.0);
	
	# Generating Request
	planningRequest = MotionPlanRequest();
	planningRequest.workspace_parameters = planning_workspace;
	
	planningRequest.start_state = start_robot_state;
	planningRequest.goal_constraints.append(goal_state);
	
	# Setting Constraint
	planningRequest.path_constraints.name = 'scoop_constraint';
	planningRequest.path_constraints.orientation_constraints.append(rotation_constraints);
	
	#traj_constraint = Constraints();
	#traj_constraint.orientation_constraints.append(rotation_constraints);
	#planningRequest.trajectory_constraints.constraints.append(traj_constraint);
	
	planningRequest.planner_id = "RRTConnectkConfigDefault";
	planningRequest.group_name = group_handle.get_name();
	planningRequest.num_planning_attempts = planning_attemps;
	planningRequest.allowed_planning_time = planning_time;
	planningRequest.max_velocity_scaling_factor = 1.0;
	
	Planning_req.publish(planningRequest);
コード例 #20
0
    def test_MoveTo(self):
        stage = stages.MoveTo("move", self.planner)

        self._check(stage, "group", "group")
        self._check(stage, "ik_frame", PoseStamped())
        self._check(stage, "path_constraints", Constraints())
        stage.setGoal(PoseStamped())
        stage.setGoal(PointStamped())
        stage.setGoal(RobotState())
        stage.setGoal("named pose")
        stage.setGoal(dict(joint1=1.0, joint2=2.0))
        self._check(stage, "path_constraints", Constraints())
コード例 #21
0
def move_to_coord(trans, rot, keep_oreint=False):
    #coordinates are in baxter's torso!

    global arm
    goal = PoseStamped()
    goal.header.frame_id = "base"

    # x, y, and z position
    goal.pose.position.x = trans[0]
    goal.pose.position.y = trans[1]
    goal.pose.position.z = trans[2]


    # Orientation as a quaternion
    goal.pose.orientation.x = rot[0]
    goal.pose.orientation.y = rot[1]
    goal.pose.orientation.z = rot[2]
    goal.pose.orientation.w = rot[3]

    # Set the goal state to the pose you just defined
    arm.set_pose_target(goal)

    # Set the start state for the arm
    arm.set_start_state_to_current_state()

    if keep_oreint:
        # Create a path constraint for the arm
        orien_const = OrientationConstraint()
        orien_const.link_name = 'left'+"_gripper";
        orien_const.header.frame_id = "base";

        #constrain it to be the same as my goal state.  Seems reasonable.

        orien_const.orientation.x = rot[0];
        orien_const.orientation.y = rot[1];
        orien_const.orientation.z = rot[2];
        orien_const.orientation.w = rot[3];
        orien_const.absolute_x_axis_tolerance = 0.02;
        orien_const.absolute_y_axis_tolerance = 0.02;
        orien_const.absolute_z_axis_tolerance = 0.02;
        orien_const.weight = 1.0;
        consts = Constraints()
        consts.orientation_constraints = [orien_const]
        print(consts)
        arm.set_path_constraints(consts)

    # Plan a path
    arm_plan = arm.plan()

    # Execute the plan
    #arm.execute(arm_plan)
    arm.go(arm_plan,wait=True)
コード例 #22
0
def create_constraint(name):
  orien_const = OrientationConstraint()
  orien_const.link_name = name;
  orien_const.header.frame_id = "base";
  orien_const.orientation.y = 2**.5/2
  orien_const.orientation.w = 2**.5/2
  orien_const.absolute_x_axis_tolerance = 0.1;
  orien_const.absolute_y_axis_tolerance = 0.1;
  orien_const.absolute_z_axis_tolerance = 0.1;
  orien_const.weight = 3.0;
  consts = Constraints()
  consts.orientation_constraints = [orien_const]
  return consts
コード例 #23
0
ファイル: right_arm.py プロジェクト: robmck1995/BlackJack
def right_to_player(right_arm, right_gripper):
    # get current number of cards player has
    global player_cards
    # get global deck coordinates
    global deck_x
    global deck_y
    global deck_z

    # create pose above drop point for player's card
    card = PoseStamped()
    card.header.frame_id = "base"

    # x, y, and z position of card
    card.pose.position.x = deck_x + .25
    card.pose.position.y = deck_y + .2 + (.1 * player_cards)
    card.pose.position.z = deck_z

    # Orientation as a quaternion - straight down
    card.pose.orientation.x = 0.0
    card.pose.orientation.y = -1.0
    card.pose.orientation.z = 0.0
    card.pose.orientation.w = 0.0

    # Plan and execute path to desired card position from current state
    right_arm.set_pose_target(card)
    right_arm.set_start_state_to_current_state()

    # Add orientation constraint for path - straight down
    orien_const = OrientationConstraint()
    orien_const.link_name = "right_gripper"
    orien_const.header.frame_id = "base"
    orien_const.orientation.y = -1.0
    orien_const.absolute_x_axis_tolerance = 0.1
    orien_const.absolute_y_axis_tolerance = 0.1
    orien_const.absolute_z_axis_tolerance = 0.1
    orien_const.weight = 1.0
    consts = Constraints()
    consts.orientation_constraints = [orien_const]
    right_arm.set_path_constraints(consts)

    right_plan = right_arm.plan()
    right_arm.execute(right_plan)

    # drop card
    right_gripper.stop()

    # sleep for a moment
    rospy.sleep(1.0)

    # increment number of cards player has
    player_cards += 1
コード例 #24
0
    def move_to(self,
                pos_x,
                pos_y,
                pos_z,
                orien_x,
                orien_y,
                orien_z,
                orien_w,
                ik,
                orien_const=None):
        #Construct the request
        request = GetPositionIKRequest()
        request.ik_request.group_name = "right_arm"
        request.ik_request.ik_link_name = "right_gripper"
        request.ik_request.attempts = 20
        request.ik_request.pose_stamped.header.frame_id = "base"

        #Set the desired orientation for the end effector HERE
        request.ik_request.pose_stamped.pose.position.x = pos_x
        request.ik_request.pose_stamped.pose.position.y = pos_y
        request.ik_request.pose_stamped.pose.position.z = pos_z
        request.ik_request.pose_stamped.pose.orientation.x = orien_x
        request.ik_request.pose_stamped.pose.orientation.y = orien_y
        request.ik_request.pose_stamped.pose.orientation.z = orien_z
        request.ik_request.pose_stamped.pose.orientation.w = orien_w

        try:
            #Send the request to the service
            response = ik(request)

            #Print the response HERE
            print(response)
            group = MoveGroupCommander("right_arm")

            if orien_const is not None:
                constraints = Constraints()
                constraints.orientation_constraints = orien_const
                group.set_path_constraints(constraints)

            # Setting position and orientation target
            group.set_pose_target(request.ik_request.pose_stamped)

            # TRY THIS
            # Setting just the position without specifying the orientation
            # group.set_position_target([0.5, 0.5, 0.0])

            # Plan IK and execute
            group.go()

        except rospy.ServiceException, e:
            print("Service call failed: %s")
コード例 #25
0
    def right_arm_go_down_goal(self):
        # 设置动作对象变量,此处为arm
        right_arm = self.right_arm

        waypoints = []

        # 获取当前末端执行器位置姿态
        pose_goal = right_arm.get_current_pose().pose

        # 添加路径起始点
        waypoints.append(copy.deepcopy(pose_goal))

        # 限制末端夹具运动
        right_joint_const = JointConstraint()
        right_joint_const.joint_name = "gripper_r_joint_r"
        # if Rightfinger > -55 :
        #     right_joint_const.position = 0.024
        # else:
        right_joint_const.position = 0.0239
        right_joint_const.weight = 1.0
        # 施加全约束
        consts = Constraints()
        consts.joint_constraints = [right_joint_const]
        right_arm.set_path_constraints(consts)

        # 设置动作对象目标位置姿态
        pose_goal.position.z = pose_goal.position.z - 0.045
        # pose_goal.position.y = pose_goal.position.y - 0.1

        # 添加路径末端点
        waypoints.append(copy.deepcopy(pose_goal))

        # 路径规划
        (plan, fraction) = right_arm.compute_cartesian_path(
            waypoints,  # waypoints to follow
            0.01,  # eef_step
            0.0)  # jump_threshold

        print "End effector pose %s" % waypoints

        # robot = self.robot
        # display_trajectory_publisher = self.display_trajectory_publisher

        # display_trajectory = moveit_msgs.msg.DisplayTrajectory()
        # display_trajectory.trajectory_start = robot.get_current_state()
        # display_trajectory.trajectory.append(plan)
        # display_trajectory_publisher.publish(display_trajectory)

        # 规划和输出动作
        right_arm.execute(plan, wait=False)
コード例 #26
0
 def set_upright_constraints(self,pose):
     upright_constraints = Constraints()
     orientation_constraint = OrientationConstraint()
     upright_constraints.name = "upright"
     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 = 3.14
     orientation_constraint.absolute_y_axis_tolerance = 0.1
     orientation_constraint.absolute_z_axis_tolerance = 0.1
     #orientation_constraint.absolute_z_axis_tolerance = 3.14 #ignore this axis
     orientation_constraint.weight = 1
     upright_constraints.orientation_constraints.append(orientation_constraint)
     group.set_path_constraints(upright_constraints)
コード例 #27
0
def planMoveToPosHoldOrientation(pos=None, orientation=None, arm=None):
  arm = arm or left_arm
  if arm is left_arm:
    pos = pos or left_arm_default_state[0]
    orientation = orientation or left_arm_default_state[1]
  if arm is right_arm:
    pos = pos or right_arm_default_state[0]
    orientation = orientation or right_arm_default_state[1]

  #Second goal pose -----------------------------------------------------
  goal_2 = PoseStamped()
  goal_2.header.frame_id = "base"

  #x, y, and z position
  goal_2.pose.position.x = pos[0]
  goal_2.pose.position.y = pos[1]
  goal_2.pose.position.z = pos[2]

  #Orientation as a quaternion
  orientation = orientation/np.linalg.norm(orientation)
  goal_2.pose.orientation.x = orientation[0]
  goal_2.pose.orientation.y = orientation[1]
  goal_2.pose.orientation.z = orientation[2]
  goal_2.pose.orientation.w = orientation[3]

  #Set the goal state to the pose you just defined
  arm.set_pose_target(goal_2)

  #Set the start state for the left arm
  arm.set_start_state_to_current_state()

  # #Create a path constraint for the arm
  # #UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS
  orien_const = OrientationConstraint()
  orien_const.link_name = "left_gripper";
  orien_const.header.frame_id = "base";
  orien_const.orientation.x = orientation[0]
  orien_const.orientation.y = orientation[1]
  orien_const.orientation.z = orientation[2]
  orien_const.orientation.w = orientation[3]
  orien_const.absolute_x_axis_tolerance = 0.1;
  orien_const.absolute_y_axis_tolerance = 0.1;
  orien_const.absolute_z_axis_tolerance = 0.1;
  orien_const.weight = 1.0;
  consts = Constraints()
  consts.orientation_constraints = [orien_const]
  arm.set_path_constraints(consts)

  #Plan a path
  return (arm, arm.plan())
コード例 #28
0
ファイル: right_arm.py プロジェクト: robmck1995/BlackJack
def right_to_deck(right_arm, right_gripper):
    # get global deck coordinates
    global deck_x
    global deck_y
    global deck_z

    # create pose above deck of cards
    deck = PoseStamped()
    deck.header.frame_id = "base"

    # x, y, and z position of deck
    deck.pose.position.x = deck_x
    deck.pose.position.y = deck_y
    deck.pose.position.z = deck_z

    # Orientation as a quaternion - straight down
    deck.pose.orientation.x = 0.0
    deck.pose.orientation.y = -1.0
    deck.pose.orientation.z = 0.0
    deck.pose.orientation.w = 0.0

    # Plan and execute path from current state
    right_arm.set_pose_target(deck)
    right_arm.set_start_state_to_current_state()

    # Add orientation constraint for path - straight down
    orien_const = OrientationConstraint()
    orien_const.link_name = "right_gripper"
    orien_const.header.frame_id = "base"
    orien_const.orientation.y = -1.0
    orien_const.absolute_x_axis_tolerance = 0.1
    orien_const.absolute_y_axis_tolerance = 0.1
    orien_const.absolute_z_axis_tolerance = 0.1
    orien_const.weight = 1.0
    consts = Constraints()
    consts.orientation_constraints = [orien_const]
    right_arm.set_path_constraints(consts)

    right_plan = right_arm.plan()
    right_arm.execute(right_plan)

    # pick up card
    right_gripper.command_suction(True)

    # increment z position as deck height decreases
    deck_z -= .001

    # sleep for a moment
    rospy.sleep(1.0)
コード例 #29
0
def planPath(positions, orientation=None, arm=None, holdOrientation=False, step=0.01, threshold=1000):
  arm = arm or left_arm
  if arm is left_arm:
    positions = (left_arm_default_state[0],) if positions is None else positions
    orientation = left_arm_default_state[1] if orientation is None else orientation
  if arm is right_arm:
    positions = (right_arm_default_state[0],) if positions is None else positions
    orientation = right_arm_default_state[1] if orientation is None else orientation

  # Compute path
  waypoints = []
  # start with the current pose
  #waypoints.append(arm.get_current_pose().pose)
  wpose = Pose()
  orientation = orientation/np.linalg.norm(orientation)
  wpose.orientation.x = orientation[0]
  wpose.orientation.y = orientation[1]
  wpose.orientation.z = orientation[2]
  wpose.orientation.w = orientation[3]

  for pos in positions:
    wpose.position.x = pos[0]
    wpose.position.y = pos[1]
    wpose.position.z = pos[2]
    waypoints.append(copy.deepcopy(wpose))


  if holdOrientation:
    # #Create a path constraint for the arm
    # #UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS
    orien_const = OrientationConstraint()
    orien_const.link_name = "left_gripper";
    orien_const.header.frame_id = "base";
    orien_const.orientation.x = orientation[0]
    orien_const.orientation.y = orientation[1]
    orien_const.orientation.z = orientation[2]
    orien_const.orientation.w = orientation[3]
    orien_const.absolute_x_axis_tolerance = 0.1;
    orien_const.absolute_y_axis_tolerance = 0.1;
    orien_const.absolute_z_axis_tolerance = 0.1;
    orien_const.weight = 1.0;
    consts = Constraints()
    consts.orientation_constraints = [orien_const]
    arm.set_path_constraints(consts)

  #Plan a path
  (plan, fraction) = arm.compute_cartesian_path(waypoints, step, threshold)
  return (arm, plan, fraction)
コード例 #30
0
def place(robot):
    p = PoseStamped()
    p.header.frame_id = robot.get_planning_frame()
    p.pose.position.x = 0.7
    p.pose.position.y = 0.0
    p.pose.position.z = 0.5
    p.pose.orientation.x = 0
    p.pose.orientation.y = 0
    p.pose.orientation.z = 0
    p.pose.orientation.w = 1.0

    g = PlaceLocation()
    g.place_pose = p

    g.pre_place_approach.direction.vector.z = -1.0
    g.post_place_retreat.direction.vector.x = -1.0
    g.post_place_retreat.direction.header.frame_id = robot.get_planning_frame()
    g.pre_place_approach.direction.header.frame_id = "r_wrist_roll_link"
    g.pre_place_approach.min_distance = 0.1
    g.pre_place_approach.desired_distance = 0.2
    g.post_place_retreat.min_distance = 0.1
    g.post_place_retreat.desired_distance = 0.25

    g.post_place_posture = open_gripper(g.post_place_posture)

    group = robot.get_group("right_arm")
    group.set_support_surface_name("table")

    # Add path constraints
    constr = Constraints()
    constr.orientation_constraints = []
    ocm = OrientationConstraint()
    ocm.link_name = "r_wrist_roll_link"
    ocm.header.frame_id = p.header.frame_id
    ocm.orientation.x = 0.0
    ocm.orientation.y = 0.0
    ocm.orientation.z = 0.0
    ocm.orientation.w = 1.0
    ocm.absolute_x_axis_tolerance = 0.2
    ocm.absolute_y_axis_tolerance = 0.2
    ocm.absolute_z_axis_tolerance = math.pi
    ocm.weight = 1.0
    constr.orientation_constraints.append(ocm)

    # group.set_path_constraints(constr)
    group.set_planner_id("RRTConnectkConfigDefault")

    group.place("part", g)
コード例 #31
0
def add_robot_constraints():
    constraint = Constraints()
    constraint.name = "camera constraint"

    roll_constraint = OrientationConstraint()
    # 'base_link' is equal to the world link
    roll_constraint.header.frame_id = 'world'
    # The link that must be oriented upwards
    roll_constraint.link_name = "camera"
    roll_constraint.orientation = Quaternion(*tf.transformations.quaternion_from_euler(0,np.pi/3,0))
    # Allow rotation of 45 degrees around the x and y axis
    roll_constraint.absolute_x_axis_tolerance = np.pi #Allow max rotation of x degrees
    roll_constraint.absolute_y_axis_tolerance = np.pi
    roll_constraint.absolute_z_axis_tolerance = np.pi/2
    # The roll constraint is the only constraint
    roll_constraint.weight = 1
    #constraint.orientation_constraints = [roll_constraint]

    joint_1_constraint = JointConstraint()
    joint_1_constraint.joint_name = "joint_1"
    joint_1_constraint.position = 0
    joint_1_constraint.tolerance_above = np.pi/2
    joint_1_constraint.tolerance_below = np.pi/2
    joint_1_constraint.weight = 1

    joint_4_constraint = JointConstraint()
    joint_4_constraint.joint_name = "joint_4"
    joint_4_constraint.position = 0
    joint_4_constraint.tolerance_above = np.pi/2
    joint_4_constraint.tolerance_below = np.pi/2
    joint_4_constraint.weight = 1

    joint_5_constraint = JointConstraint()
    joint_5_constraint.joint_name = "joint_5"
    joint_5_constraint.position = np.pi/2
    joint_5_constraint.tolerance_above = np.pi/2
    joint_5_constraint.tolerance_below = np.pi/2
    joint_5_constraint.weight = 1

    joint_6_constraint = JointConstraint()
    joint_6_constraint.joint_name = "joint_6"
    joint_6_constraint.position = np.pi-0.79
    joint_6_constraint.tolerance_above = np.pi
    joint_6_constraint.tolerance_below = np.pi
    joint_6_constraint.weight = 1

    constraint.joint_constraints = [joint_1_constraint, joint_6_constraint]
    return constraint
コード例 #32
0
    def __check_joint_validity_moveit(self, joints):
        """
        Check target joint validity (no collision) with MoveIt.
        :return: The target joint validity in a boolean, and the two links colliding if available
        """
        robot_state_target = RobotStateMoveIt()
        robot_state_target.joint_state.header.frame_id = "world"
        robot_state_target.joint_state.position = joints
        robot_state_target.joint_state.name = self.__arm_state.joints_name
        group_name = self.__arm.get_name()
        null_constraints = Constraints()
        try:
            response = self.__parameters_validator.check_state_validity(
                robot_state_target, group_name, null_constraints)
            if not response.valid:
                if len(response.contacts) > 0:
                    rospy.logwarn(
                        'Jog Controller - Joints target unreachable because of collision between %s and %s',
                        response.contacts[0].contact_body_1,
                        response.contacts[0].contact_body_2)
                    return False, response.contacts[
                        0].contact_body_1, response.contacts[0].contact_body_2
                else:  # didn't succeed to get the contacts on the real robot
                    rospy.logwarn_throttle(
                        1,
                        'Jog Controller - Joints target unreachable because of '
                        'collision between two parts of Ned')
                    return False, None, None
            else:
                return True, None, None

        except AttributeError as _e:  # maybe delete later, useful for the test on real robot when using the service
            return True, None, None
コード例 #33
0
ファイル: moveit2.py プロジェクト: 09ubberboy90/ign_moveit2
 def clear_goal_constraints(self):
     """
     Clear all goal constraints that were previously set.
     Note that this function is called automatically after each `plan_kinematic_path()`.
     """
     self.kinematic_path_request.motion_plan_request.goal_constraints = \
         [Constraints()]
コード例 #34
0
def limit_base_joint_constraint():

	ur5_constraints = Constraints()
	shoulder_pan_joint_constraint = JointConstraint()
	shoulder_lift_joint_constraint = JointConstraint()
	wrist_1_joint_constraint = JointConstraint()

	shoulder_pan_joint_constraint.joint_name = "shoulder_pan_joint"
	shoulder_pan_joint_constraint.position = 0
	shoulder_pan_joint_constraint.tolerance_above = 3.14
	shoulder_pan_joint_constraint.tolerance_below = 3.14
	shoulder_pan_joint_constraint.weight = 1

	shoulder_lift_joint_constraint.joint_name = "shoulder_lift_joint"
	shoulder_lift_joint_constraint.position = 0
	shoulder_lift_joint_constraint.tolerance_above = 0
	shoulder_lift_joint_constraint.tolerance_below = 3.14
	shoulder_lift_joint_constraint.weight = 1

	wrist_1_joint_constraint.joint_name = "wrist_1_joint"
	wrist_1_joint_constraint.position = 0
	wrist_1_joint_constraint.tolerance_above = 3.14
	wrist_1_joint_constraint.tolerance_below = 3.14
	wrist_1_joint_constraint.weight = 1

	ur5_constraints.joint_constraints.append(shoulder_pan_joint_constraint)
	ur5_constraints.joint_constraints.append(shoulder_lift_joint_constraint)
	ur5_constraints.joint_constraints.append(wrist_1_joint_constraint)

	return ur5_constraints
コード例 #35
0
ファイル: QuickTest.py プロジェクト: SiChiTong/lightning_ros
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
コード例 #36
0
ファイル: ik_control.py プロジェクト: dHutchings/ee106a
def move_to_coord(trans, rot, arm, which_arm='left', keep_oreint=False):
    goal = PoseStamped()
    goal.header.frame_id = "base"

    # x, y, and z position
    goal.pose.position.x = trans[0]
    goal.pose.position.y = trans[1]
    goal.pose.position.z = trans[2]
    
    # Orientation as a quaternion
    goal.pose.orientation.x = rot[0]
    goal.pose.orientation.y = rot[1]
    goal.pose.orientation.z = rot[2]
    goal.pose.orientation.w = rot[3]

    # Set the goal state to the pose you just defined
    arm.set_pose_target(goal)

    # Set the start state for the arm
    arm.set_start_state_to_current_state()

    if keep_oreint:
        # Create a path constraint for the arm
        orien_const = OrientationConstraint()
        orien_const.link_name = which_arm+"_gripper";
        orien_const.header.frame_id = "base";
        orien_const.orientation.y = -1.0;
        orien_const.absolute_x_axis_tolerance = 0.1;
        orien_const.absolute_y_axis_tolerance = 0.1;
        orien_const.absolute_z_axis_tolerance = 0.1;
        orien_const.weight = 1.0;
        consts = Constraints()
        consts.orientation_constraints = [orien_const]
        arm.set_path_constraints(consts)

    # Plan a path
    arm_plan = arm.plan()

    # Execute the plan
    arm.execute(arm_plan)
コード例 #37
0
def move_group_client():
    client = actionlib.SimpleActionClient('move_group', MoveGroupAction)

    client.wait_for_server()
    goal = MoveGroupGoal()
    goal.request.group_name = "right_arm"
    
    goal_constraints = Constraints()
    goal_constraints.name = 'goal_test'
    
    #goal_constraints.joint_constraints.extend(createJointConstraints())
    #goal_constraints.joint_constraints.extend(createJointConstraintsZero())
    goal_constraints.position_constraints.extend(createPositionConstraint())
    goal_constraints.orientation_constraints.extend(createOrientationConstraint())
    
    goal.request.goal_constraints.append(goal_constraints)
    

    client.send_goal(goal)

    client.wait_for_result()

    return client.get_result()
コード例 #38
0
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
コード例 #39
0
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        # Initialize the ROS node
        rospy.init_node('moveit_constraints_demo', anonymous=True)

        robot = RobotCommander()

        # Connect to the arm move group
        arm = MoveGroupCommander(GROUP_NAME_ARM)

        # Initialize the move group for the right gripper
        gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)

        # Increase the planning time since constraint planning can take a while
        arm.set_planning_time(5)

        # Allow replanning to increase the odds of a solution
        arm.allow_replanning(True)

        # Set the right arm reference frame
        arm.set_pose_reference_frame(REFERENCE_FRAME)

        # Allow some leeway in position(meters) and orientation (radians)
        arm.set_goal_position_tolerance(0.05)
        arm.set_goal_orientation_tolerance(0.1)

        # Get the name of the end-effector link
        end_effector_link = arm.get_end_effector_link()

        # Start in the "resting" configuration stored in the SRDF file
        arm.set_named_target('l_arm_init')

        # Plan and execute a trajectory to the goal configuration
        arm.go()
        rospy.sleep(1)

        # Open the gripper
        gripper.set_joint_value_target(GRIPPER_NEUTRAL)
        gripper.go()
        rospy.sleep(1)

        # Set an initial target pose with the arm up and to the right
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose.position.x = 0.263803774718
        target_pose.pose.position.y = 0.295405791959
        target_pose.pose.position.z = 0.690438884208
        q = quaternion_from_euler(0, 0, -1.57079633)
        target_pose.pose.orientation.x = q[0]
        target_pose.pose.orientation.y = q[1]
        target_pose.pose.orientation.z = q[2]
        target_pose.pose.orientation.w = q[3]

        # Set the start state and target pose, then plan and execute
        arm.set_start_state(robot.get_current_state())
        arm.set_pose_target(target_pose, end_effector_link)
        arm.go()
        rospy.sleep(2)

        # Close the gripper
        gripper.set_joint_value_target(GRIPPER_CLOSED)
        gripper.go()
        rospy.sleep(1)

        # Store the current pose
        start_pose = arm.get_current_pose(end_effector_link)

        # Create a contraints list and give it a name
        constraints = Constraints()
        constraints.name = "Keep gripper horizontal"

        # Create an orientation constraint for the right gripper
        orientation_constraint = OrientationConstraint()
        orientation_constraint.header = start_pose.header
        orientation_constraint.link_name = arm.get_end_effector_link()
        orientation_constraint.orientation.w = 1.0
        orientation_constraint.absolute_x_axis_tolerance = 0.1
        orientation_constraint.absolute_y_axis_tolerance = 0.1
        orientation_constraint.absolute_z_axis_tolerance = 0.1
        orientation_constraint.weight = 1.0
        # q = quaternion_from_euler(0, 0, -1.57079633)
        # orientation_constraint.orientation.x = q[0]
        # orientation_constraint.orientation.y = q[1]
        # orientation_constraint.orientation.z = q[2]
        # orientation_constraint.orientation.w = q[3]

        # Append the constraint to the list of contraints
        constraints.orientation_constraints.append(orientation_constraint)

        # Set the path constraints on the arm
        arm.set_path_constraints(constraints)

        # Set a target pose for the arm
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose.position.x = 0.39000848183
        target_pose.pose.position.y = 0.185900663329
        target_pose.pose.position.z = 0.732752341378
        target_pose.pose.orientation.w = 1

        # Set the start state and target pose, then plan and execute
        arm.set_start_state_to_current_state()
        arm.set_pose_target(target_pose, end_effector_link)
        arm.go()
        rospy.sleep(1)

        # Clear all path constraints
        arm.clear_path_constraints()

        # Open the gripper
        gripper.set_joint_value_target(GRIPPER_NEUTRAL)
        gripper.go()
        rospy.sleep(1)

        # Return to the "resting" configuration stored in the SRDF file
        arm.set_named_target('l_arm_init')

        # Plan and execute a trajectory to the goal configuration
        arm.go()
        rospy.sleep(1)

        # Shut down MoveIt cleanly
        moveit_commander.roscpp_shutdown()

        # Exit MoveIt
        moveit_commander.os._exit(0)
コード例 #40
0
ファイル: kendo_robot_defense.py プロジェクト: jlamyi/kendo
def callback(message):
    global last_x 
    global last_y 
    global last_z 
    global error   
    try:
        if message.transforms[0].child_frame_id == 'ar_marker_23':
            #Read the position of artag
            (trans,rot) = listener.lookupTransform('/base', '/ar_marker_23', rospy.Time(0)) 
            #Execute only when the difference of the current position and the last position exceed the threshold
            if ((abs(trans[0]-last_x) < threshold) and (abs(trans[1]-last_y) < threshold) and (abs(trans[2]-last_z) < threshold)):
                pass
            else:
                last_x = trans[0]
                last_y = trans[1]
                last_z = trans[2]
                print trans
                
                #Goal position
                goal = PoseStamped()
                goal.header.frame_id = "base"
                
                #x, y, and z position
                goal.pose.position.x = trans[0]
                goal.pose.position.y = trans[1]
                goal.pose.position.z = trans[2]-0.1
                
                #Orientation as a quaternion: default orientation
                goal.pose.orientation.x = 0.5
                goal.pose.orientation.y = 0.5
                goal.pose.orientation.z = 0.5
                goal.pose.orientation.w = -0.5
              
                #Set the goal state
                right_arm.set_pose_target(goal)

                #Set the start state
                right_arm.set_start_state_to_current_state()
                
                #Create a orientation constraint for the arm
                orien_const = OrientationConstraint()
                orien_const.link_name = "right_gripper";
                orien_const.header.frame_id = "base";
                orien_const.orientation.x = 0.5;
                orien_const.orientation.y = 0.5;
                orien_const.orientation.z = 0.5;
                orien_const.orientation.w = -0.5;
                orien_const.absolute_x_axis_tolerance = 0.1;
                orien_const.absolute_y_axis_tolerance = 0.1;
                orien_const.absolute_z_axis_tolerance = 0.1;
                orien_const.weight = 1.0;
                consts = Constraints()
                consts.orientation_constraints = [orien_const]
                right_arm.set_path_constraints(consts)
                
                #Plan a path
                right_plan = right_arm.plan()

                #Execute the plan
                right_arm.execute(right_plan)      
        else:
            pass   
    except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
        print 'exception'
コード例 #41
0
        jc1 = JointConstraint()
        jc1.joint_name = "j5"
        jc1.position = 1.25752837976
        jc1.tolerance_above = 0.0001
        jc1.tolerance_below = 0.0001
        jc1.weight = 1.0

        jc0 = JointConstraint()
        jc0.joint_name = "flange"
        jc0.position = -4.43859335239
        jc0.tolerance_above = 0.0001
        jc0.tolerance_below = 0.0001
        jc0.weight = 1.0

        cons = Constraints()
        cons.name = ""
        cons.joint_constraints.append(jc2)
        cons.joint_constraints.append(jc3)
        cons.joint_constraints.append(jc4)
        cons.joint_constraints.append(jc5)
        cons.joint_constraints.append(jc1)
        cons.joint_constraints.append(jc0)

        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
コード例 #42
0
def main():
    #Initialize moveit_commander
    moveit_commander.roscpp_initialize(sys.argv)

    #Start a node
    rospy.init_node('moveit_node')
    
    #Set up the left gripper
    left_gripper = baxter_gripper.Gripper('left')
    
    #Calibrate the gripper (other commands won't work unless you do this first)
    print('Calibrating...')
    left_gripper.calibrate()
    rospy.sleep(2.0)

    #Initialize both arms
    robot = moveit_commander.RobotCommander()
    scene = moveit_commander.PlanningSceneInterface()
    left_arm = moveit_commander.MoveGroupCommander('left_arm')
    right_arm = moveit_commander.MoveGroupCommander('right_arm')
    left_arm.set_planner_id('RRTConnectkConfigDefault')
    left_arm.set_planning_time(10)
    right_arm.set_planner_id('RRTConnectkConfigDefault')
    right_arm.set_planning_time(10)

    #First goal pose ------------------------------------------------------
    goal_1 = PoseStamped()
    goal_1.header.frame_id = "base"

    #x, y, and z position
    goal_1.pose.position.x = 0.2
    goal_1.pose.position.y = 0.6
    goal_1.pose.position.z = 0.2
    
    #Orientation as a quaternion
    goal_1.pose.orientation.x = 0.0
    goal_1.pose.orientation.y = -1.0
    goal_1.pose.orientation.z = 0.0
    goal_1.pose.orientation.w = 0.0

    #Set the goal state to the pose you just defined
    left_arm.set_pose_target(goal_1)

    #Set the start state for the left arm
    left_arm.set_start_state_to_current_state()

    #Plan a path
    left_plan = left_arm.plan()

    #Execute the plan
    raw_input('Press <Enter> to move the left arm to goal pose 1 (path constraints are never enforced during this motion): ')
    left_arm.execute(left_plan)


    #Second goal pose -----------------------------------------------------
    rospy.sleep(2.0)  
    #Close the left gripper
    #print('Closing...')
    #left_gripper.close(block=True)
    #rospy.sleep(0.5)

    #Open the left gripper
    #print('Opening...')
    #left_gripper.open(block=True)
    #rospy.sleep(1.0)
    print('Done!')        
    goal_2 = PoseStamped()
    goal_2.header.frame_id = "base"

    #x, y, and z position
    goal_2.pose.position.x = 0.6
    goal_2.pose.position.y = 0.4
    goal_2.pose.position.z = 0.0
    
    #Orientation as a quaternion
    goal_2.pose.orientation.x = 0.0
    goal_2.pose.orientation.y = -1.0
    goal_2.pose.orientation.z = 0.0
    goal_2.pose.orientation.w = 0.0

    #Set the goal state to the pose you just defined
    left_arm.set_pose_target(goal_2)

    #Set the start state for the left arm
    left_arm.set_start_state_to_current_state()

    # #Create a path constraint for the arm
    # #UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS
    orien_const = OrientationConstraint()
    orien_const.link_name = "left_gripper";
    orien_const.header.frame_id = "base";
    orien_const.orientation.y = -1.0;
    orien_const.absolute_x_axis_tolerance = 0.1;
    orien_const.absolute_y_axis_tolerance = 0.1;
    orien_const.absolute_z_axis_tolerance = 0.1;
    orien_const.weight = 1.0;
    consts = Constraints()
    consts.orientation_constraints = [orien_const]
    left_arm.set_path_constraints(consts)

    #Plan a path
    left_plan = left_arm.plan()

    #Execute the plan
    raw_input('Press <Enter> to move the left arm to goal pose 2: ')
    left_arm.execute(left_plan)


    #Third goal pose -----------------------------------------------------
    rospy.sleep(2.0)    
    #Close the left gripper
    #print('Closing...')
    #left_gripper.close(block=True)
    #rospy.sleep(0.5)

    #Open the left gripper
    #print('Opening...')
    #left_gripper.open(block=True)
    #rospy.sleep(1.0)
    #print('Done!')    
    goal_3 = PoseStamped()
    goal_3.header.frame_id = "base"

    #x, y, and z position
    goal_3.pose.position.x = 0.0
    goal_3.pose.position.y = 0.7
    goal_3.pose.position.z = 0.0
    
    #Orientation as a quaternion
    goal_3.pose.orientation.x = 0.0
    goal_3.pose.orientation.y = -1.0
    goal_3.pose.orientation.z = 0.0
    goal_3.pose.orientation.w = 0.0

    #Set the goal state to the pose you just defined
    left_arm.set_pose_target(goal_3)

    #Set the start state for the left arm
    left_arm.set_start_state_to_current_state()

    #Create a path constraint for the arm
    # #UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS
    orien_const = OrientationConstraint()
    orien_const.link_name = "left_gripper";
    orien_const.header.frame_id = "base";
    orien_const.orientation.y = -1.0;
    orien_const.absolute_x_axis_tolerance = 0.1;
    orien_const.absolute_y_axis_tolerance = 0.1;
    orien_const.absolute_z_axis_tolerance = 0.1;
    orien_const.weight = 1.0;
    consts = Constraints()
    consts.orientation_constraints = [orien_const]
    left_arm.set_path_constraints(consts)

    #Plan a path
    left_plan = left_arm.plan()

    #Execute the plan
    raw_input('Press <Enter> to move the left arm to goal pose 3: ')
    left_arm.execute(left_plan)
    
    #Fourth goal pose -----------------------------------------------------
    rospy.sleep(2.0)    
    #Close the left gripper
    #print('Closing...')
    #left_gripper.close(block=True)
    #rospy.sleep(0.5)

    #Open the left gripper
    #print('Opening...')
    #left_gripper.open(block=True)
    #rospy.sleep(1.0)
    #print('Done!')
    goal_4 = PoseStamped()
    goal_4.header.frame_id = "base"

    #x, y, and z position
    goal_4.pose.position.x = 0.4
    goal_4.pose.position.y = 0.7
    goal_4.pose.position.z = 0.3
    
    #Orientation as a quaternion
    goal_4.pose.orientation.x = 0.0
    goal_4.pose.orientation.y = -1.0
    goal_4.pose.orientation.z = 0.0
    goal_4.pose.orientation.w = 0.0

    #Set the goal state to the pose you just defined
    left_arm.set_pose_target(goal_4)

    #Set the start state for the left arm
    left_arm.set_start_state_to_current_state()

    #Create a path constraint for the arm
    # #UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS
    #orien_const = OrientationConstraint()
    #orien_const.link_name = "left_gripper";
    #orien_const.header.frame_id = "base";
    #orien_const.orientation.y = -1.0;
    #orien_const.absolute_x_axis_tolerance = 0.1;
    #orien_const.absolute_y_axis_tolerance = 0.1;
    #orien_const.absolute_z_axis_tolerance = 0.1;
    #orien_const.weight = 1.0;
    #consts = Constraints()
    #consts.orientation_constraints = [orien_const]
    #left_arm.set_path_constraints(consts)

    #Plan a path
    left_plan = left_arm.plan()

    #Execute the plan
    raw_input('Press <Enter> to move the left arm to goal pose 4: ')
    left_arm.execute(left_plan)
    
    rospy.sleep(2.0)   
    #Close the left gripper
    print('Closing...')
    left_gripper.close(block=True)
    rospy.sleep(0.5)

    #Open the left gripper
    print('Opening...')
    left_gripper.open(block=True)
    rospy.sleep(1.0)
    print('Done!')
コード例 #43
0
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        # Initialize the ROS node
        rospy.init_node('moveit_demo', anonymous=True)
        
        robot = RobotCommander()

        # Connect to the right_arm move group
        right_arm = MoveGroupCommander(GROUP_NAME_ARM)
        
        # Initialize the move group for the right gripper
        right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
                                
        # Increase the planning time since contraint planning can take a while
        right_arm.set_planning_time(15)
                        
        # Allow replanning to increase the odds of a solution
        right_arm.allow_replanning(True)
        
        # Set the right arm reference frame
        right_arm.set_pose_reference_frame(REFERENCE_FRAME)
                
        # Allow some leeway in position(meters) and orientation (radians)
        right_arm.set_goal_position_tolerance(0.05)
        right_arm.set_goal_orientation_tolerance(0.1)
        
        # Get the name of the end-effector link
        end_effector_link = right_arm.get_end_effector_link()
        
        # Start in the "resting" configuration stored in the SRDF file
        right_arm.set_named_target('resting')
         
        # Plan and execute a trajectory to the goal configuration
        right_arm.go()
        rospy.sleep(1)
        
        # Open the gripper
        right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
        right_gripper.go()
        rospy.sleep(1)
        
        # Set an initial target pose with the arm up and to the right
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose.position.x = 0.237012590198
        target_pose.pose.position.y = -0.0747191267505
        target_pose.pose.position.z = 0.901578401949
        target_pose.pose.orientation.w = 1.0
         
        # Set the start state and target pose, then plan and execute
        right_arm.set_start_state(robot.get_current_state())
        right_arm.set_pose_target(target_pose, end_effector_link)
        right_arm.go()
        rospy.sleep(2)
        
        # Close the gripper
        right_gripper.set_joint_value_target(GRIPPER_CLOSED)
        right_gripper.go()
        rospy.sleep(1)
        
        # Store the current pose
        start_pose = right_arm.get_current_pose(end_effector_link)
        
        # Create a contraints list and give it a name
        constraints = Constraints()
        constraints.name = "Keep gripper horizontal"
        
        # Create an orientation constraint for the right gripper 
        orientation_constraint = OrientationConstraint()
        orientation_constraint.header = start_pose.header
        orientation_constraint.link_name = right_arm.get_end_effector_link()
        orientation_constraint.orientation.w = 1.0
        orientation_constraint.absolute_x_axis_tolerance = 0.1
        orientation_constraint.absolute_y_axis_tolerance = 0.1
        orientation_constraint.absolute_z_axis_tolerance = 3.14
        orientation_constraint.weight = 1.0
        
        # Append the constraint to the list of contraints
        constraints.orientation_constraints.append(orientation_constraint)
          
        # Set the path constraints on the right_arm
        right_arm.set_path_constraints(constraints)
        
        # Set a target pose for the arm        
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose.position.x = 0.173187824708
        target_pose.pose.position.y = -0.0159929871606
        target_pose.pose.position.z = 0.692596608605
        target_pose.pose.orientation.w = 1.0

        # Set the start state and target pose, then plan and execute
        right_arm.set_start_state_to_current_state()
        right_arm.set_pose_target(target_pose, end_effector_link)
        right_arm.go()
        rospy.sleep(1)
          
        # Clear all path constraints
        right_arm.clear_path_constraints()
        
        # Open the gripper
        right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
        right_gripper.go()
        rospy.sleep(1)
        
        # Return to the "resting" configuration stored in the SRDF file
        right_arm.set_named_target('resting')

        # Plan and execute a trajectory to the goal configuration
        right_arm.go()
        rospy.sleep(1)

        # Shut down MoveIt cleanly
        moveit_commander.roscpp_shutdown()
        
        # Exit MoveIt
        moveit_commander.os._exit(0)
コード例 #44
0
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()