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()
示例#2
0
    def __init__(self, config):

        base_link = config['base_link']
        end_link = config['end_link']

        if 'robot_description_param' in config:
            self.robot = URDF.from_parameter_server(
                config['robot_description_param'])
        else:
            self.robot = URDF.from_parameter_server()

        self.config = config
        self.base_link = base_link

        # set up kinematics stuff
        self.tree = kdl_tree_from_urdf_model(self.robot)
        self.chain = self.tree.getChain(base_link, end_link)
        self.kdl_kin = KDLKinematics(self.robot, base_link, end_link)

        self.base_link = base_link
        self.end_link = end_link

        self.skill_instances = {}
        self.skill_features = {}
        self.skill_models = {}
        self.parent_skills = {}

        self.pubs = {}
 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
示例#4
0
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]))
示例#5
0
def is_shou_yaw_goal_in_range(joint_goal):
  # If shoulder yaw goal angle is out of joint range, abort
  upper = URDF.from_parameter_server().joint_map["j_shou_yaw"].limit.upper
  lower = URDF.from_parameter_server().joint_map["j_shou_yaw"].limit.lower
  if (joint_goal[constants.J_SHOU_YAW]<lower) or (joint_goal[constants.J_SHOU_YAW]>upper):
    return False
  else:
    return True
 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")
    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())
示例#8
0
    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)
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)
示例#10
0
文件: ccik.py 项目: yuxinf/Robotics
    def __init__(self):
        #Loads the robot model, which contains the robot's kinematics information
        self.robot = URDF.from_parameter_server()

        self.joint_transforms = []
        self.q_current = []
        self.q0_desired = 0
        self.x_current = tf.transformations.identity_matrix()
        self.x_current2 = tf.transformations.identity_matrix()
        self.R_base = tf.transformations.identity_matrix()  #???
        self.x_desired = tf.transformations.identity_matrix()

        # This is where we hold general info about the joints
        self.num_joints = 0
        self.joint_names = []
        self.joint_axes = []
        # Prepare general information about the robot
        self.get_joint_info()

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

        rospy.Subscriber("/cartesian_command", CartesianCommand,
                         self.command_callback)

        rospy.Subscriber("/ik_command", Transform, self.ik_callback)

        self.pub_jv = rospy.Publisher("/joint_velocities",
                                      JointState,
                                      queue_size=1)

        self.ik = rospy.Publisher("/joint_command", JointState, queue_size=1)
示例#11
0
    def __init__(self): 
	#Load robot from parameter server
        self.robot = URDF.from_parameter_server()

	#Subscribe to current joint state of the robot
        rospy.Subscriber('/joint_states', JointState, self.get_joint_state)

	#This will load information about the joints of the robot
        self.num_joints = 0
        self.joint_names = []
        self.q_current = []
        self.joint_axes = []
        self.get_joint_info()

	#This is a mutex
        self.mutex = Lock()

	#Subscribers and publishers for for cartesian control
        rospy.Subscriber('/cartesian_command', CartesianCommand, self.get_cartesian_command)
        self.velocity_pub = rospy.Publisher('/joint_velocities', JointState, queue_size=10)
        self.joint_velocity_msg = JointState()

        #Subscribers and publishers for numerical IK
        rospy.Subscriber('/ik_command', Transform, self.get_ik_command)
        self.joint_command_pub = rospy.Publisher('/joint_command', JointState, queue_size=10)
        self.joint_command_msg = JointState()
示例#12
0
  def __init__(self):
    super(TomDataset, self).__init__("TOM")

    # hold bag file names
    self.trash_bags = []
    self.box_bags = []

    # hold demos
    self.box = []
    self.trash = []

    self.trash_poses = []
    self.box_poses = []
    self.move_poses = []
    self.lift_poses = []
    self.test_poses = []
    self.pickup_poses = []

    self.trash_data = []
    self.box_data = []
    self.move_data = []
    self.lift_data = []
    self.test_data = []
    self.pickup_data = []

    self.robot = URDF.from_parameter_server()
    self.base_link = "torso_link"
    self.end_link = "r_gripper_base_link"

    # set up kinematics stuff
    self.tree = kdl_tree_from_urdf_model(self.robot)
    self.chain = self.tree.getChain(self.base_link, self.end_link)
    self.kdl_kin = KDLKinematics(self.robot, self.base_link, self.end_link)
	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))
示例#14
0
    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)
示例#15
0
    def __init__(self, limb):
        self._sawyer = URDF.from_parameter_server(key='robot_description')
        self._kdl_tree = kdl_tree_from_urdf_model(self._sawyer)
        self._base_link = self._sawyer.get_root()
        print "BASE LINK:", self._base_link
        self._tip_link = limb + '_hand'
        self._tip_frame = PyKDL.Frame()
        self._arm_chain = self._kdl_tree.getChain(self._base_link,
                                                  self._tip_link)

        # Sawyer Interface Limb Instances
        self._limb_interface = intera_interface.Limb(limb)
        self._joint_names = self._limb_interface.joint_names()
        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(0.0, 0.0, -9.81))
示例#16
0
    def __init__(self):
        """Read robot describtion"""
        self.robot_ = URDF.from_parameter_server()
        self.kdl_kin_ = KDLKinematics(self.robot_, 'base_link', 'end_effector')
        self.cur_pose_ = None
        self.cur_jnt_ = None
        # Creates the SimpleActionClient, passing the type of the action
        self.client_ = actionlib.SimpleActionClient(
            '/mitsubishi_arm/mitsubishi_trajectory_controller/follow_joint_trajectory',
            control_msgs.msg.FollowJointTrajectoryAction)
        self.client_.wait_for_server()
        self.goal_ = control_msgs.msg.FollowJointTrajectoryGoal()
        #time_from_start=rospy.Duration(10)
        self.goal_.trajectory.joint_names = [
            'j1', 'j2', 'j3', 'j4', 'j5', 'j6'
        ]
        #To be reached 1 second after starting along the trajectory
        trajectory_point = trajectory_msgs.msg.JointTrajectoryPoint()
        trajectory_point.positions = [0] * 6
        trajectory_point.time_from_start = rospy.Duration(0.1)
        self.goal_.trajectory.points.append(trajectory_point)

        rospy.Subscriber('joint_states', sensor_msgs.msg.JointState,
                         self.fwd_kinematics_cb)
        rospy.Subscriber('command', geometry_msgs.msg.Pose,
                         self.inv_kinematics_cb)
示例#17
0
    def __init__(self, limb, ee_frame_name="panda_link8", additional_segment_config=None, description=None):

        self._kdl_tree = None
        if description is None:
            self._franka = URDF.from_parameter_server(key='robot_description')
        else:
            self._franka = URDF.from_xml_file(description)

        self._kdl_tree = kdl_tree_from_urdf_model(self._franka)

        if additional_segment_config is not None:
            # add additional segments to account for NE_T_EE and F_T_NE transformations
            # ---- this may cause issues in eg. inertia computations maybe? TODO: test inertia behaviour
            for c in additional_segment_config:
                q = quaternion.from_rotation_matrix(c["origin_ori"]).tolist()
                kdl_origin_frame = PyKDL.Frame(PyKDL.Rotation.Quaternion(q.x, q.y, q.z, q.w),
                                             PyKDL.Vector(*(c["origin_pos"].tolist())))
                kdl_sgm = PyKDL.Segment(c["child_name"], PyKDL.Joint(c["joint_name"]),
                                      kdl_origin_frame, PyKDL.RigidBodyInertia())
                self._kdl_tree.addSegment(
                    kdl_sgm, c["parent_name"])

        self._base_link = self._franka.get_root()
        # self._tip_frame = PyKDL.Frame()
        self._limb_interface = limb
        self.create_chain(ee_frame_name)
示例#18
0
    def __init__(self):
        #Loads the robot model, which contains the robot's kinematics information
        self.robot = URDF.from_parameter_server()

        # This is where we hold general info about the joints
        self.joint_names = []
        self.joint_axes = []

        # Prepare general information about the robot
        self.get_joint_info()

        #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", CartesianCommand,
                         self.command_callback)

        #Subscribes to command for end-effector pose using numerical IK
        rospy.Subscriber("/ik_command", Transform, self.IK_command_callback)

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

        # Publishes direct command for positions
        self.pub_command = rospy.Publisher("/joint_command",
                                           JointState,
                                           queue_size=1)

        #This is where we hold the most recent joint info
        self.current_joint_state = JointState()
示例#19
0
    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 goals0.6
        #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)
示例#20
0
    def __init__(self):
        # Read the controllers parameters
        gains = read_parameter('~gains', dict())
        self.kp = joint_list_to_kdl(gains['Kp'])
        self.kd = joint_list_to_kdl(gains['Kd'])
        self.publish_rate = read_parameter('~publish_rate', 500)
        self.frame_id = read_parameter('~frame_id', 'base_link')
        self.tip_link = read_parameter('~tip_link', 'end_effector')
        # Kinematics
        self.urdf = URDF.from_parameter_server(key='robot_description')
        self.kinematics = KDLKinematics(self.urdf, self.frame_id,
                                        self.tip_link)
        # Get the joint names and limits
        self.joint_names = self.kinematics.get_joint_names()
        self.num_joints = len(self.joint_names)
        # Set-up publishers/subscribers
        self.torque_pub = dict()
        for i, name in enumerate(self.joint_names):
            self.torque_pub[name] = rospy.Publisher('/%s/command' % (name),
                                                    Float64)
        rospy.Subscriber('/joint_states', JointState, self.joint_states_cb)
        rospy.Subscriber('/grips/endpoint_state', EndpointState,
                         self.endpoint_state_cb)
        rospy.Subscriber('/grips/ik_command', PoseStamped, self.ik_command_cb)

        rospy.loginfo('Running Cartesian controller for Grips')
        # Start torque controller timer
        self.cart_command = None
        self.endpoint_state = None
        self.joint_states = None
        self.torque_controller_timer = rospy.Timer(
            rospy.Duration(1.0 / self.publish_rate), self.torque_controller_cb)
        # Shutdown hookup to clean-up before killing the script
        rospy.on_shutdown(self.shutdown)
示例#21
0
    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
示例#22
0
    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
    def __init__(self):
        self.robot = URDF.from_parameter_server()

        # This is where we hold general info about the joints
        self.num_joints = 0
        self.link = []
        self.joint = []
        self.joint_names = []
        self.joint_axes = []

        # Prepare general information about the robot
        self.get_joint_info()
        self.current_joint_state = JointState()

        # Wait for validity check service
        rospy.wait_for_service("check_state_validity")
        self.state_valid_service = rospy.ServiceProxy(
            'check_state_validity', moveit_msgs.srv.GetStateValidity)
        print "State validity service ready"

        # MoveIt parameter
        if self.num_joints == 7:
            self.group_name = "lwr_arm"
        elif self.num_joints == 6:
            self.group_name = "manipulator"

        self.pub_trajectory = rospy.Publisher('/joint_trajectory',
                                              JointTrajectory,
                                              queue_size=10)
        rospy.Subscriber("/joint_states", JointState, self.joint_callback)
        rospy.Subscriber("/motion_planning_goal", geometry_msgs.msg.Transform,
                         self.motion_planning_callback)
示例#24
0
	def __init__(self):

		# Wait for moveit IK service
		rospy.wait_for_service("compute_ik")
		self.ik_service = rospy.ServiceProxy('compute_ik',  moveit_msgs.srv.GetPositionIK)
		print "IK service ready"

		# Wait for validity check service
		rospy.wait_for_service("check_state_validity")
		self.state_valid_service = rospy.ServiceProxy('check_state_validity',  
			                                      moveit_msgs.srv.GetStateValidity)
		print "State validity service ready"

		# MoveIt parameter
		self.group_name = "manipulator"

		# Obtain basic information to implement my IK	
		self.robot = URDF.from_parameter_server()

		self.num_joints = 0
		self.joints = []
		self.joint_names = []		
		self.all_axes = []
		self.current_joint_state = sensor_msgs.msg.JointState()

		self.get_joint_info()
		
		# Initialize the publisher
		self.pub_trajectory = rospy.Publisher("/joint_trajectory", trajectory_msgs.msg.JointTrajectory, queue_size=1)
		# Subscribe to topics
		rospy.Subscriber("/joint_states", sensor_msgs.msg.JointState, self.callback_jointstate) 
		rospy.Subscriber("/motion_planning_goal", geometry_msgs.msg.Transform, self.callback_get_goal) 
    def __init__(self, limb='right'):
        self._sawyer = URDF.from_parameter_server(key='robot_description')
        self._kdl_tree = kdl_tree_from_urdf_model(self._sawyer)
        self._base_link = self._sawyer.get_root()

        # RUDI: LETS GET RID OF INTERA
        #self._tip_link = limb + '_hand'
        self._tip_link = 'right_gripper_tip'
        self._tip_frame = PyKDL.Frame()
        self._arm_chain = self._kdl_tree.getChain(self._base_link,
                                                  self._tip_link)

        # RUDI: LETS GET RID OF INTERA
        # Sawyer Interface Limb Instances
        # self._limb_interface = intera_interface.Limb(limb)
        #self._joint_names = self._limb_interface.joint_names()
        self._joint_names = ["right_j0", "right_j1", "right_j2", "right_j3", "right_j4", "right_j5", "right_j6"]
        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())
示例#26
0
    def __init__(self):
        #Load robot from parameter server
        self.robot = URDF.from_parameter_server()

        #Subscribe to current joint state of the robot
        rospy.Subscriber('/joint_states', JointState, self.get_joint_state)

        #This will load information about the joints of the robot
        self.num_joints = 0
        self.joint_names = []
        self.q_current = []
        self.joint_axes = []
        self.get_joint_info()

        #This is a mutex; a mutex(mutual exclusion object)is a program object that is
        #created so that multiple program thread can take turns sharing the same resource
        self.mutex = Lock()

        #Subscribers and publishers for for cartesian control
        rospy.Subscriber('/cartesian_command', CartesianCommand,
                         self.get_cartesian_command)
        self.velocity_pub = rospy.Publisher('/joint_velocities',
                                            JointState,
                                            queue_size=10)
        self.joint_velocity_msg = JointState()

        #Subscribers and publishers for numerical IK
        rospy.Subscriber('/ik_command', Transform, self.get_ik_command)
        self.joint_command_pub = rospy.Publisher('/joint_command',
                                                 JointState,
                                                 queue_size=10)
        self.joint_command_msg = JointState()
示例#27
0
    def __init__(self):
        rospy.init_node('expert_opt')

        # params
        self.base_link = rospy.get_param("~base_link", "base_link")
        self.biceps_link = rospy.get_param("~biceps_link", "biceps_link")
        self.camera_link = rospy.get_param("~camera_link", "camera_color_optical_frame")

        self.model_file = rospy.get_param("~model")              # required path to model file
        self.normp_file = rospy.get_param("~norm_params", "")    # optional path to normalization parameters (empty str means no norm params)

        # TODO - complete the line below to load up your model and create any necessary class instance variables
        self.model = ...

        # joint values
        self.joint1 = None
        self.joint3 = None

        # get robot model
        self.robot = URDF.from_parameter_server()

        # joint publishers
        self.joint1_pub = rospy.Publisher('/joint_1/command', Float64, queue_size=5)    # command for base rotation
        self.joint3_pub = rospy.Publisher('/joint_3/command', Float64, queue_size=5)    # command for robot tilt

        # joint subscriber
        rospy.Subscriber('/joint_states', JointState, self.joints_callback, queue_size=5)
        rospy.Subscriber('/target', PoseStamped, self.target_callback, queue_size=5)

        rospy.spin()
示例#28
0
def main(argv):

    root_name,tip_name=readArguments(argv)
    robot = URDF.from_parameter_server()


    desired_joint_chain=robot.get_chain(root=root_name,tip=tip_name,joints=True,links=False)
    desired_link_chain=robot.get_chain(root=root_name,tip=tip_name,joints=False,links=True)
    print desired_joint_chain
    print desired_link_chain
    q=sympy.symbols(desired_joint_chain)

    axes,transforms_dh,sigma=getKinematicInformation(robot,root_name,tip_name)

    transforms_sym=directGeometricModel(transforms_dh,q,axes,sigma)
    # Need deepcopy as transforms_sym is modified by function
    jacobians_sym=allDirectKinematicModels(deepcopy(transforms_sym),deepcopy(axes),deepcopy(sigma))


    write_transforms(desired_link_chain,desired_joint_chain,transforms_sym)# write transforms to file
    write_jacobians(desired_link_chain,desired_joint_chain,jacobians_sym)# write transforms to file


    for i,j in zip(transforms_sym,jacobians_sym):
        print "T=",sympy.pprint(i)
        print "J=",sympy.pprint(j)
示例#29
0
    def __init__(self, limb):
        self._kuka = URDF.from_parameter_server(key='robot_description')
        self._kdl_tree = kdl_tree_from_urdf_model(self._kuka)
        self._base_link = self._kuka.get_root()
        #         self._tip_link = limb + '_gripper'
        self._tip_link = 'tool0'
        self._tip_frame = PyKDL.Frame()
        self._arm_chain = self._kdl_tree.getChain(self._base_link,
                                                  self._tip_link)

        # Kuka Interface Limb Instances
        #         self._limb_interface = baxter_interface.Limb(limb)
        #         self._joint_names = self._limb_interface.joint_names()
        self._joint_names = [
            'joint_a1', 'joint_a2', 'joint_a3', 'joint_a4', 'joint_a5',
            'joint_a6'
        ]
        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())
示例#30
0
    def __init__(self, limb):
        self._pr2 = URDF.from_parameter_server(key='robot_description')
        self._kdl_tree = kdl_tree_from_urdf_model(self._pr2)
        self._base_link = self._pr2.get_root()
        # self._tip_link = limb + '_gripper'
        self._tip_link = limb + '_wrist_roll_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 = baxter_interface.Limb(limb)
        # self._joint_names = self._limb_interface.joint_names()
        self._joint_names = [
            limb + '_shoulder_pan_joint', limb + '_shoulder_lift_joint',
            limb + '_upper_arm_roll_joint', limb + '_elbow_flex_joint',
            limb + '_forearm_roll_joint', limb + '_wrist_flex_joint',
            limb + '_wrist_roll_joint'
        ]
        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())
    def __init__(self, disable_ft=False):
        self.urdf = URDF.from_parameter_server()
        self.overhead_vision_client = actionlib.SimpleActionClient(
            "recognize_objects", ObjectRecognitionAction)
        self.execute_trajectory_action = actionlib.SimpleActionClient(
            "execute_trajectory", ExecuteTrajectoryAction)
        self.rapid_node = rapid_node_pkg.RAPIDCommander()
        self.controller_commander = controller_commander_pkg.ControllerCommander(
        )
        self.state = 'init'
        self.current_target = None
        self.current_payload = None
        self.available_payloads = {
            'leeward_mid_panel': 'leeward_mid_panel',
            'leeward_tip_panel': 'leeward_tip_panel'
        }
        self.desired_controller_mode = self.controller_commander.MODE_AUTO_TRAJECTORY
        self.speed_scalar = 1.0
        self.disable_ft = disable_ft

        self.tf_listener = PayloadTransformListener()
        self._process_state_pub = rospy.Publisher("process_state",
                                                  ProcessState,
                                                  queue_size=100,
                                                  latch=True)
        self.publish_process_state()

        self.update_payload_pose_srv = rospy.ServiceProxy(
            "update_payload_pose", UpdatePayloadPose)
        self.get_payload_array_srv = rospy.ServiceProxy(
            "get_payload_array", GetPayloadArray)

        self.plan_dictionary = {}
示例#32
0
    def __init__(self, limb, description=None):

        if description is None:
            self._franka = URDF.from_parameter_server(key='robot_description')
        else:
            self._franka = URDF.from_xml_file(description)

        self._kdl_tree = kdl_tree_from_urdf_model(self._franka)
        self._base_link = self._franka.get_root()
        self._tip_link = limb.name + ('_hand'
                                      if limb.has_gripper else '_link8')
        self._tip_frame = PyKDL.Frame()
        self._arm_chain = self._kdl_tree.getChain(self._base_link,
                                                  self._tip_link)

        self._limb_interface = limb
        self._joint_names = deepcopy(self._limb_interface.joint_names())
        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())
    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.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.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.1), self.timer_callback)
示例#34
0
    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
示例#35
0
    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())
示例#36
0
    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)
示例#37
0
	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
示例#38
0
    def __init__(self):
        rospy.init_node('com_calculation', anonymous=True)
        self.base_link_frame = rospy.get_param("~base_link_frame",
                                               "base_link_frame")
        self.tf_prefix = rospy.get_param("~tf_prefix", "")
        self.Mass = 0
        #get robot description from URDF
        robot = URDF.from_parameter_server()
        self.links = robot.link_map

        #Delete links, which contain no mass description
        unnecessary_links = []
        for link in self.links:
            if self.links[link].inertial == None:
                unnecessary_links.append(link)

        for link in unnecessary_links:
            del self.links[link]

        #Calculate the total mass of the robot
        for link in self.links:
            self.Mass += self.links[link].inertial.mass

        rospy.loginfo("Mass of robot is %f", self.Mass)
        self.calculator()
示例#39
0
def callback(joint_values):
    #rospy.loginfo("this is joint_values.name")
    #rospy.loginfo(joint_values.name)

    robot = URDF.from_parameter_server()

    link_names = []
    joints = []
    joints_names = []

    link_name = robot.get_root()
    while True:
        if link_name not in robot.child_map:
            break

        (next_joint_name, next_link_name) = robot.child_map[link_name][0]

        if next_joint_name not in robot.joint_map:
            break

        joints.append(robot.joint_map[next_joint_name])
        link_names.append(next_link_name)
        joints_names.append(next_joint_name)

        link_name = next_link_name

    #rospy.loginfo("this is joints_names")
    #rospy.loginfo(joints_names)

    calculate_and_publish_transforms(link_names, joints, joint_values)
示例#40
0
    def __init__(self):

        #Loads the robot model, which contains the robot's kinematics information
	self.num_joints = 0
        self.joint_names = []
        self.joint_axes = []
        self.robot = URDF.from_parameter_server()
        self.base = self.robot.get_root()
        self.get_joint_info()

        # Wait for moveit IK service
        rospy.wait_for_service("compute_ik")
        self.ik_service = rospy.ServiceProxy('compute_ik',  moveit_msgs.srv.GetPositionIK)
        print "IK service ready"

        # Wait for validity check service
        rospy.wait_for_service("check_state_validity")
        self.state_valid_service = rospy.ServiceProxy('check_state_validity',  
                                                      moveit_msgs.srv.GetStateValidity)
        print "State validity service ready"

        # MoveIt parameter
        robot_moveit = moveit_commander.RobotCommander()
        self.group_name = robot_moveit.get_group_names()[0]

	#Subscribe to topics
	rospy.Subscriber('/joint_states', JointState, self.get_joint_state)
	rospy.Subscriber('/motion_planning_goal', Transform, self.motion_planning)
        self.current_obstacle = "None"
        rospy.Subscriber('/obstacle', String, self.get_obstacle)

	#Set up publisher
	self.pub = rospy.Publisher('/joint_trajectory', JointTrajectory, queue_size=10)
示例#41
0
    def __init__(self, use_moveit=False):
        self.use_moveit = use_moveit
        self.baxter = URDF.from_parameter_server(key='robot_description')
        self.kdl_tree = kdl_tree_from_urdf_model(self.baxter)
        self.base_link = self.baxter.get_root()

        self.right_limb_interface = baxter_interface.Limb('right')
        self.left_limb_interface = baxter_interface.Limb('left')

        self.get_link_state = rospy.ServiceProxy("/gazebo/get_link_state", GetLinkState)
        self.get_model_state = rospy.ServiceProxy("/gazebo/get_model_state", GetModelState)
        self.set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
        self.set_model_config = rospy.ServiceProxy('/gazebo/set_model_configuration', SetModelConfiguration)

        # Listen to collision information
        # self.collision_getter = InfoGetter()
        # self.collision_topic = "/hug_collision"

        # Verify robot is enabled
        print("Getting robot state... ")
        self._rs = baxter_interface.RobotEnable()
        self._init_state = self._rs.state().enabled
        if not self._init_state:
            print("Enabling robot... ")
            self._rs.enable()

        if self.use_moveit:
            # Moveit group setup
            moveit_commander.roscpp_initialize(sys.argv)
            self.robot = moveit_commander.RobotCommander()
            self.scene = moveit_python.PlanningSceneInterface(self.robot.get_planning_frame())
            # self.scene = moveit_commander.PlanningSceneInterface()
            self.group_name = "right_arm"
            self.group = moveit_commander.MoveGroupCommander(self.group_name)

        # ######################## Create Hugging target #############################
        # humanoid hugging target # remember to make sure target_line correct + - y
        self.target_pos_start = np.asarray([0.5, 0, -0.93]) # robot /base frame, z = -0.93 w.r.t /world frame
        self.target_line_start = np.empty([22, 3], float)
        for i in range(11):
            self.target_line_start[i] = self.target_pos_start + [0, -0.0, 1.8] - (asarray([0, -0.0, 1.8]) - asarray([0, -0.0, 0.3]))/10*i
            self.target_line_start[i+11] = self.target_pos_start + [0, -0.5, 1.3] + (asarray([0, 0.5, 1.3]) - asarray([0, -0.5, 1.3]))/10*i
        self.target_line = self.target_line_start

        # Build line point graph for interaction mesh
        # reset right arm
        start_point_right = [-0.3, 1.0, 0.0, 0.5, 0.0, 0.027, 0.0]
        t01 = threading.Thread(target=resetarm_job, args=(self.right_limb_interface, start_point_right))
        t01.start()
        # reset left arm
        start_point_left = [0.3, 1.0, 0.0, 0.5, 0.0, 0.027, 0.0]
        t02 = threading.Thread(target=resetarm_job, args=(self.left_limb_interface, start_point_left))
        t02.start()
        t02.join()
        t01.join()

        right_limb_pose, _ = limbPose(self.kdl_tree, self.base_link, self.right_limb_interface, 'right')
        left_limb_pose, _ = limbPose(self.kdl_tree, self.base_link, self.left_limb_interface, 'left')
        graph_points = np.concatenate((right_limb_pose[5:], left_limb_pose[5:], self.target_line_start), 0)
        self.triangulation = Delaunay(graph_points)
def main():
    parser = argparse.ArgumentParser(usage='Load an URDF file')
    parser.add_argument('file',
                        type=argparse.FileType('r'),
                        nargs='?',
                        default=None,
                        help='File to load. Use - for stdin')
    parser.add_argument('-o',
                        '--output',
                        type=argparse.FileType('w'),
                        default=None,
                        help='Dump file to XML')
    args = parser.parse_args()

    if args.file is None:
        print 'FROM PARAM SERVER'
        robot = URDF.from_parameter_server()
    else:
        print 'FROM STRING'
        robot = URDF.from_xml_string(args.file.read())

    print(robot)

    if args.output is not None:
        args.output.write(robot.to_xml_string())
示例#43
0
文件: mp.py 项目: Iwtbm/ROS
    def __init__(self):
        self.robot = URDF.from_parameter_server()

        self.num_joints = 0
        self.joint_names = []
        self.joint_axes = []
        self.joint_transforms = []
        self.x_current = tf.transformations.identity_matrix()
        self.get_joint_info()
        self.current_joint_state = JointState()

        rospy.Subscriber("joint_states", JointState, self.js_callback)

        # Wait for validity check service
        rospy.wait_for_service("check_state_validity")
        self.state_valid_service = rospy.ServiceProxy(
            'check_state_validity', moveit_msgs.srv.GetStateValidity)
        print "State validity service ready"

        # MoveIt parameter
        self.group_name = "lwr_arm"

        rospy.Subscriber("/motion_planning_goal", Transform, self.mp_callback)

        self.pub_trajectory = rospy.Publisher("/joint_trajectory",
                                              JointTrajectory,
                                              queue_size=1)
    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')
    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)
示例#46
0
    def __init__(self):
        """Announces that it will publish forward kinematics results, in the form of tfMessages.
        "tf" stands for "transform library", it's ROS's way of communicating around information
        about where things are in the world"""
        self.pub_tf = rospy.Publisher("/tf", tf.msg.tfMessage, queue_size=1)

        #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.callback)
示例#47
0
def main():
    a = rospy.init_node('set_base_frames')
    
    global psm1_kin, psm1_robot, psm2_kin, psm2_robot, ecm_kin, ecm_robot, mtmr_robot, mtmr_kin
    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)
        ecm_base = KDLKinematics(ecm_robot, ecm_robot.links[0].name, ecm_robot.links[3].name)
    if mtmr_robot is None:
        mtmr_robot = URDF.from_parameter_server('/dvrk_mtmr/robot_description')
        mtmr_base = KDLKinematics(mtmr_robot, mtmr_robot.links[0].name, mtmr_robot.links[1].name)
    # pdb.set_trace()    

    psm1_pub = rospy.Publisher('/dvrk/PSM1/set_base_frame', Pose, latch=True, queue_size=1)
    psm2_pub = rospy.Publisher('/dvrk/PSM2/set_base_frame', Pose, latch=True, queue_size=1)
    mtmr_pub = rospy.Publisher('/dvrk/MTMR/set_base_frame', Pose, latch=True, queue_size=1)
    
    ecm_tip = ecm_kin.forward([1,1,1,1]) # ECM Tool Tip
    ecm_tip = np.linalg.inv(ecm_tip)
    
    psm1_base_frame = psm1_kin.forward([])
    psm1_message = pose_converter.PoseConv.to_pose_msg(psm1_base_frame)

    psm2_base_frame = psm2_kin.forward([])
    psm2_message = pose_converter.PoseConv.to_pose_msg(psm2_base_frame)
    
    psm1_pub.publish(psm1_message)
    psm2_pub.publish(psm2_message)
#     mtmr_pub.publish(message)
    print (psm1_message)
    print (psm2_message)
    sleep (1)
    def _wait_and_get_robot(self):
        t_init = rospy.Time.now()
        t_timeout = rospy.Duration(5)  # 10 seconds

        while rospy.Time.now() - t_init < t_timeout:
            try:
                robot = URDF.from_parameter_server("/mitsubishi_arm/robot_description")
                print "*** Obtained robot_description ***"
                return robot
            except KeyError:
                print "Key error."
                rospy.sleep(rospy.Duration(1))

        raise KeyError("robot_description was not found")
示例#49
0
 def __init__(self):
     # 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.ecm_msg = None
     
     self.__clutchNGo_mode__ = self.MODE.simulation
     
     self.autocamera = Autocamera()
def find_feasible_release(catch_x, catch_y, catch_z, pos):
    found = False;
    robot = URDF.from_parameter_server()
    base_link = robot.get_root()
    kdl_kin = KDLKinematics(robot, base_link, 'right_gripper_base')

    while not found:
        X, th_init, throw_y, throw_z, vel, alpha = sample_state(catch_x, catch_y, catch_z, pos)
        ik_test, q_ik = test_for_ik(X, th_init, kdl_kin)
        if ik_test:
            if test_joint_vel(q_ik, vel, alpha, kdl_kin):
                found = True
        print q_ik

    return throw_y, throw_z, vel, alpha
def move_to(pos):

    pub_demo_state = rospy.Publisher('demo_state',Int16, queue_size = 10)

    if (pos == 1):
        catch = np.array([.65, .2, 0]) # my .7?
        R = np.array([[ 0.26397895, -0.34002068,  0.90260791],
        [-0.01747676, -0.93733484, -0.34799134],
        [ 0.96437009,  0.07608772, -0.25337913]])
    elif (pos == 2):
        catch = np.array([.68, -.05, 0])
        R = np.array([[0,0,1],[0,-1,0],[1,0,0]])
    elif (pos == 3):
        catch = np.array([.72, .1, 0])
        R = np.array([[ 0.26397895, -0.34002068,  0.90260791],
        [-0.01747676, -0.93733484, -0.34799134],
        [ 0.96437009,  0.07608772, -0.25337913]])
    else:
        pass

    th_init = np.array([-.3048, -.2703, -.1848, 1.908, .758, -1.234, -3.04]) 

    X = RpToTrans(R,catch)

    # Find end joint angles with IK
    robot = URDF.from_parameter_server()
    base_link = robot.get_root()
    kdl_kin = KDLKinematics(robot, base_link, 'left_gripper_base')
    seed = 0
    q_ik = kdl_kin.inverse(X, th_init)
    while q_ik == None:
        seed += 0.01
        q_ik = kdl_kin.inverse(X, th_init+seed)
        if (seed>1):
            # return False
            break
    q_goal = q_ik
    print q_goal

    limb_interface = baxter_interface.limb.Limb('left')
    angles = limb_interface.joint_angles()
    for ind, joint in enumerate(limb_interface.joint_names()):
        angles[joint] = q_goal[ind]    

    limb_interface.move_to_joint_positions(angles)
    # rospy.sleep(5)
    print 'done'
    pub_demo_state.publish(1)
    def __init__(self, side, step, x_min, x_max, y_min, y_max, z_min, z_max, pt_c_in_T2, min_dist, max_dist, rot):
        self.min_dist2 = min_dist*min_dist
        self.max_dist2 = max_dist*max_dist
        self.x_set = list(np.arange(x_min, x_max, step))
        self.y_set = list(np.arange(y_min, y_max, step))
        if z_min > z_max:
            self.z_set = list(np.arange(z_min, z_max, -step))
        else:
            self.z_set = list(np.arange(z_min, z_max, step))
        self.pt_c_in_T2 = pt_c_in_T2
        self.rot = rot

        self.robot = URDF.from_parameter_server()
        self.tree = kdl_tree_from_urdf_model(self.robot)

        self.chain = self.tree.getChain("torso_link2", side + "_HandPalmLink")

        self.q_min = PyKDL.JntArray(7)
        self.q_max = PyKDL.JntArray(7)
        self.q_limit = 0.01
        self.q_min[0] = -2.96 + self.q_limit
        self.q_min[1] = -2.09 + self.q_limit
        self.q_min[2] = -2.96 + self.q_limit
#        self.q_min[3] = -2.09 + self.q_limit
        self.q_min[3] = 15.0/180.0*math.pi
        self.q_min[4] = -2.96 + self.q_limit
        self.q_min[5] = -2.09 + self.q_limit
        self.q_min[6] = -2.96 + self.q_limit
        self.q_max[0] = 2.96 - self.q_limit
#        self.q_max[1] = 2.09 - self.q_limit
        self.q_max[1] = -15.0/180.0*math.pi
        self.q_max[2] = 2.96 - self.q_limit
        self.q_max[3] = 2.09 - self.q_limit
        self.q_max[4] = 2.96 - self.q_limit
#        self.q_max[5] = 2.09 - self.q_limit
        self.q_max[5] = -15.0/180.0*math.pi
        self.q_max[6] = 2.96 - self.q_limit
        self.fk_solver = PyKDL.ChainFkSolverPos_recursive(self.chain)
        self.vel_ik_solver = PyKDL.ChainIkSolverVel_pinv(self.chain)
        self.ik_solver = PyKDL.ChainIkSolverPos_NR_JL(self.chain, self.q_min, self.q_max, self.fk_solver, self.vel_ik_solver, 100)

        self.q_out = PyKDL.JntArray(7)

        self.singularity_angle = 15.0/180.0*math.pi
def main():
    parser = argparse.ArgumentParser(usage='Load an URDF file')
    parser.add_argument('file', type=argparse.FileType('r'), nargs='?',
                        default=None, help='File to load. Use - for stdin')
    parser.add_argument('-o', '--output', type=argparse.FileType('w'),
                        default=None, help='Dump file to XML')
    args = parser.parse_args()

    if args.file is None:
        print 'FROM PARAM SERVER'
        robot = URDF.from_parameter_server()
    else:
        print 'FROM STRING'
        robot = URDF.from_xml_string(args.file.read())

    print(robot)

    if args.output is not None:
        args.output.write(robot.to_xml_string())
示例#54
0
    def __init__(self):
        self.fk_solver = velma_fk_ik.VelmaFkIkSolver()

        self.robot = URDF.from_parameter_server()
        self.mimic_joints_map = {}
        self.joint_name_limit_map = {}

        joint_idx_ros = 0
        for i in range(len(self.robot.joints)):
            joint = self.robot.joints[i]

            if joint.joint_type == "fixed":
                continue
            if joint.mimic != None:
                self.mimic_joints_map[joint.name] = joint.mimic

            self.joint_name_limit_map[joint.name] = joint.limit

            joint_idx_ros += 1
示例#55
0
    def __init__(self):
        """Read robot describtion"""
        self.robot_ = URDF.from_parameter_server() 
        self.kdl_kin_ = KDLKinematics(self.robot_, 'base_link', 'end_effector')
        self.cur_pose_ = None
        self.cur_jnt_ = None
        # Creates the SimpleActionClient, passing the type of the action
        self.client_ = actionlib.SimpleActionClient('/mitsubishi_arm/mitsubishi_trajectory_controller/follow_joint_trajectory', control_msgs.msg.FollowJointTrajectoryAction)
        self.client_.wait_for_server()
        self.goal_ = control_msgs.msg.FollowJointTrajectoryGoal()
        #time_from_start=rospy.Duration(10)
        self.goal_.trajectory.joint_names=['j1','j2','j3','j4','j5','j6']
        #To be reached 1 second after starting along the trajectory
        trajectory_point=trajectory_msgs.msg.JointTrajectoryPoint()
        trajectory_point.positions=[0]*6
        trajectory_point.time_from_start=rospy.Duration(0.1)
        self.goal_.trajectory.points.append(trajectory_point)

        rospy.Subscriber('joint_states', sensor_msgs.msg.JointState, self.fwd_kinematics_cb)
        rospy.Subscriber('command', geometry_msgs.msg.Pose, self.inv_kinematics_cb)
示例#56
0
    def __init__(self, iksvc, limb):
        self.centroid = None
        #self.x_extremes = None
        self.axes = None
        self.ir_reading = None
        self.iksvc = iksvc
        self.limb = limb
        self.limb_iface = baxter_interface.Limb(limb)
        self.gripper_if = baxter_interface.Gripper(limb)
        self.tf_listener = tf.TransformListener()
        self.stateidx = 0
        self.states = self.wait_centroid, self.orient, self.servo_xy, self.servo_z, self.grip_state, self.done_state
        self.done = 0

        self.sign = 1

        #Check if we are exceeding the joint limit specified by robot_urdf
        robot = URDF.from_parameter_server()
        key = limb+"_w2"
        for joint in robot.joints:
            if joint.name == key:
                break
        
        self.wristlim = joint.limit

        """paramnames = ["servo_speed", "min_pose_z", "min_ir_depth"]
        paramvals = []
        for param in paramnames:
            topic = "/servo_to_object/"
            paramvals.append(rospy.get_param(topic+param))
        self.inc, self.min_pose_z, self.min_ir_depth = tuple(paramvals)
        self.goal_pos = (rospy.get_param(topic+"camera_x")*float(rospy.get_param(topic+"goal_ratio_x")), rospy.get_param(topic+"camera_y")*float(rospy.get_param(topic+"goal_ratio_y")))
        self.grip_height = self.min_pose_z"""

        self.inc = params['servo_speed']
        self.angle_inc = params['angle_inc']
        self.min_pose_z = params['min_pose_z']
        self.min_ir_depth = params['min_ir_depth']
        self.goal_pos = (params['camera_x']*params['goal_ratio_x'],\
                         params['camera_y']*params['goal_ratio_y'])
def callback(msgs):
    "msgs = ContactStatesStamped"
    global g_config
    if g_config.use_parent_link:
        urdf_robot = URDF.from_parameter_server()
    marker_array = MarkerArray()
    for msg, i in zip(msgs.states, range(len(msgs.states))):
        marker = Marker()
        link_name = msg.header.frame_id
        if g_config.use_parent_link:
            # lookup parent link
            chain = urdf_robot.get_chain(urdf_robot.get_root(), link_name)
            link_name = chain[-3]
        mesh_file, offset = find_mesh(link_name)
        marker.header.frame_id = link_name
        marker.header.stamp = rospy.Time.now()
        marker.type = Marker.MESH_RESOURCE
        if msg.state.state == ContactState.ON:
            marker.color.a = g_config.on_alpha
            marker.color.r = g_config.on_red
            marker.color.g = g_config.on_green
            marker.color.b = g_config.on_blue
        elif msg.state.state == ContactState.OFF:
            marker.color.a = g_config.off_alpha
            marker.color.r = g_config.off_red
            marker.color.g = g_config.off_green
            marker.color.b = g_config.off_blue
        marker.scale.x = g_config.marker_scale
        marker.scale.y = g_config.marker_scale
        marker.scale.z = g_config.marker_scale
        marker.pose = offset
        marker.mesh_resource = mesh_file
        marker.frame_locked = True
        marker.id = i
        if msg.state.state == ContactState.OFF:
            if not g_config.visualize_off:
                marker.action = Marker.DELETE
        marker_array.markers.append(marker)
    pub.publish(marker_array)
示例#58
0
    def __init__(self, limb):
        self._baxter = URDF.from_parameter_server(key='robot_description')
        self._kdl_tree = kdl_tree_from_urdf_model(self._baxter)
        self._base_link = self._baxter.get_root()
        self._tip_link = limb + '_gripper'
        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 = baxter_interface.Limb(limb)
        self._joint_names = self._limb_interface.joint_names()
        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())
示例#59
0
def main():
    rospy.init_node('set_base_frames')
    sleep (1)
    global psm1_kin, psm1_robot, psm2_kin, psm2_robot, ecm_kin, ecm_robot
    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)
        ecm_base = KDLKinematics(ecm_robot, ecm_robot.links[0].name, ecm_robot.links[3].name)
   
    # pdb.set_trace()    
    
    mtml_pub = rospy.Publisher('/dvrk/MTML/set_base_frame', Pose, queue_size=1)
    mtmr_pub = rospy.Publisher('/dvrk/MTMR/set_base_frame', Pose, queue_size=1)
    ecm_pub = rospy.Publisher('/dvrk/ECM/set_base_frame', Pose, queue_size=1)
    psm1_pub = rospy.Publisher('/dvrk/PSM1/set_base_frame', Pose, queue_size=1)
    psm2_pub = rospy.Publisher('/dvrk/PSM2/set_base_frame', Pose, queue_size=1)
    
    mtmr_psm1 = rospy.Publisher('/dvrk/MTMR_PSM1/set_registration_rotation', Quaternion, queue_size=1)
    mtml_psm2 = rospy.Publisher('/dvrk/MTML_PSM2/set_registration_rotation', Quaternion, queue_size=1)
        
    
    p1_base = psm1_kin.forward([]) # PSM1 Base Frame
    p2_base = psm2_kin.forward([]) # PSM2 Base Frame
    e_base = ecm_base.forward([]) # ECM Base Frame
    
    
    e = ecm_kin.forward([0,0,0,0]) # ECM Tool Tip
    
    camera_view_transform = pose_converter.PoseConv.to_homo_mat( [(0.0,0.0,0.0), (1.57079632679, 0.0, 0.0)])
    
    r = lambda axis, rad: rotate(axis, rad)
    
    mtmr_m = e;# mtmr_m = mtmr_m**-1 
    
    mtml_m = e

    print 'qmat'
    qmsg = Quaternion()
    temp = quat_from_homomat(p1_base)
    print quat_from_homomat(p1_base)
    
    message = pose_from_homomat(p1_base);
    psm1_pub.publish(message)
    while not rospy.is_shutdown():
        #print p1_base
        #print message
        psm1_pub.publish(message)
        print ('sure\n')
        
        
#     psm2_pub.publish(pose_from_homomat(p2_base))
#     psm1_pub.publish(pose_from_homomat(e_base))
#     mtml_pub.publish( pose_from_homomat(mtml_m))
#     mtmr_pub.publish( pose_from_homomat(mtmr_m))
    
           
    print ('\n\Hello:  nmtml rotation: \n')
    print( mtml_m.__repr__( ))
    print(pose_from_homomat(mtml_m)) 
    print ('\n\nmtmr rotation: \n')
    print( mtmr_m.__repr__())
    def execute_move(self, pos):
        rospy.loginfo('moving')
        # Read in pose data
        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')
        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)
        Xstart = copy(np.asarray(pose))
        pose[0:3,0:3] = R
        pose[0:3,3] = p
        Xend = copy(np.asarray(pose))
        
        # Compute straight-line trajectory for path
        N = 500
        Xlist = CartesianTrajectory(Xstart, Xend, 1, N, 5)
        thList = np.empty((N,7))
        thList[0] = q0;
        
        for i in range(N-1):
        # Solve for joint angles
            seed = 0
            q_ik = kdl_kin.inverse(Xlist[i+1], thList[i])
            while q_ik == None:
                seed += 0.3
                q_ik = kdl_kin.inverse(pose, q0+seed)
            thList[i+1] = q_ik
#            rospy.loginfo(q_ik)
        
#        # Solve for joint angles
#        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)
        
#        q_list = JointTrajectory(q0,q_ik,1,100,5)
        
#        for q in q_list:
            # Format joint angles as limb joint angle assignment      
            angles = limb_interface.joint_angles()
            angle_count = 0
            for ind, joint in enumerate(limb_interface.joint_names()):
#                if fabs(angles[joint] - q_ik[ind]) < .05:
#                    angle_count += 1
                angles[joint] = q_ik[ind]
#            rospy.loginfo(angles)
            rospy.sleep(.003)
            
            # Send joint move command
            rospy.loginfo('move to object')
#            rospy.loginfo(angle_count)
#            if angle_count > 4:
#                nothing = True;
#            else:
            limb_interface.set_joint_position_speed(.3)
            limb_interface.set_joint_positions(angles)
        self._done = True
        print('Done')