def __init__(self, robotDescriptionString=None, arm_id='panda'): if robotDescriptionString is None: self.urdf_string = _rospy.get_param('robot_description') self.arm_id = _rospy.get_param('arm_id') else: self.urdf_string = robotDescriptionString self.arm_id = arm_id self.baseLinkName = "{}_link0".format(self.arm_id) self.eeLinkName = "{}_link7".format(self.arm_id) isSuccessful, self.kdltree = _kdl_parser.treeFromString( self.urdf_string) if not isSuccessful: raise RuntimeError("could not parse 'robot_description'") self.ee_chain = self.kdltree.getChain(self.baseLinkName, self.eeLinkName) self.fk_ee = _kdl.ChainFkSolverPos_recursive(self.ee_chain) self.jointposition = _kdl.JntArray(7) self.jointvelocity = _kdl.JntArray(7) self.eeFrame = _kdl.Frame() # Calculate the jacobian expressed in the base frame of the chain, with reference point at the end effector of the *chain. # http://docs.ros.org/hydro/api/orocos_kdl/html/classKDL_1_1ChainJntToJacSolver.html#details # Sounds like base jacobian but will be used here for both self.jac_ee = _kdl.ChainJntToJacSolver(self.ee_chain) self.jacobian = _kdl.Jacobian(7) #dynamics: (needs masses added to urdf!) self.grav_vector = _kdl.Vector(0., 0., -9.81) self.dynParam = _kdl.ChainDynParam(self.ee_chain, self.grav_vector) self.inertiaMatrix_kdl = _kdl.JntSpaceInertiaMatrix(7) self.inertiaMatrix = _np.eye((8)) self.gravity_torques_kdl = _kdl.JntArray(7) self.gravity_torques = _np.zeros((8)) viscuousFriction = [1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5] #TODO: load from URDF self.viscuousFriction = _np.array(viscuousFriction)
def inertia(self,joint_values=None): inertia = PyKDL.JntSpaceInertiaMatrix(self._num_jnts) self._dyn_kdl.JntToMass(self.joints_to_kdl('positions',joint_values), inertia) return kdl_to_mat(inertia)
def inertia(self): jnt_array = self.joints_to_kdl('positions') M_x_ee = PyKDL.JntSpaceInertiaMatrix(self.n_joints) self._dyn_kdl.JntToMass(jnt_array, M_x_ee) mat = self.kdl_to_mat(M_x_ee) return mat
def inertia(self): inertia = PyKDL.JntSpaceInertiaMatrix(self._num_jnts) self._dyn_kdl.JntToMass(self.joints_to_kdl('positions'), inertia) return self.kdl_to_mat(inertia)
def inertia(self, q): h_kdl = kdl.JntSpaceInertiaMatrix(self.num_joints) self._dyn_kdl.JntToMass(joint_list_to_kdl(q), h_kdl) return kdl_to_mat(h_kdl)
snake_pb = pb.loadURDF(path_to_urdf, useFixedBase=True, flags = pb.URDF_USE_INERTIA_FROM_FILE) pb.setGravity(0, 0, -9.81) #joint info jointlist, names, q_max, q_min = snake.get_joint_info(root, tip) n_joints = snake.get_n_joints(root, tip) #declarations #kdl q_kdl = kdl.JntArray(n_joints) gravity_kdl = kdl.Vector() gravity_kdl[2] = -9.81 g_kdl = kdl.JntArray(n_joints) M_kdl = kdl.JntSpaceInertiaMatrix(n_joints) #u2c & pybullet q = [None]*n_joints qdot = [None]*n_joints zeros_pb = [None]*n_joints M_sym = snake.get_inertia_matrix_crba(root, tip) #rbdl q_np = np.zeros(n_joints) M_rbdl = (n_joints, n_joints) M_rbdl = np.zeros(M_rbdl) #error declarations error_kdl_rbdl = np.zeros((n_joints, n_joints)) error_kdl_u2c = np.zeros((n_joints, n_joints))
def __init__(self, freq_control=100, margin_workspace=0.05): # Load robot urdf_model = URDF.from_parameter_server() fetch = kdl_tree_from_urdf_model(urdf_model) fetch_arm = fetch.getChain(BASE_LINK, END_LINK) self.dof = fetch_arm.getNrOfJoints() self.kdl_pos = PyKDL.ChainFkSolverPos_recursive(fetch_arm) self.kdl_jac = PyKDL.ChainJntToJacSolver(fetch_arm) self.kdl_dyn = PyKDL.ChainDynParam(fetch_arm, PyKDL.Vector(0, 0, -9.81)) self.kdl_q = PyKDL.JntArray(self.dof) self.kdl_A = PyKDL.JntSpaceInertiaMatrix(self.dof) self.kdl_J = PyKDL.Jacobian(self.dof) self.kdl_x = PyKDL.Frame() # self.kdl_G = PyKDL.JntArray(self.dof) # self.G = np.zeros((self.dof,)) # Initialize robot values self.lock = threading.Lock() self.thread_q = np.zeros((self.dof, )) self.thread_dq = np.zeros((self.dof, )) self.thread_tau = np.zeros((self.dof, )) self.q = np.zeros((self.dof, )) self.dq = np.zeros((self.dof, )) self.tau = np.zeros((self.dof, )) self.x = np.zeros((3, )) self.quat = np.array([0., 0., 0., 1.]) self.lim_norm_x = self.compute_workspace() - margin_workspace self.A = np.zeros((self.dof, self.dof)) self.A_inv = np.zeros((self.dof, self.dof)) self.J = np.zeros((6, self.dof)) self.q_init = np.array([ 0., -np.pi / 4., 0., np.pi / 4, 0., np.pi / 2, 0., ]) # Face down self.q_tuck = np.array([1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]) # Storage position self.q_stow = np.array([1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]) self.q_intermediate_stow = np.array( [0.7, -0.3, 0.0, -0.3, 0.0, -0.57, 0.0]) self.x_des = np.array([0.8, 0., 0.35]) # q_init self.x_init = np.array([0.8, 0., 0.35]) # q_init # self.quat_des = np.array([-0.707, 0., 0.707, 0.]) # Face up self.quat_des = np.array([0., 0.707, 0., 0.707]) # Face down self.state = "INITIALIZE" print("Switching to state: " + self.state) self.t_start = time.time() self.freq_control = freq_control # Initialize pub and sub self.pub = rospy.Publisher("arm_controller/joint_torque/command", Float64MultiArray, queue_size=1) sub = rospy.Subscriber( "joint_states", JointState, lambda joint_states: self.read_joint_sensors(joint_states)) # Initialize ROS rospy.init_node("joint_space_controller")