Python RobotCommander.get_current_state方法代码示例

# 需要导入模块: from moveit_commander import RobotCommander [as 别名]
# 或者: from moveit_commander.RobotCommander import get_current_state [as 别名]
    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
#.........这里部分代码省略.........

# 需要导入模块: from moveit_commander import RobotCommander [as 别名]
# 或者: from moveit_commander.RobotCommander import get_current_state [as 别名]
#
# Author: Ioan Sucan

import sys
import rospy
from moveit_commander import RobotCommander, roscpp_initialize, roscpp_shutdown
from moveit_msgs.msg import RobotState

if __name__=='__main__':

    roscpp_initialize(sys.argv)
    rospy.init_node('moveit_py_demo', anonymous=True)
    
    robot = RobotCommander()
    rospy.sleep(1)

    print "Current state:"
    print robot.get_current_state()
    
    # plan to a random location 
    a = robot.right_arm
    a.set_start_state(RobotState())
    r = a.get_random_joint_values()
    print "Planning to random joint position: "
    print r
    p = a.plan(r)
    print "Solution:"
    print p

    roscpp_shutdown()

# 需要导入模块: from moveit_commander import RobotCommander [as 别名]
# 或者: from moveit_commander.RobotCommander import get_current_state [as 别名]
class trajectoryConstructor():
    def __init__(self):
        rospy.loginfo("Waiting for service " + IK_SERVICE_NAME)
        rospy.wait_for_service(IK_SERVICE_NAME)
        self.ik_serv = rospy.ServiceProxy(IK_SERVICE_NAME, GetPositionIK)
        
        self.joint_list = ['torso_1_joint', 'torso_2_joint',
                           'head_1_joint', 'head_2_joint',
                           'arm_left_1_joint', 'arm_left_2_joint', 'arm_left_3_joint',
                           'arm_left_4_joint', 'arm_left_5_joint', 'arm_left_6_joint',
                           'arm_left_7_joint', 
                           'arm_right_1_joint', 'arm_right_2_joint', 'arm_right_3_joint',
                           'arm_right_4_joint', 'arm_right_5_joint', 'arm_right_6_joint',
                           'arm_right_7_joint']
        self.left_arm = ['arm_left_1_joint', 'arm_left_2_joint', 'arm_left_3_joint',
                           'arm_left_4_joint', 'arm_left_5_joint', 'arm_left_6_joint',
                           'arm_left_7_joint']
        self.right_arm = ['arm_right_1_joint', 'arm_right_2_joint', 'arm_right_3_joint',
                           'arm_right_4_joint', 'arm_right_5_joint', 'arm_right_6_joint',
                           'arm_right_7_joint']
        self.right_arm_torso = ['torso_1_joint', 'torso_2_joint',
                           'arm_right_1_joint', 'arm_right_2_joint', 'arm_right_3_joint',
                           'arm_right_4_joint', 'arm_right_5_joint', 'arm_right_6_joint',
                           'arm_right_7_joint']
        self.left_arm_torso = ['torso_1_joint', 'torso_2_joint',
                           'arm_left_1_joint', 'arm_left_2_joint', 'arm_left_3_joint',
                           'arm_left_4_joint', 'arm_left_5_joint', 'arm_left_6_joint',
                           'arm_left_7_joint']
        
        
#         # Store answer so we ask only one time
#         self.joint_state = JointState()
#         self.joint_state.name = gksi_answer.kinematic_solver_info.joint_names
#         self.joint_state.position = [0.0] * len(gksi_answer.kinematic_solver_info.joint_names)
#         self.ik_link_name = gksi_answer.kinematic_solver_info.link_names[0]
        self.r_commander = RobotCommander()
        self.initial_robot_state = self.r_commander.get_current_state()

        if DEBUG_MODE:
            self.pub_ok_markers = rospy.Publisher('ik_ok_marker_list', MarkerArray, latch=True)
            self.ok_markers = MarkerArray()
        
            self.pub_fail_markers = rospy.Publisher('ik_fail_marker_list', MarkerArray, latch=True)
            self.fail_markers = MarkerArray()
            self.markers_id = 5    
        
        
        
    def getIkPose(self, pose, group="right_arm", previous_state=None):
        """Get IK of the pose specified, for the group specified, optionally using
        the robot_state of previous_state (if not, current robot state will be requested) """
        # point point to test if there is ik
        # returns the answer of the service
        rqst = GetPositionIKRequest()
        rqst.ik_request.avoid_collisions = True
        rqst.ik_request.group_name = group
        rqst.ik_request.pose_stamped.header = Header(stamp=rospy.Time.now())
        rqst.ik_request.pose_stamped.header.frame_id = 'base_link'


        # Set point to check IK for
        rqst.ik_request.pose_stamped.pose.position = pose.position
        rqst.ik_request.pose_stamped.pose.orientation = pose.orientation
        
        if previous_state == None:
            cs = self.r_commander.get_current_state()
            rqst.ik_request.robot_state = cs
        else:
            rqst.ik_request.robot_state = previous_state

        ik_answer = GetPositionIKResponse()
        if DEBUG_MODE:
            timeStart = rospy.Time.now()
        ik_answer = self.ik_serv.call(rqst)
        
        if DEBUG_MODE:
            durationCall= rospy.Time.now() - timeStart
            rospy.loginfo("Call took: " + str(durationCall.to_sec()) + "s")
        
        return ik_answer   
        
    def computeJointTrajFromCartesian(self, points, arm="right_arm_torso"):
        #fjt_goal = FollowJointTrajectoryGoal()
        poselist = []
        for point in points:
            qt = quaternion_from_euler(point.positions[3], point.positions[4], point.positions[5])
            pose = Pose(Point(point.positions[0], point.positions[1], point.positions[2]),
                        Quaternion(*qt.tolist()))
            poselist.append(pose)
        fjt_goal = self.computeIKsPose(poselist, arm)
        
        return fjt_goal
        
        
    def computeIKsPose(self, poselist, arm="right_arm"):
        rospy.loginfo("Computing " + str(len(poselist)) + " IKs" )
        fjt_goal = FollowJointTrajectoryGoal()

        if arm == 'right_arm_torso':
            fjt_goal.trajectory.joint_names = self.right_arm_torso
#.........这里部分代码省略.........

# 需要导入模块: from moveit_commander import RobotCommander [as 别名]
# 或者: from moveit_commander.RobotCommander import get_current_state [as 别名]
    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
#.........这里部分代码省略.........

# 需要导入模块: from moveit_commander import RobotCommander [as 别名]
# 或者: from moveit_commander.RobotCommander import get_current_state [as 别名]
def simple_function():
        rc = RobotCommander()
        mgc = MoveGroupCommander("manipulator")
        # print(rc.get_group_names())
        # print(rc.get_group('manipulator'))
        
        
        # exit()
        
        eef_step = 0.01
        jump_threshold = 2
        ### Create a handle for the Planning Scene Interface
        psi = PlanningSceneInterface()
        
        
        rc.get_current_state()
        rospy.logwarn(rc.get_current_state())
        sss.wait_for_input()
        #rate = rospy.Rate(0.1) # 10hz
        rate = rospy.Rate(1) # 10hz
        rospy.sleep(1)
        
        
        theSub = rospy.Subscriber('/attached_collision_object', AttachedCollisionObject, attCollObjCb, queue_size = 1)
        
        while not rospy.is_shutdown():
            approach_pose = PoseStamped()
            approach_pose.header.frame_id = "table"
            approach_pose.header.stamp = rospy.Time(0)
            approach_pose.pose.position.x = 0.146
            approach_pose.pose.position.y = 0.622
            approach_pose.pose.position.z = 0.01
            quat = tf.transformations.quaternion_from_euler(0, 0, 1.57/2)
            approach_pose.pose.orientation.x = quat[0]
            approach_pose.pose.orientation.y = quat[1]
            approach_pose.pose.orientation.z = quat[2]
            approach_pose.pose.orientation.w = quat[3]
#             mgc.set_start_state_to_current_state()
#             (traj_approach,frac_approach) = mgc.compute_cartesian_path([approach_pose.pose], eef_step, jump_threshold, True)
#             if(frac_approach != 1):
#                 rospy.logwarn("Planning did not succeed before adding collision objects")
#             else:
#                 rospy.logwarn("Planning succeeded before adding collision objects")
# 
#             rospy.logwarn("waiting for input to add dummy box")
#             sss.wait_for_input()
#             
            box_dummy_pose = PoseStamped()
            box_dummy_pose.header.frame_id =  "table"
            box_dummy_pose.pose.position.x = 0.147
            box_dummy_pose.pose.position.y = 0.636
            box_dummy_pose.pose.position.z = 0
            psi.add_box("dummy_box", box_dummy_pose, (0.18,0.09,0.26))# #size x,y,z x is always to the left viewing the robot from the PC  
#             rospy.logwarn("I have added the box")
#             rospy.sleep(1)
#             rospy.logwarn("waiting for input to try planning with dummy box")
#             sss.wait_for_input()
#             
#             (traj_approach,frac_approach) = mgc.compute_cartesian_path([approach_pose.pose], eef_step, jump_threshold, True)
#             if(frac_approach != 1):
#                 rospy.logwarn("Planning did not succeed after adding collision objects (dummy box)")
#             else:
#                 rospy.logwarn("Planning succeeded after adding collision objects (dummy box)")
#                 
            rospy.logwarn("waiting for input to add end effector box box")
            sss.wait_for_input()
            # end effector collision object
            link_attached_to_ef = "ee_link"
            mb_ef_collisonobj = "metal_bracket"
            mb_ef_pose = PoseStamped()
            mb_ef_pose.header.frame_id =  link_attached_to_ef
            mb_ef_pose.pose.position.x = 0.065/2.0
            mb_ef_pose.pose.position.y = 0.0
            mb_ef_pose.pose.position.z = 0.0
            mb_ef_size = (0.065,0.06,0.06)



            psi.attach_box(link_attached_to_ef, mb_ef_collisonobj, mb_ef_pose, mb_ef_size, [link_attached_to_ef, "wrist_3_link"])
            
            
            raw_input("0 hi")
            
            mgc.attach_object("dummy_box", link_attached_to_ef, [link_attached_to_ef, "wrist_3_link"])
            

            
            
            rospy.logwarn("I have added the attached box to the end effector")
            
            
            rospy.sleep(1)
            raw_input("1 hi")           
            
            rospy.logwarn(rc.get_current_state())
            # mgc.attach_object(object_name, link_name, touch_links)
            mgc.set_start_state_to_current_state()
            rospy.logwarn(rc.get_current_state())
            raw_input("2 hi")
            rospy.logwarn("waiting for input to try planning with end effector box")
#.........这里部分代码省略.........