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")
