Python URDF.from_parameter_server方法代码示例

# 需要导入模块: from urdf_parser_py.urdf import URDF [as 别名]
# 或者: from urdf_parser_py.urdf.URDF import from_parameter_server [as 别名]
def main():
    global psm1_kin,psm1_robot, psm2_kin, psm2_robot, ecm_kin, ecm_robot
    
    objective_function.mode = MODE
    
    if psm1_robot is None:
        psm1_robot = URDF.from_parameter_server('/dvrk_psm1/robot_description')
        psm1_kin = KDLKinematics(psm1_robot, psm1_robot.links[1].name, psm1_robot.links[-1].name)
    if psm2_robot is None:
        psm2_robot = URDF.from_parameter_server('/dvrk_psm2/robot_description')
        psm2_kin = KDLKinematics(psm2_robot, psm2_robot.links[1].name, psm2_robot.links[-1].name)
    if ecm_robot is None:
        ecm_robot = URDF.from_parameter_server('/dvrk_ecm/robot_description')
        ecm_kin = KDLKinematics(ecm_robot, ecm_robot.links[3].name, ecm_robot.links[-1].name)
        
    initial_guess = [ (.80,0.5,.3), (0.2,0.7,1.57)]
    res = minimize(objective_function, initial_guess, method='nelder-mead', options={'xtol':1e-12, 'disp':True, 'maxiter': 100000, 'maxfev':100000},)
    print(res)
    print(res.x)
    file.close()
    print(objective_function.mode)   
    if objective_function.mode == 'psm2_psm1':
        print('psm2 relative to world: ')
        v = find_everything_related_to_world('psm2', res.x)
 #       print("""xyz="{} {} {}" rpy="{} {} {}" """.format(v[0], v[1]) )
        print("""xyz="{0} {1} {2}" rpy="{3} {4} {5}" """.format(v[0][0],v[0][1],v[0][2],v[1][0],v[1][1],v[1][2]))
    if objective_function.mode == 'ecm_psm1':
        print('ecm relative to world: ')
        v = find_everything_related_to_world('ecm', res.x)
        print("""xyz="{0} {1} {2}" rpy="{3} {4} {5}" """.format(v[0][0],v[0][1],v[0][2],v[1][0],v[1][1],v[1][2]))

# 需要导入模块: from urdf_parser_py.urdf import URDF [as 别名]
# 或者: from urdf_parser_py.urdf.URDF import from_parameter_server [as 别名]
 def __init__(self):
     self.t = time.time()
     
     self.__AUTOCAMERA_MODE__ = self.MODE.simulation
     
     self.autocamera = Autocamera() # Instantiate the Autocamera Class
     
     self.jnt_msg = JointState()
     self.joint_angles = {'ecm':None, 'psm1':None, 'psm2':None}
     self.cam_info = {'left':CameraInfo(), 'right':CameraInfo()}
     
     self.last_ecm_jnt_pos = None
     
     self.first_run = True
     
     # For forward and inverse kinematics
     self.ecm_robot = URDF.from_parameter_server('/dvrk_ecm/robot_description')
     self.ecm_kin = KDLKinematics(self.ecm_robot, self.ecm_robot.links[0].name, self.ecm_robot.links[-1].name)
     self.psm1_robot = URDF.from_parameter_server('/dvrk_psm1/robot_description')
     self.psm1_kin = KDLKinematics(self.psm1_robot, self.psm1_robot.links[0].name, self.psm1_robot.links[-1].name)
     self.mtml_robot = URDF.from_parameter_server('/dvrk_mtml/robot_description')
     self.mtml_kin = KDLKinematics(self.mtml_robot, self.mtml_robot.links[0].name, self.mtml_robot.links[-1].name)
     self.mtmr_robot = URDF.from_parameter_server('/dvrk_mtmr/robot_description')
     self.mtmr_kin = KDLKinematics(self.mtmr_robot, self.mtmr_robot.links[0].name, self.mtmr_robot.links[-1].name)
     
     
     # For camera clutch control    
     self.camera_clutch_pressed = False        
     self.ecm_manual_control_lock_mtml_msg = None
     self.ecm_manual_control_lock_ecm_msg = None
     self.mtml_start_position = None
     self.mtml_end_position = None
     
     self.initialize_psms_initialized = 30
     self.__DEBUG_GRAPHICS__ = False

# 需要导入模块: from urdf_parser_py.urdf import URDF [as 别名]
# 或者: from urdf_parser_py.urdf.URDF import from_parameter_server [as 别名]
def main():
    global save, psm1_robot, psm1_kin, psm2_robot, psm2_kin, ecm_robot, ecm_kin
    
    rospy.init_node('psm_optimization_data_collector')
    # Get the joint angles from the hardware and move the simulation from hardware
    rospy.Subscriber('/dvrk/PSM1/state_joint_current', JointState, psm1_read_cb)
    rospy.Subscriber('/dvrk/PSM2/state_joint_current', JointState, psm2_read_cb)
    rospy.Subscriber('/dvrk/ECM/state_joint_current', JointState, ecm_read_cb)
    
    if psm1_robot is None:
        psm1_robot = URDF.from_parameter_server('/dvrk_psm1/robot_description')
        psm1_kin = KDLKinematics(psm1_robot, psm1_robot.links[0].name, psm1_robot.links[-1].name)
    if psm2_robot is None:
        psm2_robot = URDF.from_parameter_server('/dvrk_psm2/robot_description')
        psm2_kin = KDLKinematics(psm2_robot, psm2_robot.links[0].name, psm2_robot.links[-1].name)
    if ecm_robot is None:
        ecm_robot = URDF.from_parameter_server('/dvrk_ecm/robot_description')
        ecm_kin = KDLKinematics(ecm_robot, ecm_robot.links[0].name, ecm_robot.links[-1].name)

    while True:
        print("save now? ")
        print("(y) yes\n(n) no\n(q) quit")
        r = raw_input(" : ")
        
        if r == "q":
            global file
            file.close()
            return
        if r == "y":
            psm1_read_cb.save = True
            psm2_read_cb.save = True
            ecm_read_cb.save = True
            
    rospy.spin()

# 需要导入模块: from urdf_parser_py.urdf import URDF [as 别名]
# 或者: from urdf_parser_py.urdf.URDF import from_parameter_server [as 别名]
 def __init__(self):
     self.ecm_robot = URDF.from_parameter_server('/dvrk_ecm/robot_description')
     self.ecm_kin = KDLKinematics(self.ecm_robot, self.ecm_robot.links[0].name, self.ecm_robot.links[-1].name)
     self.psm1_robot = URDF.from_parameter_server('/dvrk_psm1/robot_description')
     self.psm1_kin = KDLKinematics(self.psm1_robot, self.psm1_robot.links[0].name, self.psm1_robot.links[-1].name)
     self.psm2_robot = URDF.from_parameter_server('/dvrk_psm2/robot_description')
     self.psm2_kin = KDLKinematics(self.psm2_robot, self.psm2_robot.links[0].name, self.psm2_robot.links[-1].name)
     
     self.zoom_level_positions = {'l1':None, 'r1':None, 'l2':None, 'r2':None, 'lm':None, 'rm':None}
     self.logerror("autocamera_initialized")

# 需要导入模块: from urdf_parser_py.urdf import URDF [as 别名]
# 或者: from urdf_parser_py.urdf.URDF import from_parameter_server [as 别名]
	def __init__(self, side):

		# Redirect annoying output of upcoming URDF command
		devnull = open(os.devnull, 'w')
		sys.stdout, sys.stderr = devnull, devnull
		
		self.robot = URDF.from_parameter_server()
		
		# Now point output back
		sys.stdout = sys.__stdout__
		sys.stderr = sys.__stderr__
		devnull.close()

		self.joint_list = {}
		for ndx, jnt in enumerate(self.robot.joints):
			self.joint_list[jnt.name] = ndx

		self.chain = list()

		# Query parameter server for joints
		arm_chain = '/' + side + '_arm_chain'
		joint_names = rospy.get_param(arm_chain)

		for joint in joint_names:
			self.chain.append(JointData(self, joint))

# 需要导入模块: from urdf_parser_py.urdf import URDF [as 别名]
# 或者: from urdf_parser_py.urdf.URDF import from_parameter_server [as 别名]
    def __init__(self, joint_names_vector, inactive_joint_names, js_pos):
        self.robot = URDF.from_parameter_server()
        self.tree, self.segment_map, self.segment_parent_map, self.segment_name_id_map = self.kdl_tree_from_urdf_model(self.robot, inactive_joint_names, js_pos)
        self.segment_id_name_map = {}
        for seg_name in self.segment_name_id_map:
            seg_id = self.segment_name_id_map[seg_name]
            self.segment_id_name_map[seg_id] = seg_name

        self.fk_solvers = {}

        self.createSegmentToJointMap(joint_names_vector, inactive_joint_names)

        joint_limit_map = {}
        for j in self.robot.joints:
            if j.limit != None:
                joint_limit_map[j.name] = j.limit

        self.lim_lower = np.empty(len(joint_names_vector))
        self.lim_lower_soft = np.empty(len(joint_names_vector))
        self.lim_upper = np.empty(len(joint_names_vector))
        self.lim_upper_soft = np.empty(len(joint_names_vector))
        q_idx = 0
        for joint_name in joint_names_vector:
            self.lim_lower[q_idx] = joint_limit_map[joint_name].lower
            self.lim_lower_soft[q_idx] = self.lim_lower[q_idx] + 15.0/180.0*math.pi
            self.lim_upper[q_idx] = joint_limit_map[joint_name].upper
            self.lim_upper_soft[q_idx] = self.lim_upper[q_idx] - 15.0/180.0*math.pi
            q_idx += 1

# 需要导入模块: from urdf_parser_py.urdf import URDF [as 别名]
# 或者: from urdf_parser_py.urdf.URDF import from_parameter_server [as 别名]
    def __init__(self):
        #Loads the robot model, which contains the robot's kinematics information
        self.robot = URDF.from_parameter_server()

        #Subscribes to information about what the current joint values are.
        rospy.Subscriber("/joint_states", JointState, self.joint_callback)

        #Subscribes to command for end-effector pose
        rospy.Subscriber("/cartesian_command", Transform, self.command_callback)

        #Subscribes to command for redundant dof
        rospy.Subscriber("/redundancy_command", Float32, self.redundancy_callback)

        # Publishes desired joint velocities
        self.pub_vel = rospy.Publisher("/joint_velocities", JointState, queue_size=1)

        #This is where we hold the most recent joint transforms
        self.joint_transforms = []
        self.q_current = []
        self.x_current = tf.transformations.identity_matrix()
        self.R_base = tf.transformations.identity_matrix()
        self.x_target = tf.transformations.identity_matrix()
        self.q0_desired = 0
        self.last_command_time = 0
        self.last_red_command_time = 0

        # Initialize timer that will trigger callbacks
        self.mutex = Lock()
        self.timer = rospy.Timer(rospy.Duration(0.1), self.timer_callback)

# 需要导入模块: from urdf_parser_py.urdf import URDF [as 别名]
# 或者: from urdf_parser_py.urdf.URDF import from_parameter_server [as 别名]
def create_kdl_kin(base_link, end_link, urdf_filename=None):
    if urdf_filename is None:
        robot = URDF.from_parameter_server()
    else:
        f = open(urdf_filename, "r")
        robot = URDF.from_xml_string(f.read())
    return KDLKinematics(robot, base_link, end_link)

# 需要导入模块: from urdf_parser_py.urdf import URDF [as 别名]
# 或者: from urdf_parser_py.urdf.URDF import from_parameter_server [as 别名]
	def __init__(self):
		
		#Create a tf listener
		self.tfListener = tf.TransformListener() 
		#tf.TransfromListener is a subclass that subscribes to the "/tf" message topic and adds transform 
		#data to the tf data structure. As a result the object is up to date with all current transforms

		#Create a publisher to a new topic name called "stylus_to_base_transf with a message type Twist"
		self.Tsb = rospy.Publisher('stylus_to_base_transf',Twist) 

		#Find the omni from the parameter server
		self.omni = URDF.from_parameter_server() 
		#Note: 	A node that initializes and runs the Phantom Omni has to be running in the background for 
		#		from_parameter_server to to find the parameter.
		#		Older versions of URDF label the function "load_from_parameter_server"


		#Subscribe to the "omni1_joint_states" topic, which provides the joint states measured by the omni in a
		# ROS message of the type sensor_msgs/JointState. Every time a message is published to that topic run the 
		#callback function self.get_joint_states, which is defined below.
		self.joint_state_sub = rospy.Subscriber("omni1_joint_states", JointState, self.get_joint_states)

		#OmniMiniProjTimers
		#Run a timer to manipulate the class structure as needed. The timer's 
		#callback function "timer_callback" then calls the desired functions
		self.timer1 = rospy.Timer(rospy.Duration(0.01), self.timer_callback) 
		
		#Create a separate slower timer on a lower frequency for a comfortable print out
		self.print_timer = rospy.Timer(rospy.Duration(1), self.print_output) 

		return

# 需要导入模块: from urdf_parser_py.urdf import URDF [as 别名]
# 或者: from urdf_parser_py.urdf.URDF import from_parameter_server [as 别名]
    def __init__(self ,urdf=None):
        if urdf is None:
            print 'FROM PARAM SERVER'
            self._youbot = URDF.from_parameter_server(key='robot_description')
        else:
            print 'FROM STRING'
            self._youbot = URDF.from_xml_string(urdf)

        self._kdl_tree = kdl_tree_from_urdf_model(self._youbot)
        self._base_link = 'arm_link_0'
        print "ROOT : ",self._base_link
        self._tip_link = 'arm_link_5'
        print "self._tip_link : ", self._tip_link
        self._tip_frame = PyKDL.Frame()
        self._arm_chain = self._kdl_tree.getChain(self._base_link,
                                                  self._tip_link)

        # Baxter Interface Limb Instances
        #self._limb_interface = youbot_interface.Limb(limb)
        #self._joint_names = self._limb_interface.joint_names()
        self._joint_names = ['arm_joint_1', 'arm_joint_2', 'arm_joint_3', 'arm_joint_4', 'arm_joint_5']
        self._num_jnts = len(self._joint_names)

        # KDL Solvers
        self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain)
        self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._arm_chain)
        self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain)
        self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain,
                                                   self._fk_p_kdl,
                                                   self._ik_v_kdl)
        self._jac_kdl = PyKDL.ChainJntToJacSolver(self._arm_chain)
        self._dyn_kdl = PyKDL.ChainDynParam(self._arm_chain,
                                            PyKDL.Vector.Zero())

# 需要导入模块: from urdf_parser_py.urdf import URDF [as 别名]
# 或者: from urdf_parser_py.urdf.URDF import from_parameter_server [as 别名]
    def __init__(self, urdf_param = 'robot_description'):
        self._urdf = URDF.from_parameter_server(urdf_param)
        (parse_ok, self._kdl_tree) = treeFromUrdfModel(self._urdf)
        # Check @TODO Exception
        if not parse_ok:
            rospy.logerr('Error parsing URDF from parameter server ({0})'.format(urdf_param))
        else:
            rospy.logdebug('Parsing URDF succesful')

        self._base_link = self._urdf.get_root()
        # @TODO Hardcoded
        self._tip_link = 'link_6'
        self._tip_frame = PyKDL.Frame()
        self._arm_chain = self._kdl_tree.getChain(self._base_link,
                                                  self._tip_link)
        # @TODO Hardcoded
        self._joint_names = ['a1', 'a2', 'a3', 'a4', 'a5', 'a6']
        self._num_joints = len(self._joint_names)

        # KDL Solvers
        self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain)
        self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._arm_chain)
        self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain)
        self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain,
                                                   self._fk_p_kdl,
                                                   self._ik_v_kdl)
        self._jac_kdl = PyKDL.ChainJntToJacSolver(self._arm_chain)
        self._dyn_kdl = PyKDL.ChainDynParam(self._arm_chain,
                                            PyKDL.Vector.Zero())

# 需要导入模块: from urdf_parser_py.urdf import URDF [as 别名]
# 或者: from urdf_parser_py.urdf.URDF import from_parameter_server [as 别名]
    def __init__(self):
        #Loads the robot model, which contains the robot's kinematics information
        self.robot = URDF.from_parameter_server()

        # Publishes Cartesian goals
        self.pub_command = rospy.Publisher("/cartesian_command", geometry_msgs.msg.Transform, 
                                           queue_size=1)

        # Publishes Redundancy goals
        self.pub_red = rospy.Publisher("/redundancy_command", Float32, queue_size=1)

        # Publisher to set robot position
        self.pub_reset = rospy.Publisher("/joint_command", JointState, queue_size=1)

        #This is where we hold the most recent joint transforms
        self.joint_transforms = []
        self.x_current = tf.transformations.identity_matrix()
        self.R_base = tf.transformations.identity_matrix()

        #Create "Interactive Marker" that we can manipulate in RViz
        self.init_marker()
        self.ee_tracking = 0
        self.red_tracking = 0
        self.q_current = []

        self.x_target = tf.transformations.identity_matrix()
        self.q0_desired = 0

        self.mutex = Lock()        
        self.timer = rospy.Timer(rospy.Duration(0.3), self.timer_callback)

        #Subscribes to information about what the current joint values are.
        rospy.Subscriber("joint_states", JointState, self.joint_callback)

# 需要导入模块: from urdf_parser_py.urdf import URDF [as 别名]
# 或者: from urdf_parser_py.urdf.URDF import from_parameter_server [as 别名]
    def __init__(self, name):

        self.elevated_distance = 0.3  # .07

        # Find the baxter from the parameter server
        self.baxter = URDF.from_parameter_server()
        # Note:  A node that initializes and runs the baxter has to be running in the background for
        #       from_parameter_server to to find the parameter.
        #       Older versions of URDF label the function "load_from_parameter_server"

        # Subscribe to the "baxter1_joint_states" topic, which provides the joint states measured by the baxter in a
        # ROS message of the type sensor_msgs/JointState. Every time a message is published to that topic run the
        # callback function self.get_joint_states, which is defined below.
        self.joint_state_sub = rospy.Subscriber("/robot/joint_states", JointState, self.get_joint_states)
        self.end_eff_state_sub = rospy.Subscriber(
            "/robot/limb/left/endpoint_state", EndpointState, self.get_end_eff_state
        )

        self.listener = tf.TransformListener()
        # self.timer1 = rospy.Timer(rospy.Duration(0.01),)

        # calibrate the gripper
        self.initialize_gripper()

        self._action_name = name
        # The main function of BaxterWeigh is called whenever a client sends a goal to weigh_server
        self._as = actionlib.SimpleActionServer(
            self._action_name, baxter_pour.msg.WeighAction, execute_cb=self.main, auto_start=False
        )
        self._as.start()

        # To test the node separatly call self.main() manually
        # self.main()
        return

# 需要导入模块: from urdf_parser_py.urdf import URDF [as 别名]
# 或者: from urdf_parser_py.urdf.URDF import from_parameter_server [as 别名]
    def execute_move(self, pos):
        rospy.loginfo('moving')
        # Read in pose data. Adjust the height to be above the block and the length so that the laser sees the table instead of the block
        pos.position.z += .1
        pos.position.x += .005
        q = [pos.orientation.w, pos.orientation.x, pos.orientation.y, pos.orientation.z]
        p =[[pos.position.x],[pos.position.y],[pos.position.z]]
        # Convert quaternion data to rotation matrix
        R = quat_to_so3(q);
        # Form transformation matrix
        robot = URDF.from_parameter_server()
        base_link = robot.get_root()
        kdl_kin = KDLKinematics(robot, base_link, 'right_gripper_base')
        # Create seed with current position
        q0 = kdl_kin.random_joint_angles()
        limb_interface = baxter_interface.limb.Limb('right')
        limb_interface.set_joint_position_speed(.3)
        current_angles = [limb_interface.joint_angle(joint) for joint in limb_interface.joint_names()]
        for ind in range(len(q0)):
            q0[ind] = current_angles[ind]
        pose = kdl_kin.forward(q0)
        pose[0:3,0:3] = R
        pose[0:3,3] = p
        
        # Solve for joint angles, iterating if no solution is found
        seed = 0.3
        q_ik = kdl_kin.inverse(pose, q0+seed)
        while q_ik == None:
            seed += 0.3
            q_ik = kdl_kin.inverse(pose, q0+seed)
        rospy.loginfo(q_ik)
        
        # Calculate the joint trajectory with a quintic time scaling
        q_list = JointTrajectory(q0,q_ik,1,50,5)
        
        # Iterate through list
        for q in q_list:
            # Format joint angles as limb joint angle assignment      
            angles = limb_interface.joint_angles()
            for ind, joint in enumerate(limb_interface.joint_names()):
                angles[joint] = q[ind]
            rospy.sleep(.07)
            
            # Send joint move command
            limb_interface.set_joint_positions(angles)
        
        # Publish state and hand position

        rospy.sleep(1)
        rospy.loginfo(4) 
        self._pub_state.publish(4)                    
        rospy.loginfo(pos)
        self._pub_hand.publish(pos)  

        self._done = True
        print('Done')

# 需要导入模块: from urdf_parser_py.urdf import URDF [as 别名]
# 或者: from urdf_parser_py.urdf.URDF import from_parameter_server [as 别名]
    def execute_cb(self,goal):	
        self.moving_to_new_x_pos = False	
        self.moving_to_new_y_pos = False
        self.stop_base_movement = False
        
        self.max_virtual_x_vel = 0.05
        self.max_virtual_y_vel = 0.05

      	self.commanded_virtual_x_pos = 0.0
        self.commanded_virtual_y_pos = 0.0
	
        self.commanded_virtual_x_vel = 0.0
        self.commanded_virtual_y_vel = 0.0

        self.virtual_x_cmd_time_sec = 0.0
        self.virtual_y_cmd_time_sec = 0.0
	
        self.virtual_x_running_time_sec = 0.0
        self.virtual_y_running_time_sec = 0.0


        youbot_urdf = URDF.from_parameter_server()
        self.kin_with_virtual = KDLKinematics(youbot_urdf, "virtual", "gripper_palm_link")
        self.kin_grasp = KDLKinematics(youbot_urdf, "base_link", "gripper_palm_link")
        
        # Create a timer that will be used to monitor the velocity of the virtual
        # joints when we need them to be positioning themselves.
        self.vel_monitor_timer = rospy.Timer(rospy.Duration(0.1), self.vel_monitor_timer_callback)
       
      
        self.arm_joint_pub = rospy.Publisher("arm_1/arm_controller/position_command",JointPositions,queue_size=10)
        self.gripper_pub = rospy.Publisher("arm_1/gripper_controller/position_command", JointPositions, queue_size=10)
        self.vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)

        # Give the publishers time to get setup before trying to do any actual work.
        rospy.sleep(2)

        # Initialize at the candle position.
        self.publish_arm_joint_positions(armJointPosCandle)
        rospy.sleep(2.0)

	#self.publish_arm_joint_positions(armJointPos1)
	#rospy.sleep(3.0)

	#self.publish_arm_joint_positions(armJointPos2)
	#rospy.sleep(10.0)

	#self.publish_arm_joint_positions(armJointPosCandle)
	#rospy.sleep(2.0)

	# Go through the routine of picking up the block.
	self.grasp_routine()

	self._result.successOrNot=1
	self._as.set_succeeded(self._result)