def __init__(self, robot, ee_link=None): rospack = rospkg.RosPack() pykdl_dir = rospack.get_path('ur_pykdl') TREE_PATH = pykdl_dir + '/urdf/' + robot + '.urdf' self._ur = URDF.from_xml_file(TREE_PATH) self._kdl_tree = kdl_tree_from_urdf_model(self._ur) self._base_link = BASE_LINK ee_link = EE_LINK if ee_link is None else ee_link self._tip_link = ee_link self._tip_frame = PyKDL.Frame() self._arm_chain = self._kdl_tree.getChain(self._base_link, self._tip_link) # UR Interface Limb Instances self._joint_names = JOINT_ORDER 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 gravity_compensation_publisher(data): try: old except NameError: old = 0 if old <> data.position[1]: print 123 pub = rospy.Publisher('/panda_arm_controller/command', Float64MultiArray, queue_size=10) data_to_send = Float64MultiArray() robot = URDF.from_parameter_server() tree = kdl_tree_from_urdf_model(robot) chain = tree.getChain("panda_link0", "panda_link7") grav_vector = kdl.Vector(0, 0, -9.81) # relative to kdl chain base link dyn_kdl = kdl.ChainDynParam(chain, grav_vector) jt_positions = kdl.JntArray(7) jt_positions[0] = data.position[2] jt_positions[1] = data.position[3] jt_positions[2] = data.position[4] jt_positions[3] = data.position[5] jt_positions[4] = data.position[6] jt_positions[5] = data.position[7] jt_positions[6] = data.position[8] old = data.position[1] grav_matrix = kdl.JntArray(7) dyn_kdl.JntToGravity(jt_positions, grav_matrix) data_to_send.data = [grav_matrix[i] for i in range(grav_matrix.rows())] pub.publish(data_to_send)
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, 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())
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())
def __init__(self, base_link=None, end_link=None): self._robot = kdl_parser_py.urdf.urdf.URDF.from_parameter_server( 'robot_description') (ok, self._kdl_tree) = kdl_parser_py.urdf.treeFromUrdfModel(self._robot) self._base_link = self._robot.get_root( ) if base_link is None else base_link self._tip_link = end_link self._tip_frame = PyKDL.Frame() self._arm_chain = self._kdl_tree.getChain(self._base_link, self._tip_link) #self.joint_names = [joint.name for joint in self._robot.joints if joint.type!='fixed'] self.joint_names = [ self._arm_chain.getSegment(i).getJoint().getName() for i in range(self._arm_chain.getNrOfSegments()) if self._arm_chain.getSegment(i).getJoint().getType() != PyKDL.Joint.None ] self._num_jnts = len(self.joint_names) # Store joint information for future use self.get_joint_information() # 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, 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, 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))
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, end_link, kdl_tree=None): if kdl_tree is None: kdl_tree = kdl_tree_from_urdf_model(urdf) self.tree = kdl_tree self.urdf = urdf base_link = base_link.split("/")[-1] # for dealing with tf convention end_link = end_link.split("/")[-1] # for dealing with tf convention self.chain = kdl_tree.getChain(base_link, end_link) self.base_link = base_link self.end_link = end_link # record joint information in easy-to-use lists self.joint_limits_lower = [] self.joint_limits_upper = [] self.joint_safety_lower = [] self.joint_safety_upper = [] self.joint_types = [] for jnt_name in self.get_joint_names(): jnt = urdf.joints[jnt_name] if jnt.limits is not None: self.joint_limits_lower.append(jnt.limits.lower) self.joint_limits_upper.append(jnt.limits.upper) else: self.joint_limits_lower.append(None) self.joint_limits_upper.append(None) if jnt.safety is not None: self.joint_safety_lower.append(jnt.safety.lower) self.joint_safety_upper.append(jnt.safety.upper) elif jnt.limits is not None: self.joint_safety_lower.append(jnt.limits.lower) self.joint_safety_upper.append(jnt.limits.upper) else: self.joint_safety_lower.append(None) self.joint_safety_upper.append(None) self.joint_types.append(jnt.joint_type) def replace_none(x, v): if x is None: return v return x self.joint_limits_lower = np.array( [replace_none(jl, -np.inf) for jl in self.joint_limits_lower]) self.joint_limits_upper = np.array( [replace_none(jl, np.inf) for jl in self.joint_limits_upper]) self.joint_safety_lower = np.array( [replace_none(jl, -np.inf) for jl in self.joint_safety_lower]) self.joint_safety_upper = np.array( [replace_none(jl, np.inf) for jl in self.joint_safety_upper]) self.joint_types = np.array(self.joint_types) self.num_joints = len(self.get_joint_names()) self._fk_kdl = kdl.ChainFkSolverPos_recursive(self.chain) self._ik_v_kdl = kdl.ChainIkSolverVel_pinv(self.chain) self._ik_p_kdl = kdl.ChainIkSolverPos_NR(self.chain, self._fk_kdl, self._ik_v_kdl) self._jac_kdl = kdl.ChainJntToJacSolver(self.chain) self._dyn_kdl = kdl.ChainDynParam(self.chain, kdl.Vector.Zero())
def __init__(self): ns = [ item for item in rospy.get_namespace().split('/') if len(item) > 0 ] if len(ns) != 2: rospy.ROSException( 'The controller must be called in the manipulator namespace') self._namespace = ns[0] self._arm_name = ns[1] if self._namespace[0] != '/': self._namespace = '/' + self._namespace if self._namespace[-1] != '/': self._namespace += '/' # The arm interface loads the parameters from the URDF file and # parameter server and initializes the KDL tree self._arm_interface = ArmInterface() self._base_link = self._arm_interface.base_link self._tip_link = self._arm_interface.tip_link self._tip_frame = PyKDL.Frame() # KDL Solvers self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_interface.chain) self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR( self._arm_interface.chain, self._arm_interface._fk_p_kdl, self._ik_v_kdl, 100, 1e-6) self._dyn_kdl = PyKDL.ChainDynParam(self._arm_interface.chain, PyKDL.Vector.Zero()) # Add a callback function to calculate the end effector's state by each # update of the joint state self._arm_interface.add_callback('joint_states', self.on_update_endeffector_state) # Publish the current manipulability index at each update of the joint # states # self._arm_interface.add_callback('joint_states', # self.publish_man_index) # Publish the end effector state self._endeffector_state_pub = rospy.Publisher('endpoint_state', EndPointState, queue_size=1) # Publish the manipulability index self._man_index_pub = rospy.Publisher('man_index', Float64, queue_size=1) self._services = dict() # Provide the service to calculate the inverse kinematics using the KDL solver self._services['ik'] = rospy.Service('ik_solver', SolveIK, self.solve_ik)
def initialize_kinematic_solvers(self): robot_description = rospy.get_param('/yumi/velocity_control/robot_description', '/robot_description') self._robot = URDF.from_parameter_server(key=robot_description) self._kdl_tree = kdl_tree_from_urdf_model(self._robot) # self._base_link = self._robot.get_root() self._ee_link = 'gripper_' +self._arm_name + '_base' #name of the frame we want self._ee_frame = PyKDL.Frame() self._ee_arm_chain = self._kdl_tree.getChain(self._base_link, self._ee_link) # KDL Solvers self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._ee_arm_chain) self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._ee_arm_chain) self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._ee_arm_chain) #self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain, self._fk_p_kdl, self._ik_v_kdl) self._ik_p_kdl = PyKDL.ChainIkSolverPos_LMA(self._ee_arm_chain) self._jac_kdl = PyKDL.ChainJntToJacSolver(self._ee_arm_chain) self._dyn_kdl = PyKDL.ChainDynParam(self._ee_arm_chain, PyKDL.Vector.Zero())
def __init__(self, limb, ee_frame_name="panda_link8", additional_segment_config=None, 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) if additional_segment_config is not None: 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_link = ee_frame_name 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 create_chain(self, ee_frame_name): self._tip_link = ee_frame_name self._arm_chain = self._kdl_tree.getChain(self._base_link, self._tip_link) 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, 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 kdl2np(asd): x = [] for i in range(n_joints): x.append(asd[i]) return np.asarray(x) n_itr = 1000 for i in range(n_itr): for j in range(n_joints): q[j] = (q_max[j] - q_min[j]) * np.random.rand() - (q_max[j] - q_min[j]) / 2 qdot[j] = (q_max[j] - q_min[j]) * np.random.rand() - (q_max[j] - q_min[j]) / 2 q_kdl[j] = q[j] qdot_kdl[j] = qdot[j] kdl.ChainDynParam(kuka_chain, grav).JntToCoriolis(q_kdl, qdot_kdl, C_kdl) C_u2c = C_sym(q, qdot) for tau_idx in range(n_joints): error[tau_idx] += np.absolute( (kdl2np(C_kdl)[tau_idx] - u2c2np(C_u2c)[tau_idx])) print "Errors in coriolis forces with", n_itr, "iterations and comparing against KDL:\n", error sum_error = 0 for err in range(n_joints): sum_error += error[err] print "Sum of errors:\n", sum_error
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")
def list2np(asd): return np.asarray(asd) n_itr = 1000 for i in range(n_itr): for j in range(n_joints): q[j] = (q_max[j] - q_min[j]) * np.random.rand() - (q_max[j] - q_min[j]) / 2 q_kdl[j] = q[j] q_np[j] = q[j] rbdl.CompositeRigidBodyAlgorithm(gantry_rbdl, q_np, M_rbdl) kdl.ChainDynParam(gantry_chain, gravity_kdl).JntToMass(q_kdl, M_kdl) M_pb = pb.calculateMassMatrix(gantry_pb, q) M_u2c = M_sym(q) for row_idx in range(n_joints): for col_idx in range(n_joints): error_kdl_u2c[row_idx][col_idx] += np.absolute( M_kdl[row_idx, col_idx] - u2c2np(M_u2c[row_idx, col_idx])) error_rbdl_u2c[row_idx][col_idx] += np.absolute( (M_rbdl[row_idx, col_idx]) - u2c2np(M_u2c[row_idx, col_idx])) error_pb_u2c[row_idx][col_idx] += np.absolute( list2np(M_pb[row_idx][col_idx]) - u2c2np(M_u2c[row_idx, col_idx])) error_kdl_rbdl[row_idx][col_idx] += np.absolute( (M_rbdl[row_idx, col_idx]) - list2np(M_kdl[row_idx, col_idx])) error_pb_kdl[row_idx][col_idx] += np.absolute(
def u2c2np(asd): return cs.Function("temp", [], [asd])()["o0"].toarray() def kdl2np(asd): return np.asarray(asd) n_itr = 1000 for i in range(n_itr): for j in range(n_joints): q[j] = (q_max[j] - q_min[j]) * np.random.rand() - (q_max[j] - q_min[j]) / 2 q_kdl[j] = q[j] kdl.ChainDynParam(kuka_chain, grav).JntToMass(q_kdl, M_kdl) M_u2c = M_sym(q) for row_idx in range(n_joints): for col_idx in range(n_joints): error[row_idx][col_idx] += np.absolute( (kdl2np(M_kdl[row_idx, col_idx])) - u2c2np(M_u2c[row_idx, col_idx])) print "Errors in inertia matrix with", n_itr, "iterations and comparing against KDL:\n", error sum_error = 0 for row in range(n_joints): for col in range(n_joints): sum_error += error[row][col] print "Sum of errors:\n", sum_error
n_itr = 1000 for i in range(n_itr): for j in range(n_joints): q[j] = (q_max[j] - q_min[j])*np.random.rand()-(q_max[j] - q_min[j])/2 q_kdl[j] = q[j] q_np[j] = q[j] qdot[j] = (q_max[j] - q_min[j])*np.random.rand()-(q_max[j] - q_min[j])/2 qdot_kdl[j] = qdot[j] qdot_np[j] = qdot[j] zeros_pb[j] = 0. rbdl.NonlinearEffects(gantry_rbdl, q_np, qdot_np, C_rbdl) kdl.ChainDynParam(gantry_kdl, gravity_kdl).JntToGravity(q_kdl, g_kdl) kdl.ChainDynParam(gantry_kdl, gravity_kdl).JntToCoriolis(q_kdl, qdot_kdl, C_kdl) pb.setGravity(0, 0, 0) C_pb = pb.calculateInverseDynamics(gantry_pb, q, qdot, zeros_pb) pb.setGravity(0, 0, -9.81) g_pb = pb.calculateInverseDynamics(gantry_pb, q, zeros_pb, zeros_pb) g_u2c = g_sym(q) C_u2c = C_sym(q, qdot) for tau_idx in range(n_joints): error_kdl_rbdl[tau_idx] += np.absolute(((list2np(g_kdl[tau_idx]) + list2np(C_kdl[tau_idx])) - C_rbdl[tau_idx])) error_kdl_u2c[tau_idx] += np.absolute((list2np(C_kdl[tau_idx]) - u2c2np(C_u2c[tau_idx]))) error_rbdl_u2c[tau_idx] += np.absolute(((u2c2np(g_u2c[tau_idx]) + u2c2np(C_u2c)[tau_idx]) - C_rbdl[tau_idx])) error_pb_u2c[tau_idx] += np.absolute((u2c2np(C_u2c[tau_idx]) - list2np(C_pb[tau_idx])))
for i in range(n_itr): for j in range(n_joints): q[j] = (q_max[j] - q_min[j]) * np.random.rand() - (q_max[j] - q_min[j]) / 2 q_kdl[j] = q[j] q_np[j] = q[j] qdot[j] = (q_max[j] - q_min[j]) * np.random.rand() - (q_max[j] - q_min[j]) / 2 qdot_kdl[j] = qdot[j] qdot_np[j] = qdot[j] zeros_pb[j] = 0. rbdl.NonlinearEffects(snake_rbdl, q_np, qdot_np, C_rbdl) kdl.ChainDynParam(snake_chain, gravity_kdl).JntToGravity(q_kdl, g_kdl) kdl.ChainDynParam(snake_chain, gravity_kdl).JntToCoriolis(q_kdl, qdot_kdl, C_kdl) pb.setGravity(0, 0, 0) C_pb = pb.calculateInverseDynamics(snake_pb, q, qdot, zeros_pb) pb.setGravity(0, 0, -9.81) g_pb = pb.calculateInverseDynamics(snake_pb, q, zeros_pb, zeros_pb) g_u2c = g_sym(q) C_u2c = C_sym(q, qdot) for tau_idx in range(n_joints): error_kdl_rbdl[tau_idx] += np.absolute( ((list2np(g_kdl[tau_idx]) + list2np(C_kdl[tau_idx])) - C_rbdl[tau_idx])) error_kdl_u2c[tau_idx] += np.absolute(
def list2np(asd): return np.asarray(asd) n_itr = 1000 for i in range(n_itr): for j in range(n_joints): q[j] = (q_max[j] - q_min[j]) * np.random.rand() - (q_max[j] - q_min[j]) / 2 q_kdl[j] = q[j] q_np[j] = q[j] zeros_pb[j] = 0. rbdl.InverseDynamics(snake_rbdl, q_np, zeros_rbdl, zeros_rbdl, g_rbdl) kdl.ChainDynParam(snake_chain, gravity_kdl).JntToGravity(q_kdl, g_kdl) g_pb = pb.calculateInverseDynamics(snake_pb, q, zeros_pb, zeros_pb) g_u2c = g_sym(q) for tau_idx in range(n_joints): error_kdl_rbdl[tau_idx] += np.absolute( (list2np(g_kdl[tau_idx]) - g_rbdl[tau_idx])) error_kdl_u2c[tau_idx] += np.absolute( (list2np(g_kdl[tau_idx]) - u2c2np(g_u2c[tau_idx]))) error_rbdl_u2c[tau_idx] += np.absolute( (u2c2np(g_u2c[tau_idx]) - g_rbdl[tau_idx])) error_pb_u2c[tau_idx] += np.absolute( (u2c2np(g_u2c[tau_idx]) - list2np(g_pb[tau_idx]))) error_pb_kdl[tau_idx] += np.absolute( (list2np(g_kdl[tau_idx]) - list2np(g_pb[tau_idx]))) error_pb_rbdl[tau_idx] += np.absolute(g_rbdl[tau_idx] -
def u2c2np(asd): return cs.Function("temp",[],[asd])()["o0"].toarray() def list2np(asd): return np.asarray(asd) n_itr = 1000 for i in range(n_itr): for j in range(n_joints): q[j] = (q_max[j] - q_min[j])*np.random.rand()-(q_max[j] - q_min[j])/2 q_kdl[j] = q[j] q_np[j] = q[j] rbdl.CompositeRigidBodyAlgorithm(snake_rbdl, q_np, M_rbdl) kdl.ChainDynParam(snake_chain, gravity_kdl).JntToMass(q_kdl, M_kdl) M_pb = pb.calculateMassMatrix(snake_pb, q) M_u2c = M_sym(q) for row_idx in range(n_joints): for col_idx in range(n_joints): error_kdl_u2c[row_idx][col_idx] += np.absolute(M_kdl[row_idx,col_idx] - u2c2np(M_u2c[row_idx, col_idx])) error_rbdl_u2c[row_idx][col_idx] += np.absolute((M_rbdl[row_idx,col_idx]) - u2c2np(M_u2c[row_idx, col_idx])) error_pb_u2c[row_idx][col_idx] += np.absolute(list2np(M_pb[row_idx][col_idx]) - u2c2np(M_u2c[row_idx, col_idx])) error_kdl_rbdl[row_idx][col_idx] += np.absolute((M_rbdl[row_idx,col_idx]) - list2np(M_kdl[row_idx, col_idx])) error_pb_kdl[row_idx][col_idx] += np.absolute(list2np(M_pb[row_idx][col_idx]) - list2np(M_kdl[row_idx, col_idx])) error_pb_rbdl[row_idx][col_idx] += np.absolute((M_rbdl[row_idx,col_idx]) - list2np(M_pb[row_idx][col_idx])) sum_error_kdl_rbdl = 0
def kdl2np(asd): x = [] for i in range(n_joints): x.append(asd[i]) return np.asarray(x) n_itr = 1000 for i in range(n_itr): for j in range(n_joints): q[j] = (q_max[j] - q_min[j]) * np.random.rand() - (q_max[j] - q_min[j]) / 2 q_kdl[j] = q[j] kdl.ChainDynParam(kuka_chain, gravity_kdl).JntToGravity(q_kdl, G_kdl) G_u2c = G_sym(q) #print G_u2c #print G_kdl for tau_idx in range(n_joints): error[tau_idx] += np.absolute( (kdl2np(G_kdl)[tau_idx] - u2c2np(G_u2c)[tau_idx])) print "Errors in gravity forces with", n_itr, "iterations and comparing against KDL:\n", error sum_error = 0 for err in range(n_joints): sum_error += error[err] print "Sum of errors:\n", sum_error
def main(): rospy.init_node('main_test', anonymous=True) rospy.Subscriber("/iiwa/joint_states", JointState, iiwaStateCallback) # Initialize publishers for send computation torques. for i in range(0,7): torque_controller_pub.append(rospy.Publisher("/iiwa/joint" + str(i+1) + "_torque_controller/command", Float64, queue_size=10)) rate = rospy.Rate(int(1.0/dt)) # Get parameters for kinematic from setup.yaml file base_link = rospy.get_param("/master/base_link_name") tool_link = rospy.get_param("/master/tool_link_name") urdf = rospy.get_param("iiwa_urdf_model") rospy.loginfo("tip_name: %s" % tool_link) rospy.loginfo("root_name: %s" % base_link) data = json.load(open("/workspace/base_ws/src/profi2021_metrics_graber/data/master_tests/track_1_traj.json", "r")) # Generate kinematic model for orocos_kdl (ok, tree) = kdl_parser_py.urdf.treeFromString(urdf) chain = tree.getChain(base_link, tool_link) L = np.transpose(np.array([[1, 1, 1, 0.01, 0.01, 0.01]])) iksolverpos = kdl.ChainIkSolverPos_LMA(chain, L) # Generate dynamic model for orocos_kdl grav = kdl.Vector(0, 0, -9.82) dyn_model = kdl.ChainDynParam(chain, grav) startControllers() torq_msg = Float64() counter = 0 u = kdl.JntArray(7) q_dest = kdl.JntArray(7) frame_dest = kdl.Frame() # Setting up initial point frame_dest.p[0] = data[0]["x"] frame_dest.p[1] = data[0]["y"] frame_dest.p[2] = 0.3 # frame_dest.p[0] = 0.3 # frame_dest.p[1] = 0.0 # frame_dest.p[2] = 0.05 frame_dest.M = kdl.Rotation.RotY(3.14) ret = iksolverpos.CartToJnt(q, frame_dest, q_dest) while not rospy.is_shutdown(): frame_dest.p[0] = data[int(counter/10)]["x"] frame_dest.p[1] = data[int(counter/10)]["y"]; # Start controllers if counter>89*10: counter=0 ret = iksolverpos.CartToJnt(q, frame_dest, q_dest) rospy.loginfo("IK solution: %f, %f, %f, %f, %f, %f, %f", q_dest[0], q_dest[1], q_dest[2], q_dest[3], q_dest[4], q_dest[5], q_dest[6]) dyn_model.JntToGravity(q, grav_torques) rospy.loginfo("Estimation torque on joins: %f, %f, %f, %f, %f, %f, %f\n" % (grav_torques[0], grav_torques[1],grav_torques[2], grav_torques[3], grav_torques[4], grav_torques[5], grav_torques[6])) for i in range(0,7): u[i] = KP[i]*(q_dest[i]-q[i]) - KD[i]*dq[i] + grav_torques[i] torq_msg.data = u[i] torque_controller_pub[i].publish(torq_msg) counter+=1 rate.sleep()