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 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.dh_params = [[-0.033, pi / 2, 0.145, pi, -1], [0.155, 0, 0, pi / 2, -1], [0.135, 0, 0, 0.0, -1], [-0.002, pi / 2, 0, -pi / 2, -1], [0, pi, -0.185, -pi, -1]] self.joint_offset = [170*pi/180, 65*pi/180, -146*pi/180, 102.5*pi/180, 167.5*pi/180] self.joint_limit_min = [-169*pi/180, -65*pi/180, -151*pi/180, -102.5*pi/180, -167.5*pi/180] self.joint_limit_max = [169*pi/180, 90*pi/180, 146*pi/180, 102.5*pi/180, 167.5*pi/180] # Setup the subscribers for the joint states self.subscriber_joint_state_ = rospy.Subscriber('/joint_states', JointState, self.joint_state_callback, queue_size=5) # TF2 broadcaster self.pose_broadcaster = tf2_ros.TransformBroadcaster() # PyKDL self.kine_chain = PyKDL.Chain() self.setup_kdl_chain() self.current_joint_position = PyKDL.JntArray(self.kine_chain.getNrOfJoints()) self.current_pose = PyKDL.Frame() self.fk_solver = PyKDL.ChainFkSolverPos_recursive(self.kine_chain) self.ik_solver = PyKDL.ChainIkSolverPos_LMA(self.kine_chain) self.jac_calc = PyKDL.ChainJntToJacSolver(self.kine_chain)
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 __init__(self): # Subscriptions self.objects = rospy.Subscriber('/found_objects', Object, self.callback_obj) self.tfBuffer = tf2_ros.Buffer() self.listener = tf2_ros.TransformListener(self.tfBuffer) self.gp_action = actionlib.SimpleActionClient('request_trajectory', RequestTrajectoryAction) self.grasping_poses = [] self.object_list = [] self.old_list = [] self.joint_limits_lower = [] self.joint_limits_upper = [] self.joint_names = [] self.gp_weights = np.zeros(3) # Arm selection self.left_arm = False self.right_arm = False self.frame_end = '' self.desired_chain = 'left_chain' self.manip_threshold = 0.05 self.distance_threshold = 1.15 # Gripper frames: self.grip_left = 'left_gripper_tool_frame' # self.grip_right = 'left_gripper_tool_frame' self.grip_right = 'right_gripper_tool_frame' self.frame_base = 'base_link' # KDL chains: self.right_chain = kdl.Chain() self.left_chain = kdl.Chain() self.ik_lambda = 0.35 # how much should singularities be avoided? self.arms_chain() # generate chains for both arms self.left_jnt_pos = kdl.JntArray(self.nJoints) self.right_jnt_pos = kdl.JntArray(self.nJoints) self.jac_solver_left = kdl.ChainJntToJacSolver(self.left_chain) self.jac_solver_right = kdl.ChainJntToJacSolver(self.right_chain) self.joint_list = rospy.Subscriber('/joint_states', JointState, self.joint_callback) rospy.sleep(0.01)
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 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 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 kinem_chain(self, name_frame_end, name_frame_base='odom'): # Transform URDF to Chain() for the joints between 'name_frame_end' and 'name_frame_base' self.chain = kdl.Chain() ik_lambda = 0.35 try: self.joint_names = self.urdf_model.get_chain(name_frame_base, name_frame_end, links=False, fixed=False) self.name_frame_in = name_frame_base self.name_frame_out = name_frame_end # rospy.loginfo("Will control the following joints: %s" %(self.joint_names)) self.kdl_tree = kdl_tree_from_urdf_model(self.urdf_model) self.chain = self.kdl_tree.getChain(name_frame_base, name_frame_end) self.kdl_fk_solver = kdl.ChainFkSolverPos_recursive(self.chain) self.kdl_ikv_solver = kdl.ChainIkSolverVel_wdls(self.chain) self.kdl_ikv_solver.setLambda(ik_lambda) self.nJoints = self.chain.getNrOfJoints() # Default Task and Joint weights self.tweights = np.identity(6) # weight matrix with 1 in diagonal to make use of all the joints. self.jweights = np.identity(self.nJoints) self.kdl_ikv_solver.setWeightTS(self.tweights.tolist()) self.kdl_ikv_solver.setWeightJS(self.jweights.tolist()) # Fill the list with the joint limits self.joint_limits_lower = np.empty(self.nJoints) self.joint_limits_upper = np.empty(self.nJoints) self.joint_vel_limits = np.empty(self.nJoints) for n, jnt_name in enumerate(self.joint_names): jnt = self.urdf_model.joint_map[jnt_name] if jnt.limit is not None: if jnt.limit.lower is None: self.joint_limits_lower[n] = -0.07 else: self.joint_limits_lower[n] = jnt.limit.lower if jnt.limit.upper is None: self.joint_limits_upper[n] = -0.07 else: self.joint_limits_upper[n] = jnt.limit.upper self.joint_vel_limits[n] = jnt.limit.velocity self.joint_vel_limits[0] = 0.3 self.joint_vel_limits[1] = 0.3 except (RuntimeError, TypeError, NameError): rospy.logerr("Unexpected error:", sys.exc_info()[0]) rospy.logerr('Could not re-init the kinematic chain') self.name_frame_out = ''
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 __init__(self): rospy.init_node('goal_selector', anonymous=True) r = rospy.Rate(2) self.objects = rospy.Subscriber('/found_objects', Object, self.callback_obj) self.tfBuffer = tf2_ros.Buffer() self.listener = tf2_ros.TransformListener(self.tfBuffer) self.grasping_poses = [] self.object_list = [] self.old_list =[] self.joint_limits_lower = [] self.joint_limits_upper = [] self.joint_names = [] self.gp_weights = np.zeros(6) # Arm selection self.left_arm = False self.right_arm = False self.frame_end = '' self.desired_chain = 'left_chain' self.manip_threshold = 0.05 self.distance_threshold = 1.15 # Gripper frames: self.grip_left = 'left_gripper_tool_frame' self.grip_right = 'right_gripper_tool_frame' self.frame_base = 'base_footprint' # KDL chains: self.right_chain = kdl.Chain() self.left_chain = kdl.Chain() self.ik_lambda = 0.35 # how much should singularities be avoided? self.arms_chain() # generate chains for both arms self.left_jnt_pos = kdl.JntArray(self.nJoints) self.right_jnt_pos = kdl.JntArray(self.nJoints) self.jac_solver_left = kdl.ChainJntToJacSolver(self.left_chain) self.jac_solver_right = kdl.ChainJntToJacSolver(self.right_chain) self.joint_list = rospy.Subscriber('/joint_states', JointState, self.joint_callback) rospy.sleep(0.5)
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): super(YoubotKDL, self).__init__() # PyKDL self.kine_chain = PyKDL.Chain() self.setup_kdl_chain() self.current_joint_position = PyKDL.JntArray( self.kine_chain.getNrOfJoints()) self.current_pose = PyKDL.Frame() self.fk_solver = PyKDL.ChainFkSolverPos_recursive(self.kine_chain) self.ik_solver = PyKDL.ChainIkSolverPos_LMA(self.kine_chain) self.jac_calc = PyKDL.ChainJntToJacSolver(self.kine_chain)
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 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 _set_solvers(self): """Initialize the solvers according to the stored base chain and tool""" self._complete_chain = kdl.Chain(self._base_chain) if self._tool_segment is not None: if isinstance(self._tool_segment, kdl.Segment): self._complete_chain.addSegment(self._tool_segment) else: raise Exception("Tool is not None, Chain or Segment") self._fk_solver_pos = kdl.ChainFkSolverPos_recursive( self._complete_chain) self._tv_solver = kdl.ChainIkSolverVel_pinv(self._complete_chain) self._ik_solver_pos = kdl.ChainIkSolverPos_NR(self._complete_chain, self._fk_solver_pos, self._tv_solver)
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 kinem_chain(self, name_frame_end, name_frame_base='triangle_base_link'): # Transform URDF to Chain() for the joints between 'name_frame_end' and 'name_frame_base' self.chain = kdl.Chain() try: self.joint_names = self.urdf_model.get_chain(name_frame_base, name_frame_end, links=False, fixed=False) self.name_frame_in = name_frame_base self.name_frame_out = name_frame_end self.njoints = len(self.joint_names) self.kdl_tree = kdl_tree_from_urdf_model(self.urdf_model) self.chain = self.kdl_tree.getChain(name_frame_base, name_frame_end) self.kdl_fk_solver = kdl.ChainFkSolverPos_recursive(self.chain) self.kdl_ikv_solver = kdl.ChainIkSolverVel_wdls(self.chain) self.kdl_ikv_solver.setLambda(self.ik_lambda) # Default Task and Joint weights self.tweights = np.identity(6) # weight matrix with 1 in diagonal to make use of all the joints. self.jweights = np.identity(self.njoints) self.kdl_ikv_solver.setWeightTS(self.tweights.tolist()) self.kdl_ikv_solver.setWeightJS(self.jweights.tolist()) # Fill the list with the joint limits self.joint_limits_lower = [] self.joint_limits_upper = [] for jnt_name in self.joint_names: jnt = self.urdf_model.joint_map[jnt_name] if jnt.limit is not None: self.joint_limits_lower.append(jnt.limit.lower) self.joint_limits_upper.append(jnt.limit.upper) self.nJoints = self.chain.getNrOfJoints() except: rospy.logerr("Unexpected error:", sys.exc_info()[0]) rospy.logerr('Could not re-init the kinematic chain') self.name_frame_out = '' return self.chain
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 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))
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) frms.append(frm4) frms.append(frm5) frms.append(frm6) print "frames:\n", frms rbt = kdl.Chain() # 建立机器人对象 #建立连杆 link = [] for i in range(6): link.append(kdl.Segment(jnts[i], frms[i])) rbt.addSegment(link[i]) fk = kdl.ChainFkSolverPos_recursive(rbt) p = kdl.Frame() q = kdl.JntArray(6) q[0] = 0 q[1] = 0 q[2] = 0 q[3] = 0
def forward_kinematics(data): chain = PyKDL.Chain() frame = PyKDL.Frame() k = 1 prev_d = 0 prev_th = 0 n_joints = len(params.keys()) for i in params.keys(): a, d, alpha, th = params[i] alpha, a, d, th = float(alpha), float(a), float(d), float(th) joint = PyKDL.Joint(PyKDL.Joint.TransZ) if k != 1: fr = frame.DH(a, alpha, prev_d, prev_th) segment = PyKDL.Segment(joint, fr) chain.addSegment(segment) k = k + 1 prev_d = d prev_th = th a, d, alpha, th = params["i3"] chain.addSegment(PyKDL.Segment(joint, frame.DH(0, 0, d, th))) joints = PyKDL.JntArray(n_joints) for i in range(n_joints): min_joint, max_joint = scope["joint" + str(i + 1)] if min_joint <= data.position[i] <= max_joint: joints[i] = data.position[i] else: rospy.logwarn("Wrong joint value") return fk = PyKDL.ChainFkSolverPos_recursive(chain) finalFrame = PyKDL.Frame() fk.JntToCart(joints, finalFrame) quaterions = finalFrame.M.GetQuaternion() pose = PoseStamped() pose.header.frame_id = 'base_link' pose.header.stamp = rospy.Time.now() pose.pose.position.x = finalFrame.p[0] pose.pose.position.y = finalFrame.p[1] pose.pose.position.z = finalFrame.p[2] pose.pose.orientation.x = quaterions[0] pose.pose.orientation.y = quaterions[1] pose.pose.orientation.z = quaterions[2] pose.pose.orientation.w = quaterions[3] pub.publish(pose) marker = Marker() marker.header.frame_id = 'base_link' marker.type = marker.CUBE marker.action = marker.ADD marker.pose.orientation.w = 1 time = rospy.Duration() marker.lifetime = time marker.scale.x = 0.09 marker.scale.y = 0.09 marker.scale.z = 0.09 marker.pose.position.x = finalFrame.p[0] marker.pose.position.y = finalFrame.p[1] marker.pose.position.z = finalFrame.p[2] marker.pose.orientation.x = quaterions[0] marker.pose.orientation.y = quaterions[1] marker.pose.orientation.z = quaterions[2] marker.pose.orientation.w = quaterions[3] marker.color.a = 0.7 marker.color.r = 0.2 marker.color.g = 0.8 marker.color.b = 0.6 marker_pub.publish(marker)
#! /usr/bin/python import rospy import json import PyKDL as kdl import os from sensor_msgs.msg import * from geometry_msgs.msg import * from tf.transformations import * from visualization_msgs.msg import Marker kdlChain = kdl.Chain() frame = kdl.Frame() print("przed") print(kdlChain) print("po") with open(os.path.dirname(os.path.realpath(__file__)) + '/../dh.json', 'r') as file: params = json.loads(file.read()) 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()):
#!/usr/bin/env python import rospy from math import pi import numpy as np from sensor_msgs.msg import JointState from geometry_msgs.msg import TransformStamped, Quaternion import tf2_ros import PyKDL as kdl chain = kdl.Chain() a = [0.0, 0.265699, 0.03, 0.0, 0.0, 0.0] alpha = [-pi / 2, 0.0, -pi / 2, -pi / 2, -pi / 2, 0.0] d = [0.159, 0.0, 0.0, 0.258, 0.0, -0.123] theta = [ 0.0, -pi / 2 + np.arctan(0.03 / 0.264), -np.arctan(0.03 / 0.264), 0.0, 0.0, 0.0 ] name_link = [ 'kdl_link_1', 'kdl_link_2', 'kdl_link_3', 'kdl_link_4', 'kdl_link_5', 'kdl_link_6' ] chain = kdl.Chain() for p_a, p_alpha, p_d, p_theta in zip(a, alpha, d, theta): chain.addSegment( kdl.Segment(kdl.Joint(kdl.Joint.RotZ), kdl.Frame().DH(a=p_a, alpha=p_alpha, d=p_d, theta=p_theta)))
def callback(data): kdl_chain =PyKDL.Chain() Frame = PyKDL.Frame(); i = 0 d=0 th=0 for joint in dh_file: name = joint['name'] last_d = d last_th = th a = joint["a"] d = joint["d"] al = joint["al"] th = joint["th"] if name == 'i3' and get_parameters('dlugosc1'): a = rospy.get_param("/dlugosc1") if name == 'hand' and get_parameters('dlugosc2'): a = rospy.get_param("/dlugosc2") #forego first iteration if i==0: i = i+1 continue kdl_chain.addSegment(PyKDL.Segment(PyKDL.Joint(PyKDL.Joint.RotZ), Frame.DH(a, al, last_d, last_th))) i = i+1 jointDisplacement = PyKDL.JntArray(kdl_chain.getNrOfJoints()) #joint displacements including restrictions jointDisplacement[0] = data.position[0] jointDisplacement[1] = data.position[1] jointDisplacement[2] = data.position[2] if(data.position[0] < restrictions[0]['backward']): jointDisplacement[0] = restrictions[0]['backward'] rospy.logerr("[KDL] Przekroczono ograniczenie dolne stawu o numerze: 1") elif(data.position[0] > restrictions[0]['forward']): jointDisplacement[0] = restrictions[0]['forward'] rospy.logerr("[KDL] Przekroczono ograniczenie gorne stawu o numerze: 1") if(data.position[1] < restrictions[1]['backward']): jointDisplacement[1] = restrictions[1]['backward'] rospy.logerr("[KDL] Przekroczono ograniczenie dolne stawu o numerze: 2") elif(data.position[1] > restrictions[1]['forward']): jointDisplacement[1] = restrictions[1]['forward'] rospy.logerr("[KDL] Przekroczono ograniczenie gorne stawu o numerze: 2") if(data.position[2] < restrictions[2]['backward']): jointDisplacement[2] = restrictions[2]['backward'] rospy.logerr("[KDL] Przekroczono ograniczenie dolne stawu o numerze: 3") elif(data.position[2] > restrictions[2]['forward']): jointDisplacement[2] = restrictions[2]['forward'] rospy.logerr("[KDL] Przekroczono ograniczenie gorne stawu o numerze: 3") f_k_solver = PyKDL.ChainFkSolverPos_recursive(kdl_chain) frame = PyKDL.Frame() f_k_solver.JntToCart(jointDisplacement, frame) quatr = frame.M.GetQuaternion() kdl_pose = PoseStamped() kdl_pose.header.frame_id = 'base_link' kdl_pose.header.stamp = rospy.Time.now() kdl_pose.pose.position.x = frame.p[0] kdl_pose.pose.position.y = frame.p[1] kdl_pose.pose.position.z = frame.p[2] kdl_pose.pose.orientation.x = quatr[0] kdl_pose.pose.orientation.y = quatr[1] kdl_pose.pose.orientation.z = quatr[2] kdl_pose.pose.orientation.w = quatr[3] pub.publish(kdl_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 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