def __init__(self): self.num_joints = 7 # Get the URDF urdf_path = os.path.join(os.path.dirname(os.path.realpath(__file__)), '../assets/urdf/sawyer_intera.urdf') self.urdf_model = URDF.from_xml_string(open(urdf_path).read()) # Set the base link, and the tip link. self.base_link = self.urdf_model.get_root() self.tip_link = "right_hand" # # Create a KDL tree from the URDF model. This is an object defining the kinematics of the entire robot. self.kdl_tree = treeFromUrdfModel(self.urdf_model) # Create a KDL chain from the tree. This is one specific chain within the overall kinematics model. self.arm_chain = self.kdl_tree[1].getChain(self.base_link, self.tip_link) # Create a solver which will be used to compute the forward kinematics self.forward_kinematics_solver = PyKDL.ChainFkSolverPos_recursive( self.arm_chain) # Create a solver which will be used to compute the Jacobian self.jacobian_solver = PyKDL.ChainJntToJacSolver(self.arm_chain) #Velocity inverse kinematics solver self.IK_v_solver = PyKDL.ChainIkSolverVel_pinv(self.arm_chain) # Create a solver to retrieve the joint angles form the eef position self.IK_solver = PyKDL.ChainIkSolverPos_NR( self.arm_chain, self.forward_kinematics_solver, self.IK_v_solver)
def __init__(self, urdf, srdf, group_name): self._srdf = SRDF.Group.from_xml_string(srdf) self._urdf = URDF.urdf.URDF.from_xml_string(urdf) self._tree = URDF.treeFromUrdfModel(self._urdf)[1] group = None for g in self._srdf.groups: if g.name == group_name: group = g if not group: raise Exception( "[ServoIk] No group named {} found.".format(group_name)) self.base_link = group.chains[0].base_link self.tip_link = group.chains[0].tip_link self._tip_frame = kdl.Frame() self._chain = self._tree.getChain(self.base_link, self.tip_link) self._joint_names = self._urdf.joint_map.keys() self._num_jnts = self._chain.getNrOfJoints() # Joint limits self.lower_limits = [] self.upper_limits = [] for jnt in self._urdf.joints: if jnt.type != 'fixed': self.lower_limits.append(jnt.limit.lower) self.upper_limits.append(jnt.limit.upper) # KDL Solvers self._fk_p = kdl.ChainFkSolverPos_recursive(self._chain) self._ik_v = kdl.ChainIkSolverVel_pinv(self._chain) rospy.loginfo( "[ServoIk] Started ServoIk for group: {}".format(group_name))
def __init__( self, urdf_relative_path='../../assets/urdf/sawyer_arm_intera.urdf'): ''' Uses inverse kinematics in order to go from end-effector cartesian velocity commands to joint velocity commands :param urdf_relative_path: The path to the URDF file of the robot, relative to the current directory ''' self.num_joints = 7 # Get the URDF urdf_path = os.path.join(os.path.dirname(os.path.realpath(__file__)), urdf_relative_path) self.urdf_model = URDF.from_xml_string(open(urdf_path).read()) # Set the base link, and the tip link. I think that these are just strings, defined somewhere in the URDF. The name 'right_hand' is how the gripper is described on the real Sawyer, but it may have a different name in your own URDF. self.base_link = self.urdf_model.get_root() self.tip_link = "right_hand" # # Create a KDL tree from the URDF model. This is an object defining the kinematics of the entire robot. self.kdl_tree = treeFromUrdfModel(self.urdf_model) # Create a KDL chain from the tree. This is one specific chain within the overall kinematics model. self.arm_chain = self.kdl_tree[1].getChain(self.base_link, self.tip_link) # Create a solver which will be used to compute the forward kinematics self.forward_kinematics_solver = PyKDL.ChainFkSolverPos_recursive( self.arm_chain) # Create a solver which will be used to compute the Jacobian self.jacobian_solver = PyKDL.ChainJntToJacSolver(self.arm_chain) #Velocity inverse kinematics solver self.IK_v_solver = PyKDL.ChainIkSolverVel_pinv(self.arm_chain) # Create a solver to retrieve the joint angles form the eef position self.IK_solver = PyKDL.ChainIkSolverPos_NR( self.arm_chain, self.forward_kinematics_solver, self.IK_v_solver)
def __init__(self): self.num_joints = 7 # Get the URDF (you will need to write your own "load_urdf()" function) urdf_path = os.path.join(assets_root, './kdl/panda_urdf/panda_arm_hand.urdf') # urdf_path= os.path.join(robosuite.models.assets_root, "bullet_data/sawyer_description/urdf/sawyer_intera.urdf") self.urdf_model = URDF.from_xml_string( open(urdf_path).read().encode( 'ascii')) # encode to ascii solve the error #self.kdl_tree = treeFromFile(urdf_path) # Set the base link, and the tip link. I think that these are just strings, defined somewhere in the URDF. The name 'right_hand' is how the gripper is described on the real Sawyer, but it may have a different name in your own URDF. self.base_link = self.urdf_model.get_root() self.tip_link = "panda_hand" # hand of panda robot, see panda_arm_hand.urdf # # Create a KDL tree from the URDF model. This is an object defining the kinematics of the entire robot. self.kdl_tree = treeFromUrdfModel(self.urdf_model) # Create a KDL chain from the tree. This is one specific chain within the overall kinematics model. self.arm_chain = self.kdl_tree[1].getChain(self.base_link, self.tip_link) # Create a solver which will be used to compute the forward kinematics self.forward_kinematics_solver = PyKDL.ChainFkSolverPos_recursive( self.arm_chain) # Create a solver which will be used to compute the Jacobian self.jacobian_solver = PyKDL.ChainJntToJacSolver(self.arm_chain) #Velocity inverse kinematics solver self.IK_v_solver = PyKDL.ChainIkSolverVel_pinv(self.arm_chain) # Create a solver to retrieve the joint angles form the eef position self.IK_solver = PyKDL.ChainIkSolverPos_NR( self.arm_chain, self.forward_kinematics_solver, self.IK_v_solver)
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())
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())
def __init__(self, urdf, base_link="base_link", end_link="gripper"): [tree_ok, robot_tree_] = treeFromUrdfModel(urdf) if not tree_ok: rospy.logerr("KinematicChain: URDF tree is not parsed properly") raise Exception("KinematicChain: URDF tree is not parsed properly") self.robot_tree_ = robot_tree_ self.urdf_model_ = urdf self.kin_chain_ = robot_tree_.getChain(base_link, end_link) self.base_link_ = base_link self.end_link_ = end_link self.joints_name_ = self.get_joints_name() self.joints_type_ = self.get_joints_type() self.links_name_ = self.get_links_name() self.num_joints_ = len(self.joints_name_) self.forward_kin_ = kdl.ChainFkSolverPos_recursive(self.kin_chain_) self.joint_states = [0., 0., 0., 0., 0., 0.] rospy.Subscriber("/arm/joint_states", JointState, self.jointStateCallback)
def __init__(self, name, base, ee_link, namespace=None, arm_name=None, compute_fk_for_all=False): # Joint states self._joint_angles = dict() self._joint_velocity = dict() self._joint_effort = dict() # Finger chain name self._name = name # Namespace if None in [namespace, arm_name]: ns = [ item for item in rospy.get_namespace().split('/') if len(item) > 0 ] if len(ns) != 2: rospy.ROSException( 'The controller must be called inside the namespace the manipulator namespace' ) self._namespace = ns[0] self._arm_name = ns[1] else: self._namespace = namespace self._arm_name = arm_name if self._namespace[0] != '/': self._namespace = '/' + self._namespace if self._namespace[-1] != '/': self._namespace += '/' # True if it has to compute the forward kinematics for all frames self._compute_fk_for_all = compute_fk_for_all # List of frames for each link self._frames = dict() # Load robot description (complete robot, including vehicle, if # available) self._robot_description = URDF.from_parameter_server( key=self._namespace + 'robot_description') # KDL tree of the whole structure ok, self._kdl_tree = treeFromUrdfModel(self._robot_description) # Base link self._base_link = base # Tip link self._tip_link = ee_link # Read the complete link name, with robot's namespace, if existent for link in self._robot_description.links: if self._arm_name not in link.name: continue linkname = link.name.split('/')[-1] if self._base_link == linkname: self._base_link = link.name if self._tip_link == linkname: self._tip_link = link.name # Get arm chain self._chain = self._kdl_tree.getChain(self._base_link, self._tip_link) # Partial tree from base to each link self._chain_part = dict() for link in self._robot_description.links: if link.name in self.link_names: self._chain_part[link.name] = self._kdl_tree.getChain( self._base_link, link.name) # Get joint names from the chain # Joint names self._joint_names = list() for idx in xrange(self._chain.getNrOfSegments()): joint = self._chain.getSegment(idx).getJoint() # Not considering fixed joints if joint.getType() == 0: name = joint.getName() self._joint_names.append(name) self._joint_angles[name] = 0.0 self._joint_velocity[name] = 0.0 self._joint_effort[name] = 0.0 # Jacobian solvers self._jac_kdl = PyKDL.ChainJntToJacSolver(self._chain) # Jacobian solver dictionary for partial trees self._jac_kdl_part = dict() for link in self._robot_description.links: if link.name in self.link_names: self._jac_kdl_part[link.name] = PyKDL.ChainJntToJacSolver( self._chain_part[link.name]) # Forward position kinematics solvers self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._chain) # Forward velocity kinematics solvers self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._chain) # Inverse kinematic solvers self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._chain) self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._chain, self._fk_p_kdl, self._ik_v_kdl, 100, 1e-6)
def __init__(self): # choose between real robot and simulator self.real_robot = True # real robot #self.real_robot = False # sim robot self.wait_for_transform = 0.1 self.listener = tf.TransformListener() self.listener2 = tf.TransformListener( ) #creating tf.TransformListener object self.turn_condition = 10 self.turn_penalty = 0 self.double_pi = 2.0 * math.pi self.Table_flag = False self.joint_msg = JointState() # to store angles self.base_vel_msg = Twist() # to store base Twist message # subscribe to current joint angles rospy.Subscriber('joint_states', JointState, self.JointStateCallback, queue_size=1) self.goalpose_msg = PoseStamped() #to store goal pose self.goalpose_msg1 = PoseStamped() self.goalpose_msg_odom = PoseStamped() self.base_to_odom = None # subscribe to (goal pose) (Type: geometry_msgs/PoseStamped) rospy.Subscriber('~target_pose', PoseStamped, self.ObjectCallback, queue_size=1) rospy.Subscriber('/cmd_vel', Twist, self.CmdVelCallback, queue_size=1) # to trigger the node and start the controller rospy.Subscriber("~event_in", String, self.eventInCallback, queue_size=1) self.pub_event_out = rospy.Publisher('~event_out', String, queue_size=1) self.running = False # to publish joint velocities self.pub_vel0 = rospy.Publisher('~joint_cmd_vel0', Float64, queue_size=1) self.pub_vel1 = rospy.Publisher('~joint_cmd_vel1', Float64, queue_size=1) self.pub_vel2 = rospy.Publisher('~joint_cmd_vel2', Float64, queue_size=1) self.pub_vel3 = rospy.Publisher('~joint_cmd_vel3', Float64, queue_size=1) self.pub_vel4 = rospy.Publisher('~joint_cmd_vel4', Float64, queue_size=1) self.pub_vel5 = rospy.Publisher('~joint_cmd_vel5', Float64, queue_size=1) self.pub_vel6 = rospy.Publisher('~joint_cmd_vel6', Float64, queue_size=1) self.pub_base_vel_twist = rospy.Publisher('~cmd_vel_pub', Twist, queue_size=1) self.vel_joint0 = Float64() # the msg to publish velocities self.vel_joint1 = Float64() self.vel_joint2 = Float64() self.vel_joint3 = Float64() self.vel_joint4 = Float64() self.vel_joint5 = Float64() self.vel_joint6 = Float64() self.vel_base = Twist() self.vel_twist_stamped_jac = TwistStamped() self.vel_twist_stamped_lin = TwistStamped() self.vel_twist_stamped_now = TwistStamped() self.vel_twist_stamped_opt = TwistStamped() self.goalpose_received = False # flag to indicate that a goal pose msg was received self.joint_received = False # flag to indicate that a joint statese msg was received self.base_vel_received = False # flag to indicate that a base vel msg was received self.translation = None self.rotation = None self.phi = None self.current_phi_vel = None self.current_base_vel = [0, 0, 0] #x,y,theta self.vel = [0, 0, 0] #ee desired linear velocity self.ang_vel = [0, 0, 0] #roll,pitch,yaw, ee desired angular velocity self.x1 = [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ] #initial conditions for optimization - servos velocities phi' (7x1) + base = (10x1) self.robot = URDF.from_parameter_server('robot_description') self.tree = treeFromUrdfModel(self.robot)[1] #tupel error before #self.joint_names = self.getJointsNames() self.joint_names = ['left_arm_joint0', 'left_arm_joint1', 'left_arm_joint2',\ 'left_arm_joint3', 'left_arm_joint4', 'left_arm_joint5', 'left_arm_joint6'] self.chain = self.tree.getChain("base_link", "left_arm_virtual_endeffector") self.num_joints = self.chain.getNrOfJoints() self.jac_kdl = PyKDL.ChainJntToJacSolver(self.chain) self.b = ( -0.2, 0.2 ) #bounds for arm servos - veocities limits same for all 7 joints self.base = (-0.3, 0.3) #and 3 for base self.bnds = (self.b, self.b, self.b, self.b, self.b, self.b, self.b, self.base, self.base, self.b) self.con1 = {'type': 'eq', 'fun': self.constraint1} self.con2 = {'type': 'ineq', 'fun': self.constraint_min} self.con3 = {'type': 'ineq', 'fun': self.constraint_max} self.cons = [self.con2, self.con3] #self.con1, self.p_prim1 = None self.solex = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # flag to know when to start, based upon required tf msg self.ready_to_start = False # query weight params from param server self.base_x_weight = rospy.get_param('~base_x_weight', 4) self.base_y_weight = rospy.get_param('~base_y_weight', 4) self.base_theta_weight = rospy.get_param('~base_theta_weight', 8)
base_link = 'panda_link0' end_link = 'panda_link7' try: rospy.init_node('jacobian_planner') load_robot_description() time.sleep(2) print("Get urdf model from parameter server") model = URDF.from_parameter_server() #print model print("Loading kdl tree using urdf from parameter server") ret, kdltree = urdf.treeFromUrdfModel(model, quiet=False) # Get the whole kinematic chain from kdl tree chain = kdltree.getChain(base_link, end_link) # Initialize kdl solver for jacobian calculation solver = kdl.ChainJntToJacSolver(chain) jnt_array = kdl.JntArray(chain.getNrOfSegments()) # Get joints from the chain #for i in range(chain.getNrOfSegments()): # print chain.getSegment(i).getJoint() jac = kdl.Jacobian(chain.getNrOfSegments()) # print(dir(kdl))