def callback(data): rospy.loginfo(rospy.get_caller_id() + 'I heard position %f from %s', data.position[0], data.name[0]) chain = kdl.Chain() rot = kdl.Rotation(m.cos(th3), m.sin(th3), 0, -1*m.sin(th3)*m.cos(al3), m.cos(th3)*m.cos(al3), m.sin(al3), m.sin(th3)*m.sin(al3), -1*m.cos(th3)*m.sin(al3), m.cos(al3)) chain.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.TransZ), kdl.Frame(rot, kdl.Vector(a3,0.0,d3)))) rot = kdl.Rotation(m.cos(th2), m.sin(th2), 0, -1*m.sin(th2)*m.cos(al2), m.cos(th2)*m.cos(al2), m.sin(al2), m.sin(th2)*m.sin(al2), -1*m.cos(th2)*m.sin(al2), m.cos(al2)) chain.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.TransZ), kdl.Frame(rot, kdl.Vector(a2,0.0,d2)))) rot = kdl.Rotation(m.cos(th1), m.sin(th1), 0, -1*m.sin(th1)*m.cos(al1), m.cos(th1)*m.cos(al1), m.sin(al1), m.sin(th1)*m.sin(al1), -1*m.cos(th1)*m.sin(al1), m.cos(al1)) chain.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.TransZ), kdl.Frame(rot, kdl.Vector(a1,0.0,d1)))) rot = kdl.Rotation(1,0,0, 0,1,0, 0,0,1) chain.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.None), kdl.Frame(rot, kdl.Vector(0.0,0.0,0.0)))) fksolver = kdl.ChainFkSolverPos_recursive(chain) jpos = kdl.JntArray(chain.getNrOfJoints()) jpos[2] = data.position[0] jpos[1] = data.position[1] jpos[0] = data.position[2] cartpos = kdl.Frame() fksolver.JntToCart(jpos,cartpos) pub_msg = PoseStamped() pub_msg.header.frame_id = "base_link" pub_msg.pose.position.x = cartpos.p[2] pub_msg.pose.position.y =-cartpos.p[1] pub_msg.pose.position.z = cartpos.p[0] pub.publish(pub_msg)
def __init__(self): self.leg = kdl.Chain() self.leg.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotY), kdl.Frame(kdl.Vector(0,0,-50)))) self.leg.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotY), kdl.Frame(kdl.Vector(0,0,-50)))) self.ik_solver = kdl.ChainIkSolverPos_LMA(self.leg) self.alpha1=45 self.alpha2=-45 self.beta=self.alpha2-self.alpha1
def simple_kinematic(data): ''' realizacja kinematyki prostej z uzyciem biblioteki KDL wraz z nadaniem obliczonych wartosci ''' if not find_position(data): rospy.logerr('Wrong position: ' + str(data)) #spr czy nie bledna pozycja return kdl_chain = kdl.Chain() kdl_frame = kdl.Frame() frame0 = kdl_frame.DH(0, 0, 0, 0) joint0 = kdl.Joint(kdl.Joint.None) kdl_chain.addSegment(kdl.Segment(joint0, frame0)) a, d, alfa, theta = params['i2'] frame1 = kdl_frame.DH(a, alfa, d, theta) joint1 = kdl.Joint(kdl.Joint.RotZ) kdl_chain.addSegment(kdl.Segment(joint1, frame1)) a, d, alfa, theta = params['i1'] frame2 = kdl_frame.DH(a, alfa, d, theta) joint2 = kdl.Joint(kdl.Joint.RotZ) kdl_chain.addSegment(kdl.Segment(joint2, frame2)) a, d, alfa, theta = params['i3'] frame3 = kdl_frame.DH(a, alfa, d, theta) joint3 = kdl.Joint(kdl.Joint.TransZ) kdl_chain.addSegment(kdl.Segment(joint3, frame3)) joint_angles = kdl.JntArray(kdl_chain.getNrOfJoints()) joint_angles[0] = data.position[0] joint_angles[1] = data.position[1] joint_angles[2] = -data.position[2] #kinematyka prosta przeliczenie na translacje i rotacje czlonu koncowego kdl_chain_t = kdl.ChainFkSolverPos_recursive(kdl_chain) frame_t = kdl.Frame() kdl_chain_t.JntToCart(joint_angles, frame_t) quat = frame_t.M.GetQuaternion() pose = PoseStamped() pose.header.frame_id = 'base_link' pose.header.stamp = rospy.Time.now() #nadanie wiadomosci z wyliczonymi wspolrzednymi czlonu koncowego pose.pose.position.x = frame_t.p[0] pose.pose.position.y = frame_t.p[1] pose.pose.position.z = frame_t.p[2] pose.pose.orientation.x = quat[0] pose.pose.orientation.y = quat[1] pose.pose.orientation.z = quat[2] pose.pose.orientation.w = quat[3] pub.publish(pose)
def simpleKinematics(data): #jesli polozenie jest niepoprawne zostaje wypisana informacja if not checkPosition(data): rospy.logerr('Wrong position: ' + str(data)) return #utworzenie trzech kolejnych macierzy z parametrow D-H kdl_chain = kdl.Chain() kdl_frame = kdl.Frame() frame0 = kdl_frame.DH(0, 0, 0, 0) joint0 = kdl.Joint(kdl.Joint.None) kdl_chain.addSegment(kdl.Segment(joint0, frame0)) a, d, alpha, theta = params['i2'] frame1 = kdl_frame.DH(a, alpha, d, theta) joint1 = kdl.Joint(kdl.Joint.RotZ) kdl_chain.addSegment(kdl.Segment(joint1, frame1)) a, d, alpha, theta = params['i1'] frame2 = kdl_frame.DH(a, alpha, d, theta) joint2 = kdl.Joint(kdl.Joint.RotZ) kdl_chain.addSegment(kdl.Segment(joint2, frame2)) a, d, alpha, theta = params['i3'] frame3 = kdl_frame.DH(a, alpha, d, theta) joint3 = kdl.Joint(kdl.Joint.TransZ) kdl_chain.addSegment(kdl.Segment(joint3, frame3)) #przeliczenie poprzednich macierzy na finalne wartosci koncowki joints_angle = kdl.JntArray(kdl_chain.getNrOfJoints()) joints_angle[0] = data.position[0] joints_angle[1] = data.position[1] joints_angle[2] = -data.position[2] kdl_chain_solver = kdl.ChainFkSolverPos_recursive(kdl_chain) final_frame = kdl.Frame() kdl_chain_solver.JntToCart(joints_angle, final_frame) quaternion = final_frame.M.GetQuaternion() #utworzenie i nadanie wiadomosci z polozeniem koncowki pose = PoseStamped() pose.header.frame_id = 'base' pose.header.stamp = rospy.Time.now() pose.pose.position.x = final_frame.p[0] pose.pose.position.y = final_frame.p[1] pose.pose.position.z = final_frame.p[2] pose.pose.orientation.x = quaternion[0] pose.pose.orientation.y = quaternion[1] pose.pose.orientation.z = quaternion[2] pose.pose.orientation.w = quaternion[3] pub.publish(pose)
def __init__(self): self.arm = kdl.Chain() self.arm.addSegment( kdl.Segment(kdl.Joint(kdl.Joint.RotZ), kdl.Frame(kdl.Vector(300, 0, 0)))) self.arm.addSegment( kdl.Segment(kdl.Joint(kdl.Joint.RotZ), kdl.Frame(kdl.Vector(0, -200, 0)))) self.ik_solver = kdl.ChainIkSolverPos_LMA(self.arm) self.current_joints = kdl.JntArray(self.arm.getNrOfJoints()) self.result_joints = kdl.JntArray(self.arm.getNrOfJoints())
def callback(data): kdlChain = kdl.Chain() frame = kdl.Frame() with open( os.path.dirname(os.path.realpath(__file__)) + '/../dh.json', 'r') as file: params = json.loads(file.read()) matrices = {} for key in sorted(params.keys()): a, d, al, th = params[key] if key == '1': d_prev = d th_prev = th continue print(params[key]) al, a, d, th = float(al), float(a), float(d), float(th) kdlChain.addSegment( kdl.Segment(kdl.Joint(kdl.Joint.RotZ), frame.DH(a, al, d_prev, th_prev))) d_prev = d th_prev = th kdlChain.addSegment( kdl.Segment(kdl.Joint(kdl.Joint.RotZ), frame.DH(0, 0, d_prev, th_prev))) jointPos = kdl.JntArray(kdlChain.getNrOfJoints()) jointPos[0] = data.position[0] jointPos[1] = data.position[1] jointPos[2] = data.position[2] forvKin = kdl.ChainFkSolverPos_recursive(kdlChain) eeFrame = kdl.Frame() forvKin.JntToCart(jointPos, eeFrame) quaternion = eeFrame.M.GetQuaternion() robot_pose = PoseStamped() robot_pose.header.frame_id = 'base_link' robot_pose.header.stamp = rospy.Time.now() robot_pose.pose.position.x = eeFrame.p[0] robot_pose.pose.position.y = eeFrame.p[1] robot_pose.pose.position.z = eeFrame.p[2] robot_pose.pose.orientation.x = quaternion[0] robot_pose.pose.orientation.y = quaternion[1] robot_pose.pose.orientation.z = quaternion[2] robot_pose.pose.orientation.w = quaternion[3] publisher.publish(robot_pose)
def __init__(self, linear_transform): """Initialize the Universal Robot class using its dh parmeters.""" linear_frame = m3d_transform_to_kdl_frame(linear_transform) chain = kdl.Chain() # Add linear segment chain.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.TransX), linear_frame)) # Add UR segments for segment in range(self.NUM_JOINTS): joint = kdl.Joint(kdl.Joint.RotZ) frame = kdl.Frame().DH(self.DH_A[segment], self.DH_ALPHA[segment], self.DH_D[segment], 0) chain.addSegment(kdl.Segment(joint, frame)) Kinematics.__init__(self, chain)
def urdf_joint_to_kdl_joint(jnt): origin_frame = urdf_pose_to_kdl_frame(jnt.origin) if jnt.joint_type == 'fixed': return kdl.Joint(jnt.name, kdl.Joint.Fixed) axis = kdl.Vector(*jnt.axis) if jnt.joint_type == 'revolute': return kdl.Joint(jnt.name, origin_frame.p, origin_frame.M * axis, kdl.Joint.RotAxis) if jnt.joint_type == 'continuous': return kdl.Joint(jnt.name, origin_frame.p, origin_frame.M * axis, kdl.Joint.RotAxis) if jnt.joint_type == 'prismatic': return kdl.Joint(jnt.name, origin_frame.p, origin_frame.M * axis, kdl.Joint.TransAxis) print("Unknown joint type: %s." % jnt.joint_type)
def setup_kdl_chain(self): for dh in self.dh_params: self.kine_chain.addSegment( PyKDL.Segment( PyKDL.Joint(PyKDL.Vector(), PyKDL.Vector(0, 0, dh[4]), PyKDL.Joint.RotAxis), PyKDL.Frame().DH(dh[0], dh[1], dh[2], dh[3])))
def urdf_joint_to_kdl_joint(jnt): origin_frame = urdf_pose_to_kdl_frame(jnt.origin) if jnt.joint_type == 'fixed': return kdl.Joint(jnt.name, getattr(kdl.Joint, 'None')) axis = kdl.Vector(*[float(s) for s in jnt.axis]) if jnt.joint_type == 'revolute': return kdl.Joint(jnt.name, origin_frame.p, origin_frame.M * axis, kdl.Joint.RotAxis) if jnt.joint_type == 'continuous': return kdl.Joint(jnt.name, origin_frame.p, origin_frame.M * axis, kdl.Joint.RotAxis) if jnt.joint_type == 'prismatic': return kdl.Joint(jnt.name, origin_frame.p, origin_frame.M * axis, kdl.Joint.TransAxis) print("Unknown joint type: %s." % jnt.joint_type) return kdl.Joint(jnt.name, getattr(kdl.Joint, 'None'))
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 _toKdlJoint(jnt): fixed = lambda j,F: PyKDL.Joint(j.name, getattr(PyKDL.Joint, 'None')) rotational = lambda j,F: PyKDL.Joint(j.name, F.p, F.M * PyKDL.Vector(*j.axis), PyKDL.Joint.RotAxis) translational = lambda j,F: PyKDL.Joint(j.name, F.p, F.M * PyKDL.Vector(*j.axis), PyKDL.Joint.TransAxis) type_map = { 'fixed': fixed, 'revolute': rotational, 'continuous': rotational, 'prismatic': translational, 'floating': fixed, 'planar': fixed, 'unknown': fixed, } return type_map[jnt.type](jnt, _toKdlPose(jnt.origin))
def create_right_chain(self): ch = kdl.Chain() self.right_arm_base_offset_from_torso_lift_link = np.matrix( [0., -0.188, 0.]).T # shoulder pan ch.addSegment( kdl.Segment(kdl.Joint(kdl.Joint.RotZ), kdl.Frame(kdl.Vector(0.1, 0., 0.)))) # shoulder lift ch.addSegment( kdl.Segment(kdl.Joint(kdl.Joint.RotY), kdl.Frame(kdl.Vector(0., 0., 0.)))) # upper arm roll ch.addSegment( kdl.Segment(kdl.Joint(kdl.Joint.RotX), kdl.Frame(kdl.Vector(0.4, 0., 0.)))) # elbox flex ch.addSegment( kdl.Segment(kdl.Joint(kdl.Joint.RotY), kdl.Frame(kdl.Vector(0.0, 0., 0.)))) # forearm roll ch.addSegment( kdl.Segment(kdl.Joint(kdl.Joint.RotX), kdl.Frame(kdl.Vector(0.321, 0., 0.)))) # wrist flex ch.addSegment( kdl.Segment(kdl.Joint(kdl.Joint.RotY), kdl.Frame(kdl.Vector(0., 0., 0.)))) # wrist roll ch.addSegment( kdl.Segment(kdl.Joint(kdl.Joint.RotX), kdl.Frame(kdl.Vector(0., 0., 0.)))) return ch
def toKdlJoint(jnt): # define a mapping for joints and kdl fixed = lambda j, F: kdl.Joint(j.name) rotational = lambda j, F: kdl.Joint(j.name, F.p, F.M * kdl.Vector(*j.axis), kdl.Joint.RotAxis) translational = lambda j, F: kdl.Joint(j.name, F.p, F.M * kdl.Vector(*j.axis), kdl.Joint.TransAxis) typeMap = { 'fixed': fixed, 'revolute': rotational, 'continuous': rotational, 'prismatic': translational, 'floating': fixed, 'planar': fixed, 'unknown': fixed, } return typeMap[jnt.type](jnt, toKdlPose(jnt.origin))
def __init__(self): """Initialize the Universal Robot class using its dh parmeters.""" chain = kdl.Chain() for segment in range(self.NUM_JOINTS): joint = kdl.Joint(kdl.Joint.RotZ) frame = kdl.Frame().DH(self.DH_A[segment], self.DH_ALPHA[segment], self.DH_D[segment], 0) chain.addSegment(kdl.Segment(joint, frame)) Kinematics.__init__(self, chain)
def to_kdl_joint(jnt): f_parent_jnt = to_kdl_frame(jnt.parent_to_joint_origin_transform) if jnt.type == urdf.Joint.FIXED: return kdl.Joint(jnt.name, kdl.Joint.None) elif jnt.type == urdf.Joint.REVOLUTE: axis = to_kdl_vector(jnt.axis) return kdl.Joint(jnt.name, f_parent_jnt.p, f_parent_jnt.M * axis, kdl.Joint.RotAxis) elif jnt.type == urdf.Joint.CONTINUOUS: axis = to_kdl_vector(jnt.axis) return kdl.Joint(jnt.name, f_parent_jnt.p, f_parent_jnt.M * axis, kdl.Joint.RotAxis) elif jnt.type == urdf.Joint.PRISMATIC: axis = to_kdl_vector(jnt.axis) return kdl.Joint(jnt.name, f_parent_jnt.p, f_parent_jnt.M * axis, kdl.Joint.RotAxis) else: rospy.logwarn("Unknown joint type %s." % jnt.name) return kdl.Joint(jnt.name, kdl.Joint.None)
def create_right_chain(self, end_effector_length): ch = kdl.Chain() ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotY),kdl.Frame(kdl.Vector(0.,-0.18465,0.)))) ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotX),kdl.Frame(kdl.Vector(0.,-0.03175,0.)))) ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotZ),kdl.Frame(kdl.Vector(0.00502,0.,-0.27857)))) ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotY),kdl.Frame(kdl.Vector(0.,0.,-0.27747)))) ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotZ),kdl.Frame(kdl.Vector(0.,0.,0.)))) ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotY),kdl.Frame(kdl.Vector(0.,0.,0.)))) ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotX),kdl.Frame(kdl.Vector(0.,0.,-end_effector_length)))) return ch
def create_left_chain(self, end_effector_length): ch = kdl.Chain() ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotY),kdl.Frame(kdl.Vector(0.,0.18493,0.)))) ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotX),kdl.Frame(kdl.Vector(0.,0.03175,0.)))) ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotZ),kdl.Frame(kdl.Vector(0.00635,0.,-0.27795)))) ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotY),kdl.Frame(kdl.Vector(0.,0.,-0.27853)))) ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotZ),kdl.Frame(kdl.Vector(0.,0.,0.)))) ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotY),kdl.Frame(kdl.Vector(0.,0.,0.)))) ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotX),kdl.Frame(kdl.Vector(0.,0.,-end_effector_length)))) return ch
def set_tool_transform(self, tool=None): """Set a tool transformation Keyword Arguments tool -- The tool as either None, Segment, Frame or math3d Transform (default None) """ if tool is None: self._tool_segment = None elif isinstance(tool, kdl.Segment): self._tool_segment = kdl.Segment(tool) elif isinstance(tool, kdl.Frame): self._tool_segment = kdl.Segment(kdl.Joint(), tool) elif isinstance(tool, m3d.Transform): frame = m3d_transform_to_kdl_frame(tool) self._tool_segment = kdl.Segment(kdl.Joint(), frame) else: raise Exception( "Tool is not None, Segment, Frame or m3d Transform") self._set_solvers() return True
def urdf_joint_to_kdl_joint(jnt): origin_frame = urdf_pose_to_kdl_frame(jnt.origin) try: # kdl issue to run with python 2 fixed_joint = kdl.Joint.Fixed except: joint_type_str = "kdl.Joint.None" fixed_joint = eval(joint_type_str) if jnt.joint_type == "fixed": return kdl.Joint(jnt.name, fixed_joint) # None axis = kdl.Vector(*[float(s) for s in jnt.axis]) if jnt.joint_type == "revolute": return kdl.Joint(jnt.name, origin_frame.p, origin_frame.M * axis, kdl.Joint.RotAxis) if jnt.joint_type == "continuous": return kdl.Joint(jnt.name, origin_frame.p, origin_frame.M * axis, kdl.Joint.RotAxis) if jnt.joint_type == "prismatic": return kdl.Joint(jnt.name, origin_frame.p, origin_frame.M * axis, kdl.Joint.TransAxis) print("Unknown joint type: %s." % jnt.joint_type) return kdl.Joint(jnt.name, fixed_joint)
def kin_fwd(params): publish = True if not check(params): publish = False rospy.logerr('Impossible position: ' + str(params)) return k = 1 chain = pykdl.Chain() frame = pykdl.Frame() angles = pykdl.JntArray(3) prev_d = 0 prev_theta = 0 counter = 0 for i in parametres.keys(): a, d, alpha, theta = parametres[i] a, d, alpha, theta = float(a), float(d), float(alpha), float(theta) joint = pykdl.Joint(pykdl.Joint.TransZ) if k != 1: fr = frame.DH(a, alpha, prev_d, prev_theta) chain.addSegment(pykdl.Segment(joint, fr)) k = k + 1 prev_d = d prev_theta = theta chain.addSegment(pykdl.Segment(joint, frame.DH(0, 0, d, theta))) angles[0] = params.position[0] angles[1] = params.position[1] angles[2] = params.position[2] solver = pykdl.ChainFkSolverPos_recursive(chain) secFrame = pykdl.Frame() solver.JntToCart(angles, secFrame) quater = secFrame.M.GetQuaternion() pose = PoseStamped() pose.header.frame_id = 'base_link' pose.header.stamp = rospy.Time.now() pose.pose.position.x = secFrame.p[0] pose.pose.position.y = secFrame.p[1] pose.pose.position.z = secFrame.p[2] pose.pose.orientation.x = quater[0] pose.pose.orientation.y = quater[1] pose.pose.orientation.z = quater[2] pose.pose.orientation.w = quater[3] if publish: publisher.publish(pose)
def __init__(self, DH): self.DH = DH ##在构造函数中建立机器人模型 #DH(theta,alpha,a,d) n = len(DH[:, 0]) #建立机器人坐标,关节 frms = [] jnts = [] for i in range(n): jnts.append(kdl.Joint(kdl.Joint.RotZ)) #建立坐标系 trans = bf.trans(DH[i, 0], DH[i, 1], DH[i, 2], DH[i, 3]) vetor = kdl.Vector(trans[3, 0], trans[3, 1], trans[3, 2]) frm1 = kdl.Frame(kdl.Rotation.RotX(-np.pi / 2), kdl.Vector(0, 0, 239.5))
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 forward_kinematics(data): if not checkScope(data): publish = False rospy.logwarn('Incorrect joint position[KDL]!') return chain = pykdl.Chain() frame = pykdl.Frame() angles = pykdl.JntArray(3) prev_d = 0 k = 1 prev_theta = 0 counter = 0 for i in params.keys(): a, d, alpha, theta = params[i] a, d, alpha, theta = float(a), float(d), float(alpha), float(theta) joint = pykdl.Joint(pykdl.Joint.TransZ) if k != 1: fr = frame.DH(a, alpha, prev_d, prev_theta) chain.addSegment(pykdl.Segment(joint, fr)) k = k + 1 prev_d = d prev_theta = theta chain.addSegment(pykdl.Segment(joint, frame.DH(0, 0, d, theta))) angles[0] = data.position[0] angles[1] = data.position[1] angles[2] = data.position[2] solver = pykdl.ChainFkSolverPos_recursive(chain) secFrame = pykdl.Frame() solver.JntToCart(angles, secFrame) quater = secFrame.M.GetQuaternion() pose = PoseStamped() pose.header.frame_id = 'base_link' pose.header.stamp = rospy.Time.now() pose.pose.position.x = secFrame.p[0] pose.pose.position.z = secFrame.p[2] pose.pose.position.y = secFrame.p[1] pose.pose.orientation.x = quater[0] pose.pose.orientation.y = quater[1] pose.pose.orientation.z = quater[2] pose.pose.orientation.w = quater[3] pub.publish(pose)
def create_right_chain(self): height = 0.0 linkage_offset_from_ground = np.matrix([0., 0., height]).T self.linkage_offset_from_ground = linkage_offset_from_ground self.n_jts = len(self.d_robot.b_jt_anchor) rospy.loginfo("number of joints is :"+str(self.n_jts)) ee_location = self.d_robot.ee_location b_jt_anchor = self.d_robot.b_jt_anchor b_jt_axis = self.d_robot.b_jt_axis ch = kdl.Chain() prev_vec = np.copy(linkage_offset_from_ground.A1) n = len(self.d_robot.b_jt_anchor) for i in xrange(n-1): if b_jt_axis[i][0] == 1 and b_jt_axis[i][1] == 0 and b_jt_axis[i][2] == 0: kdl_jt = kdl.Joint(kdl.Joint.RotX) elif b_jt_axis[i][0] == 0 and b_jt_axis[i][1] == 1 and b_jt_axis[i][2] == 0: kdl_jt = kdl.Joint(kdl.Joint.RotY) elif b_jt_axis[i][0] == 0 and b_jt_axis[i][1] == 0 and b_jt_axis[i][2] == 1: kdl_jt = kdl.Joint(kdl.Joint.RotZ) else: print "can't do off-axis joints yet!!!" np_vec = np.array(b_jt_anchor[i+1]) diff_vec = np_vec-prev_vec prev_vec = np_vec kdl_vec = kdl.Vector(diff_vec[0], diff_vec[1], diff_vec[2]) ch.addSegment(kdl.Segment(kdl_jt, kdl.Frame(kdl_vec))) np_vec = np.copy(ee_location.A1) diff_vec = np_vec-prev_vec if b_jt_axis[n-1][0] == 1 and b_jt_axis[n-1][1] == 0 and b_jt_axis[n-1][2] == 0: kdl_jt = kdl.Joint(kdl.Joint.RotX) elif b_jt_axis[n-1][0] == 0 and b_jt_axis[n-1][1] == 1 and b_jt_axis[n-1][2] == 0: kdl_jt = kdl.Joint(kdl.Joint.RotY) elif b_jt_axis[n-1][0] == 0 and b_jt_axis[n-1][1] == 0 and b_jt_axis[n-1][2] == 1: kdl_jt = kdl.Joint(kdl.Joint.RotZ) else: print "can't do off-axis joints yet!!!" kdl_vec = kdl.Vector(diff_vec[0], diff_vec[1], diff_vec[2]) ch.addSegment(kdl.Segment(kdl_jt, kdl.Frame(kdl_vec))) return ch
def forward_kinematics(data): doPub = True if not correct(data): rospy.logerr('Incorrect position! ' + str(data)) doPub = False return kdlChain = kdl.Chain() frameFactory = kdl.Frame() jntAngles = kdl.JntArray(3) jointNums = [2,3,1] for i in jointNums: a, d, al, th = params['i'+str(i)] al, a, d, th = float(al), float(a), float(d), float(th) frame = frameFactory.DH(a, al, d, th) joint = kdl.Joint(kdl.Joint.RotZ) kdlChain.addSegment(kdl.Segment(joint, frame)) jntAngles[i-1] = data.position[i-1] fksolver = kdl.ChainFkSolverPos_recursive(kdlChain) eeFrame = kdl.Frame() fksolver.JntToCart(jntAngles, eeFrame) quaternion = eeFrame.M.GetQuaternion() pose = PoseStamped() pose.header.frame_id = 'base_link' pose.header.stamp = rospy.Time.now() pose.pose.position.x = eeFrame.p[0] pose.pose.position.y = eeFrame.p[1] pose.pose.position.z = eeFrame.p[2] pose.pose.orientation.x = quaternion[0] pose.pose.orientation.y = quaternion[1] pose.pose.orientation.z = quaternion[2] pose.pose.orientation.w = quaternion[3] if doPub: pub.publish(pose)
def __init__(self, T=20, weight=[1.0,1.0]): #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.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
def callback(data): KDL_chain = PyKDL.Chain() KDL_frame = PyKDL.Frame() errorFlag = 0 # base frame0 = KDL_frame.DH(A[0], Alpha[0], 0, 0) joint0 = PyKDL.Joint(PyKDL.Joint.None) KDL_chain.addSegment(PyKDL.Segment(joint0, frame0)) # joint 1 d = rospy.get_param("d1", D[0]) if (data.position[0] < -d) or (data.position[0] > 0): errorFlag = 1 frame1 = KDL_frame.DH(A[1], Alpha[1], d, Theta[0]) joint1 = PyKDL.Joint(PyKDL.Joint.TransZ) KDL_chain.addSegment(PyKDL.Segment(joint1, frame1)) # joint 2 d = rospy.get_param("d2", D[1]) if (data.position[1] < -d) or (data.position[1] > 0): errorFlag = 2 frame2 = KDL_frame.DH(A[2], Alpha[2], d, Theta[1]) joint2 = PyKDL.Joint(PyKDL.Joint.TransZ) KDL_chain.addSegment(PyKDL.Segment(joint2, frame2)) # joint 3 d = rospy.get_param("d3", D[2]) if (data.position[2] < -d) or (data.position[2] > 0): errorFlag = 3 frame3 = KDL_frame.DH(0, 0, d, Theta[2]) joint3 = PyKDL.Joint(PyKDL.Joint.TransZ) KDL_chain.addSegment(PyKDL.Segment(joint3, frame3)) # final touches joint_array = PyKDL.JntArray(KDL_chain.getNrOfJoints()) joint_array[0] = data.position[0] joint_array[1] = data.position[1] joint_array[2] = data.position[2] KDL_chain_solver = PyKDL.ChainFkSolverPos_recursive(KDL_chain) frame_end = PyKDL.Frame() KDL_chain_solver.JntToCart(joint_array, frame_end) q = frame_end.M.GetQuaternion() poseStamped = PoseStamped() poseStamped.header.frame_id = 'base_link' poseStamped.header.stamp = rospy.Time.now() poseStamped.pose.position.x = frame_end.p[0] poseStamped.pose.position.y = frame_end.p[1] poseStamped.pose.position.z = frame_end.p[2] poseStamped.pose.orientation.x = q[0] poseStamped.pose.orientation.y = q[1] poseStamped.pose.orientation.z = q[2] poseStamped.pose.orientation.w = q[3] marker = Marker() marker.header.frame_id = 'base_link' marker.header.stamp = rospy.Time.now() marker.type = marker.SPHERE marker.action = marker.ADD marker.pose = poseStamped.pose marker.scale.x = 0.06 marker.scale.y = 0.06 marker.scale.z = 0.06 marker.color.a = 1 marker.color.r = 1 marker.color.g = 0 marker.color.b = 0 if errorFlag == 0: pubP.publish(poseStamped) pubM.publish(marker) else: print('Error: joint{} out of bounds'.format(errorFlag))
# coding=utf-8 import numpy as np import PyKDL as kdl # import copy #用于深度拷贝 # 注意一些基本概念 # Segment初始化由绕的轴和末端姿态来生成,可能跟书上的D-H方法有点不一样,区别在于用末端和轴来定义一个关节 # 轴是在当前关节的坐标系中绕轴V旋转,默认设置为kdl.Joint.RotZ # f_tip,末端在当前坐标系中的姿态 #建立关节 jnts = [] for i in range(6): jnts.append(kdl.Joint(kdl.Joint.RotZ)) print "jnts:\n", jnts #建立坐标系系列 frms = [] # 注意,这个框架是f_tip,末端 frm1 = kdl.Frame(kdl.Rotation.RotX(-np.pi / 2), kdl.Vector(0, 0, 239.5)) frm2 = kdl.Frame(kdl.Rotation.RotX(0), kdl.Vector(250, 0, 0)) frm3 = kdl.Frame(kdl.Rotation.RotX(np.pi / 2), kdl.Vector(0, 262, 0)) frm4 = kdl.Frame(kdl.Rotation.RotX(-np.pi / 2), kdl.Vector(0, 0, 0)) frm5 = kdl.Frame(kdl.Rotation.RotX(np.pi / 2), kdl.Vector(0, 0, 0)) frm6 = kdl.Frame(kdl.Rotation.RotX(0), kdl.Vector(-168, 0, 0)) frms.append(frm1) frms.append(frm2) frms.append(frm3)
def create_right_chain(self): height = 0.0 linkage_offset_from_ground = np.matrix([0., 0., height]).T self.linkage_offset_from_ground = linkage_offset_from_ground b_jt_q_ind = [True, True, True] self.n_jts = len(b_jt_q_ind) torso_half_width = 0.196 upper_arm_length = 0.334 forearm_length = 0.288 #+ 0.115 ee_location = np.matrix([ 0., -torso_half_width - upper_arm_length - forearm_length, height ]).T b_jt_anchor = [[0, 0, height], [0, -torso_half_width, height], [0., -torso_half_width - upper_arm_length, height]] b_jt_axis = [[0, 0, 1], [0, 0, 1], [0, 0, 1]] ch = kdl.Chain() prev_vec = np.copy(linkage_offset_from_ground.A1) n = len(b_jt_q_ind) for i in xrange(n - 1): if b_jt_axis[i][0] == 1 and b_jt_axis[i][1] == 0 and b_jt_axis[i][ 2] == 0: kdl_jt = kdl.Joint(kdl.Joint.RotX) elif b_jt_axis[i][0] == 0 and b_jt_axis[i][1] == 1 and b_jt_axis[ i][2] == 0: kdl_jt = kdl.Joint(kdl.Joint.RotY) elif b_jt_axis[i][0] == 0 and b_jt_axis[i][1] == 0 and b_jt_axis[ i][2] == 1: kdl_jt = kdl.Joint(kdl.Joint.RotZ) else: print "can't do off-axis joints yet!!!" np_vec = np.array(b_jt_anchor[i + 1]) diff_vec = np_vec - prev_vec prev_vec = np_vec kdl_vec = kdl.Vector(diff_vec[0], diff_vec[1], diff_vec[2]) ch.addSegment(kdl.Segment(kdl_jt, kdl.Frame(kdl_vec))) np_vec = np.copy(ee_location.A1) diff_vec = np_vec - prev_vec if b_jt_axis[n - 1][0] == 1 and b_jt_axis[n - 1][1] == 0 and b_jt_axis[ n - 1][2] == 0: kdl_jt = kdl.Joint(kdl.Joint.RotX) elif b_jt_axis[n - 1][0] == 0 and b_jt_axis[ n - 1][1] == 1 and b_jt_axis[n - 1][2] == 0: kdl_jt = kdl.Joint(kdl.Joint.RotY) elif b_jt_axis[n - 1][0] == 0 and b_jt_axis[ n - 1][1] == 0 and b_jt_axis[n - 1][2] == 1: kdl_jt = kdl.Joint(kdl.Joint.RotZ) else: print "can't do off-axis joints yet!!!" kdl_vec = kdl.Vector(diff_vec[0], diff_vec[1], diff_vec[2]) ch.addSegment(kdl.Segment(kdl_jt, kdl.Frame(kdl_vec))) return ch