def _add_children_to_tree(robot_model, root, tree): # constructs the optional inertia inert = kdl.RigidBodyInertia(0) if root.inertial: inert = _toKdlInertia(root.inertial) # constructs the kdl joint (parent_joint_name, parent_link_name) = robot_model.parent_map[root.name] parent_joint = robot_model.joint_map[parent_joint_name] # construct the kdl segment sgm = kdl.Segment(root.name, _toKdlJoint(parent_joint), _toKdlPose(parent_joint.origin), inert) # add segment to tree if not tree.addSegment(sgm, parent_link_name): return False if root.name not in robot_model.child_map: return True children = [ robot_model.link_map[l] for (j, l) in robot_model.child_map[root.name] ] # recurslively add all children for child in children: if not _add_children_to_tree(robot_model, child, tree): return False return True
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)
def add_children_to_tree(parent, segment_id): if parent in urdf.child_map: for joint, child_name in urdf.child_map[parent]: if joint in js_inactive_names_vector: print "setting as fixed:", joint, js_pos[joint] joint_rot = -js_pos[joint] urdf.joint_map[joint].joint_type = 'fixed' else: joint_rot = 0.0 child = urdf.link_map[child_name] if child.inertial is not None: kdl_inert = kdl_urdf.urdf_inertial_to_kdl_rbi( child.inertial) else: kdl_inert = PyKDL.RigidBodyInertia() kdl_jnt = kdl_urdf.urdf_joint_to_kdl_joint( urdf.joint_map[joint]) kdl_origin = kdl_urdf.urdf_pose_to_kdl_frame( urdf.joint_map[joint].origin) * PyKDL.Frame( PyKDL.Rotation.RotZ(joint_rot)) kdl_sgm = PyKDL.Segment(child_name, kdl_jnt, kdl_origin, kdl_inert) segment_map[segment_id] = kdl_sgm segment_parent_map[segment_id] = segment_name_id_map[ parent] segment_name_id_map[child_name] = segment_id segment_id += 1 tree.addSegment(kdl_sgm, parent) segment_id = add_children_to_tree(child_name, segment_id) return segment_id
def urdf_inertial_to_kdl_rbi(i): origin = urdf_pose_to_kdl_frame(i.origin) rbi = kdl.RigidBodyInertia( i.mass, origin.p, kdl.RotationalInertia(i.inertia.ixx, i.inertia.iyy, i.inertia.izz, i.inertia.ixy, i.inertia.ixz, i.inertia.iyz)) return origin.M * rbi
def add_children_to_tree(parent): if parent in urdf.child_map: if len(urdf.child_map[parent]) > 1: ef_tree[parent] = [ child_name for _, child_name in urdf.child_map[parent] ] for joint, child_name in urdf.child_map[parent]: for lidx, link in enumerate(urdf.links): if child_name == link.name: child = urdf.links[lidx] if child.inertial is not None: kdl_inert = urdf_inertial_to_kdl_rbi( child.inertial) else: kdl_inert = kdl.RigidBodyInertia() for jidx, jnt in enumerate(urdf.joints): if jnt.name == joint: kdl_jnt = urdf_joint_to_kdl_joint( urdf.joints[jidx]) kdl_origin = urdf_pose_to_kdl_frame( urdf.joints[jidx].origin) kdl_sgm = kdl.Segment(child_name, kdl_jnt, kdl_origin, kdl_inert) tree.addSegment(kdl_sgm, parent) add_children_to_tree(child_name)
def _toKdlInertia(i): # kdl specifies the inertia in the reference frame of the link, the urdf # specifies the inertia in the inertia reference frame origin = _toKdlPose(i.origin) inertia = i.inertia return origin.M * PyKDL.RigidBodyInertia( i.mass, origin.p, PyKDL.RotationalInertia(inertia.ixx, inertia.iyy, inertia.izz, inertia.ixy, inertia.ixz, inertia.iyz))
def urdf_inertial_to_kdl_rbi(i): origin = urdf_pose_to_kdl_frame(i.origin) rbi = kdl.RigidBodyInertia(i.mass, origin.p, kdl.RotationalInertia(i.matrix['ixx'], i.matrix['iyy'], i.matrix['izz'], i.matrix['ixy'], i.matrix['ixz'], i.matrix['iyz'])) return origin.M * rbi
def add_children_to_tree(parent): if parent in urdf.child_map: for joint, child_name in urdf.child_map[parent]: child = urdf.link_map[child_name] if child.inertial is not None: kdl_inert = urdf_inertial_to_kdl_rbi(child.inertial) else: kdl_inert = kdl.RigidBodyInertia() kdl_jnt = urdf_joint_to_kdl_joint(urdf.joint_map[joint]) kdl_origin = urdf_pose_to_kdl_frame(urdf.joint_map[joint].origin) kdl_sgm = kdl.Segment(child_name, kdl_jnt, kdl_origin, kdl_inert) tree.addSegment(kdl_sgm, parent) add_children_to_tree(child_name)
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 add_children_to_tree(root, tree): children = root.child_links inert = kdl.RigidBodyInertia() if root.inertial is not None: inert = to_kdl_rbi(root.inertial) jnt = to_kdl_joint(root.parent_joint) sgm = kdl.Segment( root.name, jnt, to_kdl_frame(root.parent_joint.parent_to_joint_origin_transform), inert) tree.addSegment(sgm, root.parent_joint.parent_link_name) for child in children: add_children_to_tree(child, tree)
def addChildrenToTree(robotModel, root, tree): """ Helper function that adds children to a KDL tree. """ # constructs the optional inertia inert = kdl.RigidBodyInertia(0) if root.inertial: inert = toKdlInertia(root.inertial) # constructs the kdl joint parentJointName, parentLinkName = robotModel.parent_map[root.name] parentJoint = robotModel.joint_map[parentJointName] # construct the kdl segment sgm = kdl.Segment( root.name, toKdlJoint(parentJoint), toKdlPose(parentJoint.origin), inert) # add segment to tree if not tree.addSegment(sgm, parentLinkName): return False if root.name not in robotModel.child_map: return True children = [robotModel.link_map[l] for (j, l) in robotModel.child_map[root.name]] # recurslively add all children for child in children: if not addChildrenToTree(robotModel, child, tree): return False return True
def to_kdl_rbi(i): origin = to_kdl_frame(i.origin) rbi = kdl.RigidBodyInertia( i.mass, origin.p, kdl.RotationalInertia(i.ixx, i.iyy, i.ixy, i.ixz, i.iyz)) return rbi.rot_mult(origin.M)
import PyKDL #********创建段*******# #坐标 frame1 = PyKDL.Frame(PyKDL.Rotation.RPY(0, 1, 0), PyKDL.Vector(3, 2, 4)) print "frame1:\n", frame1 #关节 joint1 = PyKDL.Joint(PyKDL.Joint.RotZ) print "joint:", joint1 #惯性张量 m = 0.5 oc = PyKDL.Vector(1, 0, 1) Ic = PyKDL.RotationalInertia(1.0, 2.0, 3.0, 4.0, 5.0, 6.0) RBI = PyKDL.RigidBodyInertia(m, oc, Ic) m = RBI.getMass() print "m:\n", m oc = RBI.getCOG() print "oc:\n", oc #创建段 segment1 = PyKDL.Segment(joint1, frame1, RBI) print "segment1:\n", segment1 #********创建链*******# links = [] for i in range(6): links.append(segment1)
def __init__(self, T=20, weight=[1.0, 1.0], verbose=True): #initialize model self.chain = PyKDL.Chain() self.chain.addSegment( PyKDL.Segment( "Base", PyKDL.Joint(PyKDL.Joint.None), PyKDL.Frame(PyKDL.Vector(0.0, 0.0, 0.042)), PyKDL.RigidBodyInertia( 0.08659, PyKDL.Vector(0.00025, 0.0, -0.02420), PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0)))) self.chain.addSegment( PyKDL.Segment( "Joint1", PyKDL.Joint(PyKDL.Joint.RotZ), PyKDL.Frame(PyKDL.Vector(0.0, -0.019, 0.028)), PyKDL.RigidBodyInertia( 0.00795, PyKDL.Vector(0.0, 0.019, -0.02025), PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0)))) self.chain.addSegment( PyKDL.Segment( "Joint2", PyKDL.Joint(PyKDL.Joint.RotY), PyKDL.Frame(PyKDL.Vector(0.0, 0.019, 0.0405)), PyKDL.RigidBodyInertia( 0.09312, PyKDL.Vector(0.00000, -0.00057, -0.02731), PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0)))) self.chain.addSegment( PyKDL.Segment( "Joint3", PyKDL.Joint(PyKDL.Joint.RotZ), PyKDL.Frame(PyKDL.Vector(0.024, -0.019, 0.064)), PyKDL.RigidBodyInertia( 0.19398, PyKDL.Vector(-0.02376, 0.01864, -0.02267), PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0)))) self.chain.addSegment( PyKDL.Segment( "Joint4", PyKDL.Joint("minus_RotY", PyKDL.Vector(0, 0, 0), PyKDL.Vector(0, -1, 0), PyKDL.Joint.RotAxis), PyKDL.Frame(PyKDL.Vector(0.064, 0.019, 0.024)), PyKDL.RigidBodyInertia( 0.09824, PyKDL.Vector(-0.02099, 0.0, -0.01213), PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0)))) self.chain.addSegment( PyKDL.Segment( "Joint5", PyKDL.Joint(PyKDL.Joint.RotX), PyKDL.Frame(PyKDL.Vector(0.0405, -0.019, 0.0)), PyKDL.RigidBodyInertia( 0.09312, PyKDL.Vector(-0.01319, 0.01843, 0.0), PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0)))) self.chain.addSegment( PyKDL.Segment( "Joint6", PyKDL.Joint("minus_RotY", PyKDL.Vector(0, 0, 0), PyKDL.Vector(0, -1, 0), PyKDL.Joint.RotAxis), PyKDL.Frame(PyKDL.Vector(0.064, 0.019, 0.0)), PyKDL.RigidBodyInertia( 0.09824, PyKDL.Vector(-0.02099, 0.0, 0.01142), PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0)))) self.chain.addSegment( PyKDL.Segment( "Joint7", PyKDL.Joint(PyKDL.Joint.RotX), PyKDL.Frame(PyKDL.Vector(0.14103, 0.0, 0.0)), PyKDL.RigidBodyInertia( 0.26121, PyKDL.Vector(-0.09906, 0.00146, -0.00021), PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0)))) self.min_position_limit = [ -160.0, -90.0, -160.0, -90.0, -160.0, -90.0, -160.0 ] self.max_position_limit = [ 160.0, 90.0, 160.0, 90.0, 160.0, 90.0, 160.0 ] self.min_joint_position_limit = PyKDL.JntArray(7) self.max_joint_position_limit = PyKDL.JntArray(7) for i in range(0, 7): self.min_joint_position_limit[ i] = self.min_position_limit[i] * pi / 180 self.max_joint_position_limit[ i] = self.max_position_limit[i] * pi / 180 self.fksolver = PyKDL.ChainFkSolverPos_recursive(self.chain) self.iksolver = PyKDL.ChainIkSolverVel_pinv(self.chain) self.iksolverpos = PyKDL.ChainIkSolverPos_NR_JL( self.chain, self.min_joint_position_limit, self.max_joint_position_limit, self.fksolver, self.iksolver, 100, 1e-6) #parameter self.T = T self.weight_u = weight[0] self.weight_x = weight[1] self.verbose = verbose self.nj = self.chain.getNrOfJoints() self.q_init = np.zeros(self.nj) self.q_out = np.zeros(self.nj) ret, self.dest, self.q_out = self.generate_dest() self.fin_position = self.dest.p return