def get_jacobians(self, state, robot_id=0): """ Produce a Jacobian from the urdf that maps from joint angles to x, y, z. This makes a 6x6 matrix from 6 joint angles to x, y, z and 3 angles. The angles are roll, pitch, and yaw (not Euler angles) and are not needed. Returns a repackaged Jacobian that is 3x6. """ # Initialize a Jacobian for self.scara_chain.getNrOfJoints() joint angles by 3 cartesian coords and 3 orientation angles jacobian = Jacobian(self.scara_chain.getNrOfJoints()) # Initialize a joint array for the present self.scara_chain.getNrOfJoints() joint angles. angles = JntArray(self.scara_chain.getNrOfJoints()) # Construct the joint array from the most recent joint angles. for i in range(self.scara_chain.getNrOfJoints()): angles[i] = state[i] # Update the jacobian by solving for the given angles.observation_callback self.jac_solver.JntToJac(angles, jacobian) # Initialize a numpy array to store the Jacobian. J = np.array([[jacobian[i, j] for j in range(jacobian.columns())] for i in range(jacobian.rows())]) # Only want the cartesian position, not Roll, Pitch, Yaw (RPY) Angles ee_jacobians = J return ee_jacobians
def compute_effector_position(self, msg): positions = JntArray(3) chain = Chain() kdl_frame = Frame() d = 0 theta = 0 order = [1, 2, 0] for i in order: joint = self.params[i] a = joint["a"] d = joint["d"] alpha = joint["alpha"] theta = joint["theta"] frame = kdl_frame.DH(a, alpha, d, theta) chain.addSegment(Segment(Joint(Joint.RotZ), frame)) positions[i] = msg.position[i] fk_solver = ChainFkSolverPos_recursive(chain) result = Frame() fk_solver.JntToCart(positions, result) kdl_pose = PoseStamped() kdl_pose.header.frame_id = 'base_link' kdl_pose.header.stamp = rospy.Time.now() kdl_pose.pose.position.x = result.p[0] kdl_pose.pose.position.y = result.p[1] kdl_pose.pose.position.z = result.p[2] quat = result.M.GetQuaternion() kdl_pose.pose.orientation.x = quat[0] kdl_pose.pose.orientation.y = quat[1] kdl_pose.pose.orientation.z = quat[2] kdl_pose.pose.orientation.w = quat[3] return kdl_pose