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
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
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)
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
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)
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
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)
def add_robot_constraints(): constraint = Constraints() constraint.name = "camera constraint" roll_constraint = OrientationConstraint() # 'base_link' is equal to the world link roll_constraint.header.frame_id = 'world' # The link that must be oriented upwards roll_constraint.link_name = "camera" roll_constraint.orientation = Quaternion(*tf.transformations.quaternion_from_euler(0,np.pi/3,0)) # Allow rotation of 45 degrees around the x and y axis roll_constraint.absolute_x_axis_tolerance = np.pi #Allow max rotation of x degrees roll_constraint.absolute_y_axis_tolerance = np.pi roll_constraint.absolute_z_axis_tolerance = np.pi/2 # The roll constraint is the only constraint roll_constraint.weight = 1 #constraint.orientation_constraints = [roll_constraint] joint_1_constraint = JointConstraint() joint_1_constraint.joint_name = "joint_1" joint_1_constraint.position = 0 joint_1_constraint.tolerance_above = np.pi/2 joint_1_constraint.tolerance_below = np.pi/2 joint_1_constraint.weight = 1 joint_4_constraint = JointConstraint() joint_4_constraint.joint_name = "joint_4" joint_4_constraint.position = 0 joint_4_constraint.tolerance_above = np.pi/2 joint_4_constraint.tolerance_below = np.pi/2 joint_4_constraint.weight = 1 joint_5_constraint = JointConstraint() joint_5_constraint.joint_name = "joint_5" joint_5_constraint.position = np.pi/2 joint_5_constraint.tolerance_above = np.pi/2 joint_5_constraint.tolerance_below = np.pi/2 joint_5_constraint.weight = 1 joint_6_constraint = JointConstraint() joint_6_constraint.joint_name = "joint_6" joint_6_constraint.position = np.pi-0.79 joint_6_constraint.tolerance_above = np.pi joint_6_constraint.tolerance_below = np.pi joint_6_constraint.weight = 1 constraint.joint_constraints = [joint_1_constraint, joint_6_constraint] return constraint
def addConstrains(): pose = group.get_current_pose() constraint = Constraints() constraint.name = "downRight" 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 = 0.1 #orientation_constraint.absolute_z_axis_tolerance = 3.14 #ignore this axis orientation_constraint.weight = 1 constraint.orientation_constraints.append(orientation_constraint) group.set_path_constraints(constraint)
def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_test') arm = MoveGroupCommander('arm') end_effector_link = arm.get_end_effector_link() arm.allow_replanning(True) arm.set_planning_time(5) target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_init_orientation = Quaternion() target_init_orientation = quaternion_from_euler(0.0, 1.57, 0.0) self.setPose(target_pose, [0.5, 0.2, 0.5],list(target_init_orientation)) current_pose = arm.get_current_pose(end_effector_link) self.setPose(current_pose, [current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z], list(target_init_orientation)) arm.set_pose_target(current_pose) arm.go() rospy.sleep(2) constraints = Constraints() orientation_constraint = OrientationConstraint() constraints.name = 'gripper constraint' orientation_constraint.header = target_pose.header orientation_constraint.link_name = end_effector_link orientation_constraint.orientation.x = target_init_orientation[0] orientation_constraint.orientation.y = target_init_orientation[1] orientation_constraint.orientation.z = target_init_orientation[2] orientation_constraint.orientation.w = target_init_orientation[3] orientation_constraint.absolute_x_axis_tolerance = 0.03 orientation_constraint.absolute_y_axis_tolerance = 0.03 orientation_constraint.absolute_z_axis_tolerance = 3.14 orientation_constraint.weight = 1.0 constraints.orientation_constraints.append(orientation_constraint) arm.set_path_constraints(constraints) arm.set_pose_target(target_pose, end_effector_link) arm.go() rospy.sleep(1) moveit_commander.roscpp_shutdown() moveit_commander.os._exit(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
def get_constraint(self, euler_orientation=[0, math.pi / 2, 0], tol=[3, 3, .5]): #method takes euler-angle inputs, this converts it to a quaternion q_orientation = tf.transformations.quaternion_from_euler( euler_orientation[0], euler_orientation[1], euler_orientation[2]) orientation_msg = Quaternion(q_orientation[0], q_orientation[1], q_orientation[2], q_orientation[3]) #defines a constraint that sets the end-effector so a cup in it's hand will be upright, or straight upside-down constraint = Constraints() constraint.name = 'upright_wrist' upright_orientation = OrientationConstraint() upright_orientation.link_name = self.group.get_end_effector_link() upright_orientation.orientation = orientation_msg upright_orientation.absolute_x_axis_tolerance = tol[0] upright_orientation.absolute_y_axis_tolerance = tol[1] upright_orientation.absolute_z_axis_tolerance = tol[2] upright_orientation.weight = 1.0 upright_orientation.header = self.start_pose.header constraint.orientation_constraints.append(upright_orientation) return (constraint)
def test_constraints(self): constraint = Constraints() constraint.name = "tilt constraint" ocm = OrientationConstraint() # 'base_link' is equal to the world link ocm.header.frame_id = "world" # The link that must be oriented upwards ocm.link_name = "tcp" # ocm.orientation = Quaternion(0.5, 0.5, 0.5, 0.5) ocm.orientation.x = 0.683951325248 ocm.orientation.y = -0.209429903965 ocm.orientation.z = -0.202265206016 ocm.orientation.w = 0.668908429049 # Allow rotation of 45 degrees around the x and y axis ocm.absolute_x_axis_tolerance = 0.5 #Allow max rotation of 45 degrees ocm.absolute_y_axis_tolerance = 1.5 #Allow max rotation of 360 degrees ocm.absolute_z_axis_tolerance = 0.5 #Allow max rotation of 45 degrees # The tilt constraint is the only constraint ocm.weight = 1 constraint.orientation_constraints = [ocm] move_group = self.robot_group pose_goal = geometry_msgs.msg.Pose() pose_goal.orientation.x = 0.683951325248 pose_goal.orientation.y = -0.209429903965 pose_goal.orientation.z = -0.202265206016 pose_goal.orientation.w = 0.668908429049 pose_goal.position.x = -0.461741097024 pose_goal.position.y = -0.433851093412 pose_goal.position.z = 1.26149858128 move_group.set_pose_target(pose_goal) move_group.set_path_constraints(constraint) move_group.plan()
def plan_between_states(group_name, joint_state_A, joint_state_B, robot_state=None): """ Plan trajectory between two joint states. """ # Create robot state robot_state = joint_to_robot_state(joint_state_A, robot_state) # Prepare motion plan request req = GetMotionPlanRequest() req.motion_plan_request.start_state = robot_state req.motion_plan_request.group_name = group_name req.motion_plan_request.planner_id = "RRTConnectkConfigDefault" constraints = Constraints() constraints.name = "goal" for name, pos in zip(joint_state_B.name, joint_state_B.position): joint_constraints = JointConstraint() joint_constraints.joint_name = name joint_constraints.position = pos joint_constraints.tolerance_above = 0.001 joint_constraints.tolerance_below = 0.001 joint_constraints.weight = 100 constraints.joint_constraints.append(joint_constraints) req.motion_plan_request.goal_constraints.append(constraints) # Call planner planner_srv = rospy.ServiceProxy('/plan_kinematic_path', GetMotionPlan) try: res = planner_srv(req) except rospy.service.ServiceException, e: rospy.logerr("Service 'plan_kinematic_path' call failed: %s", str(e)) return None
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()
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()
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)
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 __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) # 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_tolerance(0.00001) # 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('right') # Plan and execute a trajectory to the goal configuration right_arm.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.4 target_pose.pose.position.y = 0.3 target_pose.pose.position.z = 0.4 target_pose.pose.orientation.x = 0.670820393249937 target_pose.pose.orientation.y = 0.223606797749979 target_pose.pose.orientation.z = 0.223606797749979 target_pose.pose.orientation.w = 0.670820393249937 # Set the start state and target pose, then plan and execute right_arm.set_pose_target(target_pose, end_effector_link) right_arm.go() rospy.sleep(2) # 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 = end_effector_link orientation_constraint.absolute_x_axis_tolerance = 0.00001 orientation_constraint.absolute_y_axis_tolerance = 3.14 orientation_constraint.absolute_z_axis_tolerance = 0.00001 orientation_constraint.weight = 1.0 orientation_constraint.orientation = start_pose.pose.orientation # 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 = deepcopy(start_pose) target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.4 target_pose.pose.position.y = -0.3 target_pose.pose.position.z = 0.4 target_pose.pose.orientation.x = 0.670820393249937 target_pose.pose.orientation.y = -0.223606797749979 target_pose.pose.orientation.z = -0.223606797749979 target_pose.pose.orientation.w = 0.670820393249937 #target_pose.pose.orientation.x = 0.707107 #target_pose.pose.orientation.y = 0 #target_pose.pose.orientation.z = 0 #target_pose.pose.orientation.w = 0.707107 # 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() # Return to the "resting" configuration stored in the SRDF file right_arm.set_named_target('up') # 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) def getOrientation(self, xyz): pass
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 msg.group_name = "manipulator"
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
rospy.loginfo("Add Objects to the Planning Scene...") box_pose = PoseStamped() box_pose.header.frame_id = group.get_planning_frame() box_pose.pose.position.x = 0.3 box_pose.pose.position.y = -0.3 box_pose.pose.position.z = -0.25 box_pose.pose.orientation.w = 1.0 scene.add_box('box_object', box_pose, (0.4, 0.1, 0.5)) rospy.sleep(2) rospy.loginfo("Scene Objects : {}".format(scene.get_known_object_names())) # Set Path Constraint constraints = Constraints() 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()))
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)
def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') scene = PlanningSceneInterface('world') self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) # self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped) self.colors = dict() max_pick_attempts = 10 max_place_attempts = 10 rospy.sleep(1) arm = MoveGroupCommander('arm') end_effector_link = arm.get_end_effector_link() # arm.set_goal_position_tolerance(0.02) # arm.set_goal_orientation_tolerance(0.03) arm.allow_replanning(True) arm.set_planning_time(5) table_id = 'table' side_id = 'side' table2_id = 'table2' box1_id = 'box1' box2_id = 'box2' target_id = 'target' wall_id = 'wall' ground_id = 'ground' scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) scene.remove_world_object(table_id) scene.remove_world_object(table2_id) scene.remove_world_object(target_id) scene.remove_world_object(wall_id) scene.remove_world_object(ground_id) scene.remove_world_object(side_id) rospy.sleep(1) table_ground = 0.55 table_size = [0.2, 0.7, 0.01] table2_size = [0.25, 0.6, 0.01] box1_size = [0.02, 0.9, 0.6] box2_size = [0.05, 0.05, 0.15] target_size = [0.03, 0.06, 0.10] wall_size = [0.01, 0.9, 0.6] ground_size = [3, 3, 0.01] side_size = [0.01, 0.7, table_ground] table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME self.setPose(table_pose, [0.7, 0.0, table_ground + table_size[2] / 2.0]) scene.add_box(table_id, table_pose, table_size) side_pose = PoseStamped() side_pose.header.frame_id = REFERENCE_FRAME self.setPose(side_pose, [0.605, 0.0, side_size[2] / 2.0]) scene.add_box(side_id, side_pose, side_size) table2_pose = PoseStamped() table2_pose.header.frame_id = REFERENCE_FRAME self.setPose(table2_pose, [-0.325, 0.0, box1_size[2] + table2_size[2] / 2.0]) # scene.add_box(table2_id, table2_pose, table2_size) box1_pose = PoseStamped() box1_pose.header.frame_id = REFERENCE_FRAME self.setPose(box1_pose, [-0.36, 0.0, box1_size[2] / 2.0]) # scene.add_box(box1_id, box1_pose, box1_size) box2_pose = PoseStamped() box2_pose.header.frame_id = REFERENCE_FRAME self.setPose( box2_pose, [0.63, -0.12, table_ground + table_size[2] + box2_size[2] / 2.0]) # scene.add_box(box2_id, box2_pose, box2_size) target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME self.setPose( target_pose, [0.62, 0.0, table_ground + table_size[2] + target_size[2] / 2.0]) scene.add_box(target_id, target_pose, target_size) wall_pose = PoseStamped() wall_pose.header.frame_id = REFERENCE_FRAME self.setPose( wall_pose, [-0.405, 0.0, box1_size[2] + table2_size[2] + wall_size[2] / 2.0]) # scene.add_box(wall_id, wall_pose, wall_size) ground_pose = PoseStamped() ground_pose.header.frame_id = REFERENCE_FRAME self.setPose(ground_pose, [0.0, 0.0, -ground_size[2] / 2.0]) # scene.add_box(ground_id, ground_pose, ground_size) self.setColor(table_id, 0.8, 0.0, 0.0, 1.0) # self.setColor(table2_id, 0.8, 0.0, 0.0) # self.setColor(box1_id, 0.9, 0.9, 0.9) # self.setColor(box2_id, 0.8, 0.4, 1.0) self.setColor(target_id, 0.5, 0.4, 1.0) # self.setColor(wall_id, 0.9, 0.9, 0.9) self.setColor(ground_id, 0.3, 0.3, 0.3, 1.0) self.setColor(side_id, 0.8, 0.0, 0.0, 1.0) self.sendColors() constraints = Constraints() constraints.name = 'gripper constraint' orientation_constraint = OrientationConstraint() arm.set_support_surface_name(table_id) test_pose = PoseStamped() test_pose.header.frame_id = REFERENCE_FRAME test_orientation = Quaternion() test_orientation = quaternion_from_euler(0.0, 1.57, -3.14) self.setPose(test_pose, [ -0.15, -0.22, box1_size[2] + table2_size[2] + target_size[2] / 2.0 ], list(test_orientation)) place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME self.setPose( place_pose, [0.62, -0.22, table_ground + table_size[2] + target_size[2] / 2.0]) grasp_pose = target_pose grasp_init_orientation = Quaternion() grasp_init_orientation = quaternion_from_euler(0.0, 1.57, 0.0) grasp_pose.pose.orientation.x = grasp_init_orientation[0] grasp_pose.pose.orientation.y = grasp_init_orientation[1] grasp_pose.pose.orientation.z = grasp_init_orientation[2] grasp_pose.pose.orientation.w = grasp_init_orientation[3] grasp_pose.pose.position.x -= 0.02 rospy.loginfo('quaterion: ' + str(grasp_pose.pose.orientation)) place_pose.pose.orientation.x = grasp_init_orientation[0] place_pose.pose.orientation.y = grasp_init_orientation[1] place_pose.pose.orientation.z = grasp_init_orientation[2] place_pose.pose.orientation.w = grasp_init_orientation[3] orientation_constraint.header = grasp_pose.header orientation_constraint.link_name = end_effector_link orientation_constraint.orientation.x = grasp_init_orientation[0] orientation_constraint.orientation.y = grasp_init_orientation[1] orientation_constraint.orientation.z = grasp_init_orientation[2] orientation_constraint.orientation.w = grasp_init_orientation[3] orientation_constraint.absolute_x_axis_tolerance = 0.03 orientation_constraint.absolute_y_axis_tolerance = 0.03 orientation_constraint.absolute_z_axis_tolerance = 3.14 orientation_constraint.weight = 1.0 constraints.orientation_constraints.append(orientation_constraint) grasps = self.make_grasps(grasp_pose, [table_id], [0.05, 0.07, [1.0, 0.0, 0.0]], [0.22, 0.26, [-1.0, 0.0, 0.0]]) result = None n_attempts = 0 while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: result = arm.pick(target_id, grasps) n_attempts += 1 rospy.loginfo('Pick attempt:' + str(n_attempts)) rospy.sleep(0.2) arm.set_path_constraints(constraints) arm.set_pose_target(place_pose) arm.go() ''' if result == MoveItErrorCodes.SUCCESS: result = None n_attempts = 0 places = self.make_places(place_pose, [table_id], [0.8, 0.13, [1.0, 0.0, 0.0]], [0.1, 0.12, [-1.0, 0.0, 0.0]]) while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts: for place in places: result = arm.place(target_id, place) if result == MoveItErrorCodes.SUCCESS: break n_attempts += 1 rospy.loginfo('Place attempt:' + str(n_attempts)) rospy.sleep(0.2) ''' ''' grasp_pose = place_pose grasp_pose.pose.orientation.x = grasp_init_orientation[0] grasp_pose.pose.orientation.y = grasp_init_orientation[1] grasp_pose.pose.orientation.z = grasp_init_orientation[2] grasp_pose.pose.orientation.w = grasp_init_orientation[3] grasp_pose.pose.position.x -= 0.02 grasps = self.make_grasps(grasp_pose, [table_id], [0.05, 0.07, [1.0, 0.0, 0.0]], [0.15, 0.20, [-1.0, 0.0, 0.0]]) result = None n_attempts = 0 while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: result = arm.pick(target_id, grasps) n_attempts += 1 rospy.loginfo('Pick attempt:' + str(n_attempts)) rospy.sleep(0.2) ''' ''' arm.set_support_surface_name(table2_id) self.setPose(place_pose, [-0.26, -0.22, box1_size[2] + table2_size[2] + target_size[2]/2.0]) if result == MoveItErrorCodes.SUCCESS: result = None n_attempts = 0 places = self.make_places(place_pose, [table2_id], [0.03, 0.06, [-1.0, 0.0, 0.0]], [0.1, 0.15, [1.0, 0.0, 0.0]]) while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts: for place in places: result = arm.place(target_id, place) if result == MoveItErrorCodes.SUCCESS: break n_attempts += 1 rospy.loginfo('Place attempt:' + str(n_attempts)) rospy.sleep(0.2) ''' rospy.sleep(2) moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
# 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")
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 msg.group_name = "manipulator"
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) print "====== Waiting for server..." client.wait_for_server() print "====== Connected to server" while not rospy.is_shutdown(): 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: group.set_joint_value_target(joint_target) print "=========== Calculating trajectory... \n" # Generate several plans and compare them to get the shorter one plan_opt = dict() differ = dict() 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() #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(): roscpp_initialize(sys.argv) rospy.init_node('grasp_ur5', anonymous=True) robot = RobotCommander() scene = PlanningSceneInterface() arm_group = MoveGroupCommander("manipulator") q = quaternion_from_euler(1.5701, 0.0, -1.5701) pose_target = geometry_msgs.msg.Pose() pose_target.orientation.x = q[0] pose_target.orientation.y = q[1] pose_target.orientation.z = q[2] pose_target.orientation.w = q[3] pose_target.position.z = 0.23 #0.23 pose_target.position.y = 0.11 #0.11 pose_target.position.x = -0.45 #-0.45 arm_group.set_pose_target(pose_target) plan = arm_group.plan(pose_target) while plan[0]!= True: plan = arm_group.plan(pose_target) if plan[0]: traj = plan[1] arm_group.execute(traj, wait = True) arm_group.stop() arm_group.clear_pose_targets() # rospy.sleep(1) #FAKE PLAN WITHOUT RESTRICTIONS # # state = RobotState() # arm_group.set_start_state(state) pose_target.position.z = 0.77 arm_group.set_pose_target(pose_target) plan_fake = arm_group.plan(pose_target) while plan_fake[0] != True: plan_fake = arm_group.plan(pose_target) pose = arm_group.get_current_pose() constraint = Constraints() constraint.name = "restricao" orientation_constraint = OrientationConstraint() orientation_constraint.header = pose.header orientation_constraint.link_name = arm_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 = 3.14 orientation_constraint.absolute_z_axis_tolerance = 3.14 orientation_constraint.weight = 1 constraint.orientation_constraints.append(orientation_constraint) arm_group.set_path_constraints(constraint) # state = RobotState() # arm_group.set_start_state(state) pose_target.position.z = 0.77 # 0.77 # pose_target.position.y = -0.11 # -0.11 # pose_target.position.x = 0.31 # 0.31 arm_group.set_pose_target(pose_target) plan = arm_group.plan(pose_target) while plan[0]!= True: plan = arm_group.plan(pose_target) if plan[0]: traj = plan[1] arm_group.execute(traj, wait = True) arm_group.stop() arm_group.clear_pose_targets()
rospy.init_node('move_group_python_interface_tutorial', anonymous=True) robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() group_name = "manipulator" group = moveit_commander.MoveGroupCommander(group_name) # We can get the joint values from the group and adjust some of the values: pose_goal = geometry_msgs.msg.Pose() #Rotation group.set_max_velocity_scaling_factor(0.1) #group.set_planner_id("ESTkConfigDefault") upright_constraints = Constraints() upright_constraints.name = "upright" """ joint_constraint=JointConstraint() joint_constraint.joint_name='upperArm_joint' joint_constraint.position=-1.57 joint_constraint.tolerance_above=1.57 joint_constraint.tolerance_below=1.57 joint_constraint.weight=.5 upright_constraints.joint_constraints.append(joint_constraint) joint_constraint=JointConstraint() joint_constraint.joint_name='foreArm_joint' joint_constraint.position=1.57 joint_constraint.tolerance_above=1.57 joint_constraint.tolerance_below=1.57
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 constraint 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('right_arm_zero') # 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('right_arm_zero') # 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)
# Initialize moveit_commander and node moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('d1_get_basic_info', anonymous=False) # 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
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)
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